How to convert grid_map to costmap_2d using grid_map_costmap_2d?

[NeilNie]

Hello,

I am wondering if someone could provide an example of how to convert a grid_map to costmap_2d, and publish the costmap using ROS. Furthermore, if grid_map_costmap_2d is the way to go, it seems like the grid_map_costmap_2d package doesn't come installed with grid_map. Are there additional steps I need to take to install grid_map_costmap_2d?

Thank you very much!

Here is my code that creates the map

map = grid_map::GridMap({"elevation"});
map.setFrameId("map");
map.setGeometry(grid_map::Length(10, 10), 0.10);
ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.",
         map.getLength().x(), map.getLength().y(),
         map.getSize()(0), map.getSize()(1),
         map.getPosition().x(), map.getPosition().y(), map.getFrameId().c_str());

Here is my ROS publisher node:

ros::Time time = ros::Time::now();
map.setTimestamp(time.toNSec());
grid_map_msgs::GridMap message;
grid_map::GridMapRosConverter::toMessage(map, message);
map_pub.publish(message);

[maximilianwulf]

Hi @NeilNie, You can use the function toOccupancyGrid in GridMapRosConverter.cpp on line 251. It will generate an occupancy grid for you and then you can convert it to a costmap_2d.

grid_map_costmap_2d supports only import functionality at the moment. Does this answer your question?

[NeilNie]

Hi @maximilianwulf Thank you very much for your comment. I will try this method.

[NeilNie]

Hello @maximilianwulf. I was able to create and publish an occupancy grid. Here is my code for publishing the occupancy_grid:

// Publish grid map.
ros::Time time = ros::Time::now();
map.setTimestamp(time.toNSec());
nav_msgs::OccupancyGrid message;
grid_map::GridMapRosConverter::toOccupancyGrid(map, "elevation", -10.0, 10.0, message);
map_pub.publish(message);

However, I am not sure if I understand the ROS answer for converting the occupancy grid to costmap_2d.

Based on my understanding, do I need to publish the occupancy_grid and then have Costmap2DPublisher subscribe to the occupancy_grid? Thank you very much!

[maximilianwulf]

Hi @NeilNie, the answer is a bit sparse. ROS does not provide a _costmap2d message, therefore, it cannot be published. It is rather a C++ API that the common navigation tools use internally, as explained here.

To send the information to another node you have two possibilities:

• publish a _gridmap message or
• publish a _occupancygrid message.

Then on your receiving node, you have to implement code that generates a _costmap2d from either a _gridmap or an _occupancygrid. To do so you have to implement the reverse functionality of grid_map_costmap_2d to generate a _costmap2d from a _gridmap. Or, you have to implement the inverse mapping function Costmap2DPublisher::prepareGrid() to generate a _costmap2d from an _occupancygrid.

Feel free to create a pull request if you implemented the code or ask for a feature in this repo.

Does this answer your question?

[NeilNie]

Hi @maximilianwulf Thank you very much for the thorough response. I was able to generate a costmap by using the move_base node. My issue has been resolved. Closing for now.

[houhaol]

Hi @maximilianwulf Thank you very much for the thorough response. I was able to generate a costmap by using the move_base node. My issue has been resolved. Closing for now.

Hi, could you confirm using move_base node to generate cost map? I'm very new to this, sry. Basically, I tried to create a map (costmap) for navigation.

I have a occupancy grid like this figure which is created from point cloud Then in my understanding, you saved this occupancy_grid as map using map_server map_saver. Next is to load this map file into navigation and have costmap with help of move base, am i right?

[maximilianwulf]

Hey, I guess best would be if @NeilNie explains how he managed to do it.

[NeilNie]

Hey @houhaol, for my application, I needed to generate costmaps for the navigation stack, using stereo cameras and lidars. I used the move_base node that comes with ROS. (http://wiki.ros.org/navigation/Tutorials/RobotSetup).

On a high level, move_base is the heart of the generic ROS navigation stack. It calls on planners, costmaps, and other components. As a developer, using move_base properly will simplify your workflow, you don't have to worry about the details of costmaps or spend time implementing A* search. Please see ROS's official tutorial and launch file example for how to use move_base. Also, maybe the tutorial on mapping might help you? (http://wiki.ros.org/slam_gmapping/Tutorials/MappingFromLoggedData).

Assuming you have done mapping and the image above is the static map, for autonomous navigation, I believe you need to use map_server to load the maps, and move_base will generate the costmaps based on this static map provided by the map server. (as long as the topic names are correct, and you have the correct configuration). Usually there's the global costmap (based on this static map you have saved and provided), and there's the local costmap (which contains new obstacles and dynamic obstacles). Once you have successfully localized on this map, you can send move_base goals and the planner with plan the path. Your drive by wire system will probably take care of the rest.

Here's the link to the project I was using this on: https://github.com/sigmaai/self-driving-golf-cart/tree/master/ros/src/navigation. It has been a long time since I last worked on this... I hope my answer makes sense.

[maximilianwulf]

@NeilNie thank you, that is actually very interesting.

[houhaol]

@NeilNie Thanks for your reply and resources. Then it makes sense to me.

[Mobilepayload]

@NeilNie Can you explain on the procedure how you generated the costmap or preferably your script file?