In this tutorial, I will show you how to use the Regulated Pure Pursuit controller plugin that comes with the ROS 2 Navigation Stack (also known as Nav2). This controller plugin is used to track a path that is generated by a path planning algorithm. I have found that Regulated Pure Pursuit generates smoother control than any other control algorithm I have used with Nav2, including the default DWB algorithm. Here is the final output you will be able to achieve after going through this tutorial:

At a high level, with the pure pursuit algorithm, we assume that we know the path to a goal location. The algorithm calculates the linear velocity and angular velocity that will move the robot from its current location to some look-ahead point along the path in front of the robot.

You can read more about the pure pursuit algorithm in the original paper.

The Regulated Pure Pursuit algorithm is an improvement over the pure pursuit algorithm. What I like about this algorithm is that it slows down while making sharp turns around blind corners.

You can read about the Regulated Pure Pursuit algorithm on this page. That page also has the different parameters that you can configure inside your parameters yaml file (which I will give you later in this tutorial).

Prerequisites

• ROS 2 Foxy Fitzroy installed on Ubuntu Linux 20.04 or newer. I am using ROS 2 Galactic, which is the latest version of ROS 2 as of the date of this post.
• You have already created a ROS 2 workspace. The name of our workspace is “dev_ws”, which stands for “development workspace.” 
• You have Python 3.7 or higher.
• (Optional) You have completed my Ultimate Guide to the ROS 2 Navigation Stack.
• (Optional) You have a package named two_wheeled_robot inside your ~/dev_ws/src folder, which I set up in this post. If you have another package, that is fine.
• (Optional) You know how to load a world file into Gazebo using ROS 2.

You can find the files for this post here on my Google Drive. 

Create a World

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/worlds

Make sure this world is inside this folder. The name of the file is hospital.world.

Create a Map

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/maps/hospital_world

Make sure the pgm and yaml map files are inside this folder.

My hospital map is made up of two files:

• hospital_world.pgm
• hospital_world.yaml

Create the Parameters File

Let’s create the parameters file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/params/hospital_world

Add the nav2_params_regulated_pure_pursuit.yaml file from this folder. 

Create the RViz Configuration File

Let’s create the RViz configuration file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/rviz/hospital_world

Add the nav2_config.rviz file from this folder. 

Create the Launch File

Let’s create the launch file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/launch/hospital_world

Add the hospital_world_regulated_pure_pursuit.launch.py file from this folder. 

Here is the code:

# Author: Addison Sears-Collins
# Date: November 9, 2021
# Description: Launch a two-wheeled robot using the ROS 2 Navigation Stack. 
#              The spawning of the robot is performed by the Gazebo-ROS spawn_entity node.
#              The robot must be in both SDF and URDF format.
#              If you want to spawn the robot in a pose other than the default, be sure to set that inside
#              the nav2_params_path yaml file: amcl ---> initial_pose: [x, y, z, yaw]
#              The robot uses the Regulated Pure Pursuit Controller for path tracking.
# https://automaticaddison.com

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
  package_name = 'two_wheeled_robot'
  robot_name_in_model = 'two_wheeled_robot'
  default_launch_dir = 'launch'
  gazebo_models_path = 'models'
  map_file_path = 'maps/hospital_world/hospital_world.yaml'
  nav2_params_path = 'params/hospital_world/nav2_params_regulated_pure_pursuit.yaml'
  robot_localization_file_path = 'config/ekf.yaml'
  rviz_config_file_path = 'rviz/hospital_world/nav2_config.rviz'
  sdf_model_path = 'models/two_wheeled_robot_description/model.sdf'
  urdf_file_path = 'urdf/two_wheeled_robot.urdf'
  world_file_path = 'worlds/hospital.world'
  
  # Pose where we want to spawn the robot
  spawn_x_val = '0.0'
  spawn_y_val = '2.0'
  spawn_z_val = '0.25'
  spawn_yaw_val = '0.0'

  ########## You do not need to change anything below this line ###############
  
  # Set the path to different files and folders.  
  pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')   
  pkg_share = FindPackageShare(package=package_name).find(package_name)
  default_launch_dir = os.path.join(pkg_share, default_launch_dir)
  default_urdf_model_path = os.path.join(pkg_share, urdf_file_path)
  robot_localization_file_path = os.path.join(pkg_share, robot_localization_file_path) 
  default_rviz_config_path = os.path.join(pkg_share, rviz_config_file_path)
  world_path = os.path.join(pkg_share, world_file_path)
  gazebo_models_path = os.path.join(pkg_share, gazebo_models_path)
  os.environ["GAZEBO_MODEL_PATH"] = gazebo_models_path
  nav2_dir = FindPackageShare(package='nav2_bringup').find('nav2_bringup') 
  nav2_launch_dir = os.path.join(nav2_dir, 'launch') 
  sdf_model_path = os.path.join(pkg_share, sdf_model_path)
  static_map_path = os.path.join(pkg_share, map_file_path)
  nav2_params_path = os.path.join(pkg_share, nav2_params_path)
  nav2_bt_path = FindPackageShare(package='nav2_bt_navigator').find('nav2_bt_navigator')
  
  # Launch configuration variables specific to simulation
  autostart = LaunchConfiguration('autostart')
  headless = LaunchConfiguration('headless')
  map_yaml_file = LaunchConfiguration('map')
  namespace = LaunchConfiguration('namespace')
  params_file = LaunchConfiguration('params_file')
  rviz_config_file = LaunchConfiguration('rviz_config_file')
  sdf_model = LaunchConfiguration('sdf_model')
  slam = LaunchConfiguration('slam')
  urdf_model = LaunchConfiguration('urdf_model')
  use_namespace = LaunchConfiguration('use_namespace')
  use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
  use_rviz = LaunchConfiguration('use_rviz')
  use_sim_time = LaunchConfiguration('use_sim_time')
  use_simulator = LaunchConfiguration('use_simulator')
  world = LaunchConfiguration('world')
  
  # Map fully qualified names to relative ones so the node's namespace can be prepended.
  # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
  # https://github.com/ros/geometry2/issues/32
  # https://github.com/ros/robot_state_publisher/pull/30
  # TODO(orduno) Substitute with `PushNodeRemapping`
  #              https://github.com/ros2/launch_ros/issues/56
  remappings = [('/tf', 'tf'),
                ('/tf_static', 'tf_static')]
  
  # Declare the launch arguments  
  declare_namespace_cmd = DeclareLaunchArgument(
    name='namespace',
    default_value='',
    description='Top-level namespace')

  declare_use_namespace_cmd = DeclareLaunchArgument(
    name='use_namespace',
    default_value='false',
    description='Whether to apply a namespace to the navigation stack')
        
  declare_autostart_cmd = DeclareLaunchArgument(
    name='autostart', 
    default_value='true',
    description='Automatically startup the nav2 stack')

  declare_map_yaml_cmd = DeclareLaunchArgument(
    name='map',
    default_value=static_map_path,
    description='Full path to map file to load')
        
  declare_params_file_cmd = DeclareLaunchArgument(
    name='params_file',
    default_value=nav2_params_path,
    description='Full path to the ROS2 parameters file to use for all launched nodes')
    
  declare_rviz_config_file_cmd = DeclareLaunchArgument(
    name='rviz_config_file',
    default_value=default_rviz_config_path,
    description='Full path to the RVIZ config file to use')

  declare_sdf_model_path_cmd = DeclareLaunchArgument(
    name='sdf_model', 
    default_value=sdf_model_path, 
    description='Absolute path to robot sdf file')

  declare_simulator_cmd = DeclareLaunchArgument(
    name='headless',
    default_value='False',
    description='Whether to execute gzclient')

  declare_slam_cmd = DeclareLaunchArgument(
    name='slam',
    default_value='False',
    description='Whether to run SLAM')

  declare_urdf_model_path_cmd = DeclareLaunchArgument(
    name='urdf_model', 
    default_value=default_urdf_model_path, 
    description='Absolute path to robot urdf file')
    
  declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
    name='use_robot_state_pub',
    default_value='True',
    description='Whether to start the robot state publisher')

  declare_use_rviz_cmd = DeclareLaunchArgument(
    name='use_rviz',
    default_value='True',
    description='Whether to start RVIZ')
    
  declare_use_sim_time_cmd = DeclareLaunchArgument(
    name='use_sim_time',
    default_value='true',
    description='Use simulation (Gazebo) clock if true')

  declare_use_simulator_cmd = DeclareLaunchArgument(
    name='use_simulator',
    default_value='True',
    description='Whether to start the simulator')

  declare_world_cmd = DeclareLaunchArgument(
    name='world',
    default_value=world_path,
    description='Full path to the world model file to load')
   
  # Specify the actions

  # Start Gazebo server
  start_gazebo_server_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
    condition=IfCondition(use_simulator),
    launch_arguments={'world': world}.items())

  # Start Gazebo client    
  start_gazebo_client_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')),
    condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])))

  # Launch the robot
  spawn_entity_cmd = Node(
    package='gazebo_ros',
    executable='spawn_entity.py',
    arguments=['-entity', robot_name_in_model,
               '-file', sdf_model,
                  '-x', spawn_x_val,
                  '-y', spawn_y_val,
                  '-z', spawn_z_val,
                  '-Y', spawn_yaw_val],
       output='screen')

  # Start robot localization using an Extended Kalman filter
  start_robot_localization_cmd = Node(
    package='robot_localization',
    executable='ekf_node',
    name='ekf_filter_node',
    output='screen',
    parameters=[robot_localization_file_path, 
    {'use_sim_time': use_sim_time}])

  # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
  start_robot_state_publisher_cmd = Node(
    condition=IfCondition(use_robot_state_pub),
    package='robot_state_publisher',
    executable='robot_state_publisher',
    namespace=namespace,
    parameters=[{'use_sim_time': use_sim_time, 
    'robot_description': Command(['xacro ', urdf_model])}],
    remappings=remappings,
    arguments=[default_urdf_model_path])

  # Launch RViz
  start_rviz_cmd = Node(
    condition=IfCondition(use_rviz),
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    output='screen',
    arguments=['-d', rviz_config_file])    

  # Launch the ROS 2 Navigation Stack
  start_ros2_navigation_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(os.path.join(nav2_launch_dir, 'bringup_launch.py')),
    launch_arguments = {'namespace': namespace,
                        'use_namespace': use_namespace,
                        'slam': slam,
                        'map': map_yaml_file,
                        'use_sim_time': use_sim_time,
                        'params_file': params_file,
                        'autostart': autostart}.items())

  # Create the launch description and populate
  ld = LaunchDescription()

  # Declare the launch options
  ld.add_action(declare_namespace_cmd)
  ld.add_action(declare_use_namespace_cmd)
  ld.add_action(declare_autostart_cmd)
  ld.add_action(declare_map_yaml_cmd)
  ld.add_action(declare_params_file_cmd)
  ld.add_action(declare_rviz_config_file_cmd)
  ld.add_action(declare_sdf_model_path_cmd)
  ld.add_action(declare_simulator_cmd)
  ld.add_action(declare_slam_cmd)
  ld.add_action(declare_urdf_model_path_cmd)
  ld.add_action(declare_use_robot_state_pub_cmd)  
  ld.add_action(declare_use_rviz_cmd) 
  ld.add_action(declare_use_sim_time_cmd)
  ld.add_action(declare_use_simulator_cmd)
  ld.add_action(declare_world_cmd)

  # Add any actions
  ld.add_action(start_gazebo_server_cmd)
  ld.add_action(start_gazebo_client_cmd)
  #ld.add_action(spawn_entity_cmd)
  ld.add_action(start_robot_localization_cmd)
  ld.add_action(start_robot_state_publisher_cmd)
  ld.add_action(start_rviz_cmd)
  ld.add_action(start_ros2_navigation_cmd)

  return ld

Launch the Launch File

We will now build our package.

cd ~/dev_ws/

colcon build

Open a new terminal and launch the robot in a Gazebo world. All of this is a single command: 

ros2 launch two_wheeled_robot hospital_world_regulated_pure_pursuit.launch.py

Select the Nav2 Goal button at the top of RViz, and click somewhere on the map to command the robot to navigate to any reachable goal location.

The robot will move to the goal location.

That’s it! Keep building!

In this tutorial, I will show you how to create an object-following robot using the ROS 2 Navigation Stack (also known as Nav2). The goal is to develop an application that will enable a robot to follow a moving object at a distance indefinitely. Here is the final output you will be able to achieve after going through this tutorial:

The official tutorial is on this page, but we will walk through the steps below.

Developing the code to detect the dynamic object is outside the scope of this tutorial (you can see this post though on how to integrate OpenCV and ROS 2). What we will focus on here is making sure we keep publishing an updated pose (of a dynamic object) to a topic. The Navigation Stack will then have logic to navigate the robot toward that pose.

Real-World Applications

The application that we will develop in this tutorial can be used in a number of real-world robotic applications: 

• Person-following robot
• Luggage-carrying robot

We will focus on creating an application that will follow a moving point (that we set on the map) around a hospital.

Prerequisites

• ROS 2 Foxy Fitzroy installed on Ubuntu Linux 20.04 or newer. I am using ROS 2 Galactic, which is the latest version of ROS 2 as of the date of this post.
• You have already created a ROS 2 workspace. The name of our workspace is “dev_ws”, which stands for “development workspace.” 
• You have Python 3.7 or higher.
• (Optional) You have completed my Ultimate Guide to the ROS 2 Navigation Stack.
• (Optional) You have a package named two_wheeled_robot inside your ~/dev_ws/src folder, which I set up in this post. If you have another package, that is fine.
• (Optional) You know how to load a world file into Gazebo using ROS 2.

You can find the files for this post here on my Google Drive. 

Create a World

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/worlds

Make sure this world is inside this folder. The name of the file is hospital.world.

Create a Map

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/maps/hospital_world

Make sure the pgm and yaml map files are inside this folder.

My hospital map is made up of two files:

• hospital_world.pgm
• hospital_world.yaml

Create the Parameters File

Let’s create the parameters file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/params/hospital_world

Add the nav2_object_following_params.yaml file from this folder. 

The definitions of the Behavior-Tree parameters are located on this page.

Create the Behavior Tree XML File

Let’s create the behavior tree.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/params/hospital_world

Add the follow_point_bt.xml file from this folder. This file comes from here.

Create the RViz Configuration File

Let’s create the RViz configuration file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/rviz/hospital_world

Add the nav2_config_object_following.rviz file from this folder.

Create the Launch File

Let’s create the launch file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/launch/hospital_world

Add the hospital_world_object_following.launch.py file from this folder.

Launch the Launch File

We will now build our package.

cd ~/dev_ws/

colcon build

Open a new terminal and launch the robot in a Gazebo world. 

ros2 launch two_wheeled_robot hospital_world_object_following.launch.py

Command the robot to navigate to any position. Use the Nav2 Goal button at the top of RViz to simulate a new detection of the object of interest.

The robot will follow the point that you click. It will maintain a distance of 1.0 meter behind it.

You can also use this command in the terminal window to set a goal pose. The “object’s location” is published to the /goal_pose topic as a geometry_msgs/PoseStamped type message. All of this stuff below is a single command.

ros2 topic pub -1 /goal_pose  geometry_msgs/PoseStamped '{ header: {stamp: {sec: 0, nanosec: 0}, frame_id: "map"}, pose: {position: {x: 5.0, y: 0.0, z: 0.25}, orientation: {w: 1.0}}} '

You can also put the above command in code by creating a publisher node, populating a geometry_msgs/PoseStamped type message inside that node, and publishing that message to the /goal_pose topic.

That’s it! Keep building!

. 

In this tutorial, I will show you how to navigate using speed limit zones using the ROS 2 Navigation Stack (also known as Nav2). A speed limit zone is an area which limits the maximum speed of a robot. Here is the final output you will be able to achieve after going through this tutorial:

Notice how the robot moves slower through the gray/black areas than the white areas of the map.

Real-World Applications

The application that we will develop in this tutorial can be used in a number of real-world robotic applications: 

• Hospitals and Medical Centers
• Hotels (e.g. Room Service)
• House
• Offices
• Restaurants
• Warehouses
• And more…

We will focus on creating an application that will limit the speed of a robot in certain areas of a warehouse or factory floor.

Prerequisites

• ROS 2 Foxy Fitzroy installed on Ubuntu Linux 20.04 or newer. I am using ROS 2 Galactic, which is the latest version of ROS 2 as of the date of this post.
• You have already created a ROS 2 workspace. The name of our workspace is “dev_ws”, which stands for “development workspace.” 
• You have Python 3.7 or higher.
• (Optional) You have completed my Ultimate Guide to the ROS 2 Navigation Stack.
• (Optional) You have a package named two_wheeled_robot inside your ~/dev_ws/src folder, which I set up in this post. If you have another package, that is fine.
• (Optional) You know how to load a world file into Gazebo using ROS 2.

You can find the files for this post here on my Google Drive. Credit to this GitHub repository for the map files. And credit to this tutorial that shows the steps for implementing a speed limit filter.

Create a World

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/worlds

Make sure this world is inside this folder. The name of the file is warehouse_speed_limit_zones.world.

Create a Map

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/maps/warehouse_world

Make sure the pgm and yaml map files are inside this folder.

My world map is made up of two files:

• warehouse_world_speed_limit_zones.pgm
• warehouse_world_speed_limit_zones.yaml

Create a Speed Limit Filter

Now we need to create a mask that identifies the speed limit zones. 

This tutorial has instructions on how to use a graphics editor like GIMP (you can install using the sudo apt-get install gimp command) to prepare the filter mask. 

You will start with a copy of the world file you want to use. In this tutorial, I am going to use my warehouse_world_speed_limit_zones.pgm file. You need to edit this file so that speed limit zones are different shades of gray….the darker the gray, the lower the speed limit.

My filter mask is made up of two files:

• speed_mask.pgm
• speed_mask.yaml

Both of these files are located in my ~/dev_ws/src/two_wheeled_robot/maps/warehouse_world/ folder.

Create the Parameters File

Let’s create the parameters file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/params/warehouse_world

Add the speed_limit_zones_params.yaml file from this folder. 

Create the RViz Configuration File

Let’s create the RViz configuration file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/rviz/warehouse_world

Add the nav2_config_speed_limit_zones.rviz file from this folder. 

Create the Launch File

Let’s create the launch file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/launch/warehouse_world

Add the warehouse_world_speed_limit_zones.launch.py file from this folder. 

Launch the Launch File

We will now build our package.

cd ~/dev_ws/

colcon build

Open a new terminal and launch the robot in a Gazebo world. 

ros2 launch two_wheeled_robot warehouse_world_speed_limit_zones.launch.py

Wait for the robot to snap to the estimated initial location within RViz. This process should take a minute or two.

You might notice that the robot’s pose in RViz is not aligned with the pose in Gazebo. Localization using the AMCL (Adaptive Monte Carlo Localization) package is really sensitive to the initial pose estimate. The trick is to make sure the initial location of the robot is in a location with a lot of distinctively shaped obstacles (i.e. the shelves and boxes) for the LIDAR to read. 

Even though the initial pose was set when we launched the robot, it is likely that the estimate in RViz is pretty bad. Let’s set the initial pose again by clicking the 2D Pose Estimate button at the top of RViz and then clicking on the map.

You can also set the initial pose by opening a new terminal window and typing the following command.

ros2 topic pub -1 /initialpose geometry_msgs/PoseWithCovarianceStamped '{ header: {stamp: {sec: 0, nanosec: 0}, frame_id: "map"}, pose: { pose: {position: {x: -3.7, y: 9.0, z: 0.0}, orientation: {w: 1.0}}, } }'

When the robot snaps to the location, the map and odom axes should be pretty close to each other right at the origin of the map (x=0, y=0).

Now send the robot to a goal that is on the other side of the speed limit zones by clicking the “Nav2 Goal” button at the top of RViz and clicking on a goal location.

The robot will move to the goal, slowing down in the speed limit zones.

A success message will print once the robot has reached the goal location.

That’s it! Keep building!