Global costmap plugin for NAV2. It generates a costmap based on a digital elevation map (DEM) received by topic.
For a further understanding of the algorithm check:
@INPROCEEDINGS{9340912,
author={Paz-Delgado, G.J. and Azkarate, M. and Sánchez-Ibáñez, J. R. and Pérez-del-Pulgar, C.J. and Gerdes, L. and García-Cerezo, A.J.},
booktitle={2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
title={Improving Autonomous Rover Guidance in Round-Trip Missions Using a Dynamic Cost Map},
year={2020},
volume={},
number={},
pages={7014-7019},
doi={10.1109/IROS45743.2020.9340912}
}
If you are trying to use this plugin in your NAV2 project, please read carefully.
matrix_msg (Required)
dem_publisher (Required)
map_publisher (Required)
types (Required)
You can download them from nav2_plugins_srl
The installation process for each of the plugins is analogous. In this guide, it is assumed that you already have a ROS2 workspace with the NAV2 package navigation2 installed (see NAV2 Installation):
- Download the corresponding plugin file from the relevant repository. The architecture of all packages is the same: a main folder containing an src folder, an include folder, and various files such as
CMakeLists.txt. - Place the main folder of the plugin inside the src folder of the ROS2 workspace you are using. That is, the package should be in the same directory as the navigation2 package of NAV2 (do not put the downloaded package inside the navigation2 package). In the next picture, I show an example for the workspace architecture:
- Once the package is placed in the correct directory, you need to build the package. Open a terminal in Ubuntu and navigate to the workspace directory (it is important to be inside the main folder of the directory, not inside any of the subfolders). Note that the cost map plugin makes use of a customised ROS2 message. Before building the cost map plugin package, it is necessary to build the message package and source the workspace directory. To do this, execute the following command:
cd <your_workspace>
colcon build --symlink-install --packages-select matrix_msg
. install/local_setup.bash
colcon build --symlink-install --packages-select <plugin_package_name>Where <your_workspace> is the directory of your workspace, and <plugin_package_name> is the name of the plugin package you want to build (in this case nav2_global_dem_layer).
Remember to replace <your_workspace> and <plugin_package_name> with the appropriate values in your case.
Once these steps are executed, the plugin package will be built and ready to be used in your ROS2 workspace.
When running NAV2, Python files are used as launch files. By default, the navigation2 package includes an example launch file that can still be used. This launch file takes parameters from the nav2_params.yaml file (located at navigation2/nav2_bringup/params). It is possible to configure NAV2 to use a different parameter file. The important thing is that this file must specify several aspects of the execution. There are two crucial aspects: configuring the plugin to use for each navigation server and specifying the values of the specific parameters used by the plugin. Therefore, before running NAV2, it is important to indicate which installed plugin to use and configure the plugin parameters.
Below is an example configuration of the global costmap:
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: map
robot_base_frame: base_link
use_sim_time: true
robot_radius: 0.22
resolution: 0.05
track_unknown_space: true
plugins: ["global_dem_layer"]
global_dem_layer:
plugin: "nav2_global_dem_layer/global_dem_layer"
map_subscribe_transient_local: True
enabled: True
d_temptative_slope_threshold: 20.0
d_temptative_sd_threshold: 16.82
d_temptative_valid_ratio_threshold: 0.5
d_temptative_contour_ratio_threshold: 0.3
d_temptative_wheel_radius: 0.06 #meters
d_temptative_avoid_dist: 1.0 #meters
d_temptative_occ_radius: 0.70 #meters
d_temptative_dilation: 0.8 #%
b_temptative_clear_underneath: true
d_temptative_rover2DEM_gap: 1.0 #meters
d_temptative_DEM_height: 6.0 #meters
b_temptative_accept_underneath_goals: false
plan_topic: /plan
DEM_topic: /elevation_map
goal_topic: /goal_pose
initial_pose_topic: /estimate_pose
rover_pose_topic: /estimate_pose
global_frame_id: map
use_maximum: True
transform_tolerance: 0.1 #0.1
demLayer_debugMode: False
slope_weight_gain: 300.0
always_send_full_costmap: true- update_frequency: Frequency at which the costmap algorithm is executed and displayed in RVIZ in hertz.
- publish_frequency: Frequency at which the costmap is published on the costmap topic in hertz.
- plugins: List of plugins used for the global costmap. Multiple plugins can be combined.
- plugin: Name of the developed plugin.
- d_temptative_slope_threshold: Average slope threshold in degrees. Used to determine which slopes should be considered obstacles.
- d_sd_threshold: Spherical deviation in degrees.
- d_temptative_valid_ratio_threshold: Ratio of valid cells in the input DEM to be used by the algorithm.
- d_temptative_contour_ratio_threshold: Proportion of valid contour cells.
- d_temptative_wheel_radius: Wheel radius in meters.
- d_temptative_avoid_dist: Distance in meters to the risk area (obstacles) that the robot should maintain.
- d_temptative_occ_radius: Robot radius in meters.
- d_temptative_dilation: Obstacle dilation radius in % (1.0 = 100% --> dilatation == robot radius).
- b_temptative_clear_underneath: This parameter decides whether to create a zone around the robot's initial position where the costmap is cleared of obstacles.
- d_temptative_rover2DEM_gap: Distance in meters from the robot to the start of the DEM.
- d_DEM_height: Average height of the DEM. This helps identify possible holes in the DEM.
- b_temptative_accept_underneath_goals: Parameter that controls whether it is allowed to request the robot to move to a position within the underneath area.
- plan_topic: Topic on which the global plan created by the planner is published.
- DEM_topic: Topic on which the digital elevation map used to compute the costmap is published.
- goal_topic: Topic on which the final pose is published if it needs to be changed.
- initial_pose_topic: Topic on which the robot's initial pose is received.
- rover_pose_topic: Topic on which the robot's pose is received.
- global_frame_id: Name of the reference frame in which the global path is defined.
- use_maximum: This parameter defines how the costmap is updated. If set to true, the maximum cost value for each cell in the master layer of the costmap is always maintained (considering the fusion of all costmap layers). If set to false, the cost information calculated by this plugin overwrites any other costmap layer.
- transform_tolerance: When performing control, the pose needs to be transformed to the path's reference frame. This parameter defines the maximum tolerance in seconds for the transformation.
- demLayer_debugMode: Enables or disables plugin-related messages displayed in the terminal during NAV2 execution.
- slope_weight_gain: Gain that modulates the weight given to slopes in the costmap.
In the case of the developed plugin, it does not require any environment variables.
cd <your_workspace>
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/route/to/your/gazebo/models
export DEM_FILES_PATH="route/to/the/file/with/the/routes/of/DEM/stuff"
export <ENVIROMENT_VARIABLE>=$ENVIROMENT_VARIABLE:/route/to/any/other/enviroment/variable
. install/local_setup.bash
ros2 launch <package/where/your/launch/file/is> <launch/file> params_file:=/route/to/your/yaml/configuration/fileAn example with the figure showed before is (attention, the following example will not work on your computer. You have to adapt it to your directories and paths):
cd test_ws
export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/humble/share/turtlebot3_gazebo/models
export DEM_FILES_PATH="/home/sergio/test_ws/src/dem_publisher/inputs/DEMs/directories.txt"
. install/local_setup.bash
ros2 launch nav2_bringup tb3_simulation_launch.py params_file:=/home/sergio/test_ws/src/navigation2/nav2_bringup/params/nav2_params.yamlNote that the plugin expects to receive the digital elevation map along with the offset and resolution via a topic using a custom ROS2 message. A mechanism is needed to publish the digital elevation map in that topic. One option is to use the dem_publisher node provided in this repository.
cd <your_workspace>
colcon build --symlink-install --packages-select matrix_msg
. install/local_setup.bash
colcon build --symlink-install --packages-select dem_publisherIn this case, dem_publisher needs to be told to indicate in its environment variable the directory where the file containing the addresses of the digital elevation map, the offset of the map source and its resolution.
cd <your_workspace>
export DEM_FILES_PATH="route/to/the/file/with/the/routes/of/DEM/stuff"
. install/local_setup.bash
ros2 run <package_name> <executable_name>An example (attention, the following example will not work on your computer. You have to adapt it to your directories and paths):
cd test_ws
export DEM_FILES_PATH="/home/sergio/test_ws/src/dem_publisher/inputs/DEMs/directories.txt"
. install/local_setup.bash
ros2 run dem_publisher dem_publisher_nodeNAV2 has a map publishing node capable of publishing maps in three-colour scale (0-255). One of the major problems with this is that in case you want to display a non-occupancy map in RVIZ, the result is not graphically interpretable. map_publisher is a node capable of publishing a greyscale map in RVIZ. It is provided in this repository in case you would like to use it.
cd <your_workspace>
colcon build --symlink-install --packages-select matrix_msg
. install/local_setup.bash
colcon build --symlink-install --packages-select map_publisherIn this case, dem_publisher needs to be told to indicate in its environment variable the directory where the file containing the addresses of the digital elevation map, the offset of the map source and its resolution.
cd <your_workspace>
export MAP_FILES_PATH="route/to/the/file/with/the/routes/of/DEM/stuff"
. install/local_setup.bash
ros2 run <package_name> <executable_name>An example (attention, the following example will not work on your computer. You have to adapt it to your directories and paths):
cd test_ws
export MAP_FILES_PATH="/home/sergio/test_ws/src/map_publisher/inputs/MAPs/directories.txt"
. install/local_setup.bash
ros2 run map_publisher map_publisher_nodeIn case you decide to include in the NAV2 launch file the dem_publisher and map_publisher nodes, don't forget to export the environment variable DEM_FILES_PATH and MAP_FILES_PATH in the terminal where you are going to run NAV2 before executing its launch file.