nav2_costmap_2d
The costmap 2D package makes use of the sensor information to provide a representation of the robot’s environment in the form of an occupancy grid. The cells in the occupancy grid store cost values between 0-254 which denote a cost to travel through these zones. A cost of 0 means the cell is free while a cost of 254 means that the cell is lethally occupied. Values in between these extremes are used by navigation algorithms to steer your robot away from obstacles as a potential field. Costmaps in Nav2 are implemented through the nav2_costmap_2d package.
The costmap implementation consists of multiple layers, each of which has a certain function that contributes to a cell’s overall cost. The package consists of the following layers, but are plugin-based to allow customization and new layers to be used as well: static layer, inflation layer, range layer, obstacle layer, and voxel layer. The static layer represents the map section of the costmap, obtained from the messages published to the /map topic like those produced by SLAM. The obstacle layer includes the objects detected by sensors that publish either or both the LaserScan and PointCloud2 messages. The voxel layer is similar to the obstacle layer such that it can use either or both the LaserScan and PointCloud2 sensor information but handles 3D data instead. The range layer allows for the inclusion of information provided by sonar and infrared sensors. Lastly, the inflation layer represents the added cost values around lethal obstacles such that our robot avoids navigating into obstacles due to the robot’s geometry. In the next subsection of this tutorial, we will have some discussion about the basic configuration of the different layers in nav2_costmap_2d.
The layers are integrated into the costmap through a plugin interface and then inflated using a user-specified inflation radius, if the inflation layer is enabled. For a deeper discussion on costmap concepts, you can have a look at the ROS1 costmap_2D documentation. Note that the nav2_costmap_2d package is mostly a straightforward ROS2 port of the ROS1 navigation stack version with minor changes required for ROS2 support and some new layer plugins.
Configuring nav2_costmap_2d
In this subsection, we will show an example configuration of nav2_costmap_2d such that it uses the information provided by the lidar sensor of sam_bot. We will show an example configuration that uses static layer, obstacle layer, voxel layer, and inflation layer. We set both the obstacle and voxel layer to use the LaserScan messages published to the /scan topic by the lidar sensor. We also set some of the basic parameters to define how the detected obstacles are reflected in the costmap. Note that this configuration is to be included in the configuration file of Nav2.
In the configuration above, notice that we set the parameters for two different costmaps: global_costmap and local_costmap. We set up two costmaps since the global_costmap is mainly used for long-term planning over the whole map while local_costmap is for short-term planning and collision avoidance.
The layers that we use for our configuration are defined in the plugins parameter, as shown in line 13 for the global_costmap and line 50 for the local_costmap. These values are set as a list of mapped layer names that also serve as namespaces for the layer parameters we set up starting at lines 14 and line 51. Note that each layer/namespace in this list must have a plugin parameter (as indicated in lines 15, 18, 32, 52, and 68) defining the type of plugin to be loaded for that specific layer.
For the static layer (lines 14-16), we set the map_subscribe_transient_local parameter to True. This sets the QoS settings for the map topic. Another important parameter for the static layer is the map_topic which defines the map topic to subscribe to. This defaults to /map topic when not defined.
For the obstacle layer (lines 17-30), we define its sensor source under the observation_sources parameter (line 20) as scan whose parameters are set up in lines 22-30. We set its topic parameter as the topic that publishes the defined sensor source and we set the data_type according to the sensor source it will use. In our configuration, the obstacle layer will use the LaserScan published by the lidar sensor to /scan.
Note that the obstacle layer and voxel layer can use either or both LaserScan and PointCloud2 as their data_type but it is set to LaserScan by default. The code snippet below shows an example of using both the LaserScan and PointCloud2 as the sensor sources. This may be particularly useful when setting up your own physical robot.
For the other parameters of the obstacle layer, the max_obstacle_height parameter sets the maximum height of the sensor reading to return to the occupancy grid. The minimum height of the sensor reading can also be set using the min_obstacle_height parameter, which defaults to 0 since we did not set it in the configation. The clearing parameter is used to set whether the obstacle is to be removed from the costmap or not. The clearing operation is done by raytracing through the grid. The maximum and minimum range to raytrace clear objects from the costmap is set using the raytrace_max_range and raytrace_min_range respectively. The marking parameter is used to set whether the inserted obstacle is marked into the costmap or not. We also set the maximum and minimum range to mark obstacles in the costmap through the obstacle_max_range and obstacle_min_range respectively.
For the inflation layer (lines 31-34 and 67-70), we set the exponential decay factor across the inflation radius using the cost_scaling_factor parameter. The value of the radius to inflate around lethal obstacles is defined using the inflation_radius.
For the voxel layer (lines 51-66), we set the publish_voxel_map parameter to True to enable the publishing of the 3D voxel grid. The resolution of the voxels in height is defined using the z_resolution parameter, while the number of voxels in each column is defined using the z_voxels parameter. The mark_threshold parameter sets the minimum number of voxels in a column to mark as occupied in the occupancy grid. We set the observation_sources parameter of the voxel layer to scan, and we set the scan parameters (in lines 61-66) similar to the parameters that we have discussed for the obstacle layer. As defined in its topic and data_type parameters, the voxel layer will use the LaserScan published on the /scan topic by the lidar scanner.
Note that the we are not using a range layer for our configuration but it may be useful for your own robot setup. For the range layer, its basic parameters are the topics, input_sensor_type, and clear_on_max_reading parameters. The range topics to subscribe to are defined in the topics parameter. The input_sensor_type is set to either ALL, VARIABLE, or FIXED. The clear_on_max_reading is a boolean parameter that sets whether to clear the sensor readings on max range. Have a look at the configuration guide in the link below in case you need to set it up.
Last updated