Conversion from CostMap2D to GridMap

[skohlbr]

Standard ROS navigation uses CostMap2D as a cost map representation and when implementing a nav_core::BaseGlobalPlanner plugin that uses grid_map as a back-end, it would be desirable to have a converter that allows initalizing the internal grid_map from a CostMap2D.
Are there any plans for implementing this (or even some implementation out there that I missed)?

[stonier]

We have a function in the cost_map library (based on this one, but has char based grids instead of double) which does this...here. There's an active PR on that too looking to improve it a bit.

At any rate, it could very easily be templatised to handle grid_map and cost_map objects together I think.

[skohlbr]

Thanks for the link @stonier! I made my own small converter by pulling out some code from navigation, converting from CostMap2D to nav_msgs/OccupancyGrid and then just using the conversion to GridMap. Not terribly efficient, but gets the job done: https://github.com/tu-darmstadt-ros-pkg/grid_map_navigation_planner/blob/master/grid_map_planner_nav_plugin/include/grid_map_planner_nav_plugin/grid_map_cost_map_2d_conversion.h#L71

[stonier]

Oh, two step. Bit expensive, but it should work.

Good to have this issue here. I've been a bit delayed getting my cost map revamped (has to wait until I get back from interview travels), but this would be a good function to get into the core for both cost_map and grid_map.

[pfankhauser]

Thanks @skohlbr and @stonier for your input! I added a quick implementation of a Costmap2D to Grid Map in the feature/costmap_conversion branch.

• The class Costmap2DConverter provides two methods
  1. First one has to initialize a new grid map with initializeFromCostmap2d to adapt to the geometry of the costmap.
  2. Then, data from the costmap can be copied to a layer of the grid map with addLayerFromCostmap2d.
• Direct conversion from Costmap2d to Grid Map, no intermediate steps.
• Templated on MapType, so can be used by Grid Map derivatives as well.
• Unit tests here.

Please let me know if this works for you or if you have any suggestions. Thanks!

[pfankhauser]

Update: I used the cost translation table as suggested by @skohlbr and added him as an author, please let me know if this is not ok.

[skohlbr]

Fine with me, but full disclosure: I extracted and modified that code from costmap_2d_publisher (the cost translation table is a static char* cost_translation_table_ there).

[pfankhauser]

@skohlbr Ok great, were you able to check if the new conversion feature in branch feature/costmap_conversion works for your use case?

[skohlbr]

Not yet, but will do next week.

[stonier]

We've been refining what we do with Costmap2DROS conversions into cost maps - details here. Basically the use case we were interested in supporting is the ability to catch a subwindow of the cost map around the ros pose. @skohlbr is this a use case you were looking for as well?

This required us to be a little more careful (i.e. catch a few bugs) to make sure we don't introduce errors by having the resulting cost map not exactly aligning with the underlying ros costmap - it is not as straight forward as just setting the cost map centre to the robot pose position.

Anyway, seems to be working now and we have a battery of tests catching the different permutations. @pfankhauser it looks like something I could easily template in the style you made in the PR so we could use it for both cost map and grid map converters, just with different policies applied (e.g. direct value cast, or normalising from 0 to 100.0 or similar).

[pfankhauser]

Solved by #78.