nav2_fmm_multilayered_planner

This plugin implements a multilayered planning algorithm (DyMu). For a further understanding of the algorithm check:

@article{sanchez2019dynamic,
  title={Dynamic path planning for reconfigurable rovers using a multi-layered grid},
  author={S{\'a}nchez-Ib{\'a}nez, J Ricardo and P{\'e}rez-del-Pulgar, Carlos J and Azkarate, Martin and Gerdes, Levin and Garc{\'\i}a-Cerezo, Alfonso},
  journal={Engineering Applications of Artificial Intelligence},
  volume={86},
  pages={32--42},
  year={2019},
  publisher={Elsevier},
  doi={10.1016/j.engappai.2019.08.011}
}

If you are trying to use this plugin in your NAV2 project, please read carefully.

Installation Guide

Description

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):

Installation Steps

1. 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.
2. 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:

3. 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). To do this, execute the following command:

cd <your_workspace>
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_fmm_multilayered_planner).

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.

Configuration Guide

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 trajectory control:

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0 #20.0 cambio
    use_sim_time: True 
    plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_fmm_multilayered_planner/FastMarching"
      fm_waypointDistance: 0.5
      fm_costmap_resolution: 0.1      
      fm_debugMode: True
      fm_risk_map_topic: /risk_map     
      fm_goal_topic: /goal_pose
      fm_current_wp_topic: /trajectory_control_current_segment
      fm_rover_pose_topic: /estimate_pose
      fm_risk_ratio: 100.0
      fm_risk_distance: 1.3
      fm_risk_thres: 0.0      

• plugin: Specifies the plugin to be used by the planning server.
• fm_waypointDistance: Distance between two points on the computed path in the x and y directions (in map cells).
• fm_costmap_resolution: Resolution of the costmap used for path planning.
• fm_debugMode: Enables or disables the messages displayed in the terminal.
• fm_re_plan_topic: Topic through which the plugin can receive a hypothetical externally planned path from other methods.
• fm_goal_topic: Topic where the last pose of the path can be published.
• fm_risk_map_topic: Topic where the local risk map is published and used for local planning.
• fm_current_wp_topic: Topic where the current waypoint is published. This is used in order to know which waypoints must be taken into consideration in the replannification.
• fm_rover_pose_topic: Topic where the current pose is published.
• fm_risk_ratio: The importance associated with risk mapping when executing the FM* wave in replanning.
• fm_risk_distance: This controls how far is going to be the replanned path from the obstacles.
• fm_risk_thres: Threshold value to consider a node on the risk map a true risk.

Execution

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 <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/file

An 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
. 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.yaml