Costmap should reflect the z-location of the input map

[dev10110]

In my experiments, I provide a slice of an ESDF at z = 1.0, but when it goes through nav2_costmap_2d the z height is set to 0.

Can we make it so it uses the same z as the origin of the input map?

https://github.com/ros-planning/navigation2/blob/4f98db411d94a9a810423fc4bf8870cc4a5973e8/nav2_costmap_2d/src/costmap_2d_publisher.cpp#L140

[SteveMacenski]

Why is there a Z component of a 2D map which is important to reflect? Just trying to understand the need.

[dev10110]

Totally fair question.

So I have a vicon/world frame, where the z=0 corresponds to the ground. I am now flying a quadrotor at approximately z=1 and using nvidia's isaac_ros_nvblox package to build up a voxblox representation of the obstacles (in 3D). This gets sliced between z=0.7 and z=1.3 to determine the euclidean signed distance field, and ultimately this is the map that I am sending to nav2. This slice has a origin.z=1.

Now I want to visualize this in rviz, but since the costmap has a specified origin.z=0 I am unable to render it at the correct height.

[SteveMacenski]

Ok, sounds good. How do you propose getting the Z info over to that method?

that costmap publisher doesn’t know anything about a “map” height since the costmap is the combination of many layers (which may or may not include a static layer from a map or multiple static layers of multiple maps)

[dev10110]

So the map slice that you receive has a MapMetaData associated with it right? According to this https://docs.ros2.org/foxy/api/nav_msgs/msg/MapMetaData.html, the map has an origin which is a geometry_msgs::msg::Pose and therefore has a z component associated with it - can we use that z component when we republish new map messages?

[SteveMacenski]

The static map != the cost map. There is a costmap layer that deals with the map information to populate the costmap informational costs. This publisher operates on costmaps, not on maps, which is a subtle distinction.

So you'd need to devise a way to actually expose the map's Z origin to the costmap from the map level for potential publication. Actually, storing a costmap's "height" could be useful for multi-slice navigation, so that could be some useful metadata to just include in the Costmap2D object itself -- which would put it in a place that this publisher could then use it!

[dev10110]

Hmmm, I see.

I'm not familiar enough with the internal representations of nav2 to fully understand what you mean, or how we will address this. Are you suggesting adding a metadata field here: https://github.com/ros-planning/navigation2/blob/abf3e85d05ac1dbb3a10bfda4afe37cf780184f4/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp#L78C29-L78C29? like adding a origin_z field?

[SteveMacenski]

Yes, add height info to the costmap 2D struct that can be used

[SteveMacenski]

@dev10110 does that help? Any other questions?

[SteveMacenski]

Marking this as won't fix. I looked into adding this feature today and while straight forward (add a origin_z param which a Costmap2D stores, then accesses in the costmap publisher) I don't think its part of the design intent of costmap2D to have multiple height slices in its core structure. As such, if using this, its expected that all layers are with respect to the same global_frame as is set in the costmap ros class and shared among all layers. That isn't to say that the global_frame itself has to be on the ground - which would allow you to represent non-ground-relative costmap slices, but the z will be aligned with the global frame's height. You should create a frame at the height in question and use that here for getting an elevated costmap

I have made note of this though for future efforts to redesign environmental models so that different layers can represent different slices.

As such: Closing this ticket but happy to reopen or discuss if there are more comments in the future with respect to this