An Introduction to Path Planning using in ROS2

Ever wonder how a mobile robot intelligently navigates from its charging dock to the kitchen without bumping into walls or falling down the stairs? The magic behind this is a process called path planning. Think of it as your robot’s personal GPS, but instead of navigating roads, it’s charting a course through a much more complex and dynamic environment like a warehouse floor or your living room.

The primary objective of a path planning algorithm is straightforward: to find the safest and most efficient route from a starting point (Point A) to a destination (Point B). It takes a map of the environment, often represented as a costmap where different values indicate obstacles or difficult terrain, and computes a collision-free series of waypoints for the robot to follow. This calculated route is known as the global path.

What is nav2_planner_server?

Navigation2 stack (Nav2) handles all aspects of autonomous navigation, and the component responsible for generating this global path is the nav2_planner_server. Its job is to:

1. Receive a navigation goal.
2. Load the current map of the environment.
3. Use a specific algorithm to compute the optimal path.
4. Publish this path for other Nav2 components, like the controller, to execute.

The nav2_planner_server operates using a plugin-based architecture, which means you can easily swap between different path planning algorithms to best suit your robot and its environment. For this, you need to configure the planner serve configuration file, that is usually a YAML file. Here, you define a list of planner_plugins that you want to be available. Each plugin is given a unique name and is then mapped to a specific planner implementation.

planner_server:
  ros__parameters:
    planner_plugins: ['GridBased']
    GridBased:
      plugin: 'nav2_navfn_planner::NavfnPlanner' # In Iron and older versions, "/" was used instead of "::"
      use_astar: True
      allow_unknown: True
      tolerance: 1.0

The NavfnPlanner Plugin

The Navfn Planner plugin is the default path planning plugin that is used by the planner_server. It’s implements a wavefront Dijkstra or A* expanded holonomic planner. Your use its recommend for simple, grid-based navigation where speed is a priority and the robot can be approximated as a circle. It’s a great starting point for many applications. Keep in mind that the paths generated can sometimes be jagged or not perfectly smooth, which might not be ideal for all robot controllers. Your parameters are:

• tolerance: Tolerance in meters between requested goal pose and end of path. The default value is 0.5.
• use_astar: Whether to use A*. If false, uses Dijkstra’s expansion. The default value is False.
• allow_unknown: Whether to allow planning in unknown space. The default value is True.
• use_final_approach_orientation: If true, the last pose of the path generated by the planner will have its orientation set to the approach orientation, i.e. the orientation of the vector connecting the last two points of the path. The default value is False.
• allow_parameter_qos_overrides: Whether to allow QoS profiles to be overwritten with parameterized values. The default value is True.

How to use the nav2_planner_server?

The Planner Server acts as a ROS2 Action Server. Action servers are similar to a canonical service server. A client will request some task to be completed, except, this task may take a long time. An example would be moving the shovel up from a bulldozer or ask a robot to travel 10 meters to the right.

To initialize the Planner Server using the .yaml configuration file, run on your workspace:

$ ros2 run nav2_planner planner_server --ros-args --params-file /the/path/to/your/config.yaml

This command will initialize the action server and the planner node, which is a lifecycle node. This is a special type of node that contains state machine transitions for bringup and teardown of ROS 2 servers. When you start the action server, it is initialized in the unconfigured state. For configure the lifecycle node, run the command in another terminal:

$ ros2 lifecycle set /planner_server 1

Now, the node needs to be actived. You can do this by running:

$ ros2 lifecycle set /planner_server 3

After this, the Planner server is up running and read to receive goals for tracing paths. To see if its configured correctly, run the command ros2 action list. The servers /compute_path_to_pose and compute_path_throught_poses must be shown on the terminal.

To tracing a new path to the desired goal, you need to send a goal to the planner_server using the nav2_msg/action/ComputePathToPose messsage. You can press Tab to autocomplete the message layout in the terminal and complete with the desired location for the robot. The planned path is published in the topic /plan and be seen on the RViZ, as showed below.