Cost Map

This is a C++ library directly analogous to ETHZ ASL’s [GridMap] library, but designed for use with costs where the data element is an unsigned char (as opposed to grid_map’s doubles).

Table of Contents

1. Packages Overview
2. CostMap
3. Publishing and Subscribing
4. Visualisations
5. Saving and Loading
6. Costmap2DROS Conversions
7. Other Conversions
8. Inflation Computers

Packages Overview

• cost_map : meta-package for the grid map library.
• cost_map_core : core interfaces and algorithms of the cost map library, this package has no [ROS] dependencies.
• cost_map_ros : converters and utilities for cost maps in a [ROS] ecosystem - Image Bundles, CostMap2DROS, OccupancyGrid, CostMap messsages.
• cost_map_cv : conversions from and to [OpenCV] image types.
• cost_map_msgs : [ROS] message definitions related to the [cost_map_msgs/CostMap] type.
• cost_map_visualisations : helper nodes that bridge cost maps to [RViz].
• cost_map_demos : several nodes for demonstration purposes and a test suite.

The source code is released under a BSD 3-Clause license.

CostMap

The core cost_map::CostMap class is designed to maintain as close a compatibility to grid maps as possible. This compatibility extends to conventions, definitions and API. For convenience, an illustrative reference of a few of the conventions that hold for both cost maps and grid maps is provided below.

Please refer to the GridMap README for a more comprehensive outline of what is possible with cost maps and grid maps.

Doxygen documentation for the cost map API’s is served via the Code API links on the ROS wiki for each respective release/package. In addition the latest doxygen documentation can always be found for each package at:

• cost_map_core
• cost_map_ros
• cost_map_visualisations

Publishing and Subscribing

Use the cost_map::toMessage()/fromMessage(...) methods.

// Publishing

#include <cost_map_ros/converter.hpp>

void publishCostMap(const cost_map::CostMap& cost_map) {
  cost_map_msgs::CostMap msg;
  toMessage(cost_map, msg);
  publisher.publish(msg);
}

// Subscribing

#include <cost_map_ros/converter.hpp>

void costMapCallback(const cost_map_msgs::CostMap::ConstPtr& msg) {
  cost_map::CostMap cost_map;
  cost_map::fromMessage(*msg, cost_map);
}

Visualisations

As with grid maps, the method employed to visualise is to drop in a relay node between the cost map publisher and rviz to convert it to the appropriate type for visualisation. This spares you from having to write any gui-related code in the cost maps or the planners/controllers that use the cost maps.

cost_map_visualisations has a relay node that converts each layer of a subscribed cost map topic to a nav_msgs/OccupancyGrid topic for rviz. Simply include and remap the input cost map topic in your launcher, for example:

<launch>
  <!-- node that publishes cost maps -->

  <node pkg="cost_map_visualisations" type="node" name="cost_map_visualisation">
    <remap from="cost_map_visualisation/cost_map" to="funky_cost_maps/cost_map"/>
  </node>

  <!-- rviz, with a pre-loaded configuration that loads the resulting occupancy grid maps -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find funky_demos)/rviz/funky_cost_maps.rviz" />
</launch>

File truncated at 100 lines see the full file