About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Costmap conversion

Description: In this tutorial you will learn how to apply costmap conversion plugins to convert occupied costmap2d cells to geometric primitives for optimization (experimental).

Keywords: Trajectory Optimization Local Planner Navigation Costmap Obstacles

Tutorial Level: INTERMEDIATE

Next Tutorial: Planning for car-like robots

The teb_local_planner package supports costmap_converter plugins. Those plugins convert occupied costmap_2d cells to geometric primitives (points, lines, polygons) as obstacles.

Without activating any plugin, each occupied costmap cell is treated as single point-obstacle. This leads to a large amount of obstacles if the resolution of the map is high and may introduce longer computation times or instabilities in the calculation of distinctive topologies (which depend on the number of obstacles). On the other hand, the conversion of obstacles also takes time. However, the conversion time strongly depends on the selected algorithm and it can be performed in a separate thread. Up to know, the costmap conversion is experimental and the best configuration in sense of efficiency and navigation quality must be determined in the future. Any feedback or improvement is welcome!

Refer to the Node-API for detailed information on parameters.

Activate a plugin by setting its name via the ros parameter ~<name>/costmap_converter_plugin. You can find the names in the description of the costmap_converter package or by checking available plugins:

rospack plugins --attrib=plugin costmap_converter

The namespace for individual plugin parameters is extended by the plugin name (see parameter examples below).

You might also specify parameters ~<name>/costmap_converter_spin_thread and ~<name>/costmap_converter_rate.

The plugin must be loaded before launching the node. All parameters related to the plugin itself are changeable during runtime using rqt_reconfigure.

Obstacles are visualized using the ~<name>/teb_markers topic.

Deactivate the conversion by setting an empty string as plugin_name.

Example parameter file

The following file costmap_converter_params.yaml demonstrates how to set the parameters according to the robot setup in tutorial Configure and run Robot Navigation:

TebLocalPlannerROS:

  ## Costmap converter plugin   
  costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
  #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
  #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
  #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
  #costmap_converter_plugin: "" # deactivate plugin
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5
 
 
  ## Configure plugins (namespace move_base/TebLocalPlannerROS/PLUGINNAME)
  ## The parameters must be added for each plugin separately
  costmap_converter/CostmapToLinesDBSRANSAC:
    cluster_max_distance: 0.4
    cluster_min_pts: 2
    ransac_inlier_distance: 0.15
    ransac_min_inliers: 10
    ransac_no_iterations: 2000
    ransac_remainig_outliers: 3
    ransac_convert_outlier_pts: True
    ransac_filter_remaining_outlier_pts: False
    convex_hull_min_pt_separation: 0.1

Now you can load your parameters into the move_base namespace. Change your navigation launch file to load the costmap_conversion_params.yaml.

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

    <!-- Load all costmap_2d and planner parameters here -->

    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />


    <!-- LOAD COSTMAP_CONVERTER PARAMETERS HERE -->
    <rosparam file="$(find my_robot_name_2dnav)/costmap_converter_params.yaml" command="load" />

</node>

Complete Example

You can find an example setup with the stage simulator in the teb_local_planner_tutorials package.

1. Install the teb_local_planner_tutorials package

2. Inspect the parameter/config files

3. Launch the diff_drive setup: roslaunch teb_local_planner_tutorials robot_diff_drive_in_stage_costmap_conversion.launch

4. Modify parameters using rosrun rqt_reconfigure rqt_reconfigure or by adapting the files.

5. Switch different costmap conversion algorithms in costmap_converter_params.yaml