How To Create a Straight Line Path Planner Plugin – ROS 2

In this tutorial, I will show you how to create a path planner plugin that will enable a robot to move in straight lines using the ROS 2 Navigation Stack (also known as Nav2). Here is the final output you will be able to achieve after going through this tutorial:

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
House
Hotels (e.g. Room Service)
Offices
Restaurants
Warehouses
And more…

We will focus on creating an autonomous lawn mower 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. Credit to this GitHub repository for the code.

Download and Build the Navigation 2 Tutorials Packages

Let’s create a ROS 2 package inside our workspace.

In a new terminal window, move to the src folder of your workspace.

cd ~/dev_ws/src

Download the navigation_2 tutorials package.

git clone https://github.com/ros-planning/navigation2_tutorials.git

Move to the root of your workspace.

cd ~/dev_ws/

Check that dependencies are installed for all packages in the workspace.

rosdep install --from-paths src --ignore-src -r -y

You should see a message that says:

“ERROR: the following packages/stacks could not have their rosdep keys resolved to system dependencies: sam_bot_description…”... go to the package.xml file inside the sam_bot_description folder and change rviz to rviz2.

Now run it again.

cd ~/dev_ws/

rosdep install --from-paths src --ignore-src -r -y

Here is the message you should see:

#All required rosdeps installed successfully

Now build the package by typing the following command:

colcon build

You will see some errors that look like this:

rclcpp_lifecycle::LifecycleNode::declare_parameter(const string&)’ is deprecated: declare_parameter() with only a name is deprecated and will be deleted in the future…

Go to your sms_recovery.cpp file inside this directory:

cd ~/dev_ws/src/navigation2_tutorials/nav2_sms_recovery/src

gedit sms_recovery.cpp

Make sure the code looks like this (note the changes made to the node->declare_parameter lines):

// Copyright (c) 2020 Samsung Research America
// This code is licensed under MIT license (see LICENSE.txt for details)

#include <cmath>
#include <chrono>
#include <memory>
#include <string>

#include "nav2_sms_recovery/sms_recovery.hpp"

namespace nav2_sms_recovery
{

SMSRecovery::SMSRecovery()
: Recovery<Action>()
{
}

SMSRecovery::~SMSRecovery()
{
}

void SMSRecovery::onConfigure()
{
  auto node = node_.lock();
  node->declare_parameter<std::string>("account_sid");
  _account_sid = node->get_parameter("account_sid").as_string();
  node->declare_parameter<std::string>("auth_token");
  _auth_token = node->get_parameter("auth_token").as_string();
  node->declare_parameter<std::string>("from_number");
  _from_number = node->get_parameter("from_number").as_string();
  node->declare_parameter<std::string>("to_number");
  _to_number = node->get_parameter("to_number").as_string();
  _twilio = std::make_shared<twilio::Twilio>(_account_sid, _auth_token);
}

Status SMSRecovery::onRun(const std::shared_ptr<const Action::Goal> command)
{
  auto node = node_.lock();
  std::string response;
  bool message_success = _twilio->send_message(
    _to_number,
    _from_number,
    command->message,
    response,
    "",
    false);

  if (!message_success) {
    RCLCPP_INFO(node->get_logger(), "SMS send failed.");
    return Status::FAILED;
  }

  RCLCPP_INFO(node->get_logger(), "SMS sent successfully!");
  return Status::SUCCEEDED;
}

Status SMSRecovery::onCycleUpdate()
{
  return Status::SUCCEEDED;
}

}  // namespace nav2_sms_recovery

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_sms_recovery::SMSRecovery, nav2_core::Recovery)

Save the file and close it.

Now build the packages again.

cd ~/dev_ws/

colcon build

Here is the output:

Here is my final navigation_2_tutorials folder that contains the nav2_straightline_planner package.

You can learn more about the nav2_straightline_planner plugin here.
The code that generates the straight-line path from the starting location to the goal location is the straight_line_planner.cpp file inside the src folder of the nav2_straightline_planner package.

Add the Launch File

Add the launch file.

cd ~/dev_ws/src/two_wheeled_robot/launch/lawn_world

gedit lawn_world_straightline.launch.py

# Author: Addison Sears-Collins
# Date: September 28, 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]
# 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/lawn_world/lawn_world.yaml'
  nav2_params_path = 'params/lawn_world/nav2_params_straightline.yaml'
  robot_localization_file_path = 'config/ekf.yaml'
  rviz_config_file_path = 'rviz/lawn_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/lawn.world'
  
  # Pose where we want to spawn the robot
  spawn_x_val = '0.0'
  spawn_y_val = '0.0'
  spawn_z_val = '0.0'
  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

Save and close.

Add the Parameters File

Add the Nav2 parameters.

cd ~/dev_ws/src/two_wheeled_robot/params/lawn_world

gedit nav2_params_straightline.yaml

Save and close.

Now we build the package.

cd ~/dev_ws/

colcon build

Launch the Autonomous Robotic Lawn Mower

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

ros2 launch two_wheeled_robot lawn_world_straightline.launch.py

Now send the robot on a straight-line path by clicking the “Nav2 Goal” button at the top of RViz and clicking on a goal location.

The robot will move along a straight-line path to the goal.

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

That’s it! Keep building!

How to Load a New Map for Multi-Floor Navigation Using ROS 2

In this tutorial, I will show you how to load a new map at runtime using the command line. We will work with ROS 2. The use case for this tutorial is a robot that needs to navigate between floors of a multi-floor building.

Update: There is a new way to load a new map for navigation at runtime. You can use the changeMap() method provided by the Nav 2 Simple Commander API.

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.

Directions

Suppose you have a room service robot that is running on ROS 2. The robot is moving around the 5th floor of a hospital.

A hospital patient on the 5th floor has finished his meal and summons the robot to his room so the robot can carry the dirty dishes down to the kitchen of the hotel’s cafe. Suppose the hotel cafe is located on the 2nd floor of the building.

We want our robot to navigate from the 5th floor of the hospital –> elevator –> hospital cafe (2nd floor).

The process for performing multi-floor navigation might look something like this:

Navigate to the Patient’s Room
Pick up the Dirty Dishes
Navigate to the Elevator Door
Press the Down Button
Wait for the Elevator to Open
Enter the Elevator
Press the Elevator Button for the 2nd Floor
Wait for the Elevator to Reach the 2nd Floor
Exit the Elevator
Load a New Map of the 2nd Floor Hospital Cafe

Let’s take a look at how to do step 11.

Load the hospital world by opening a new terminal window and typing:

cd ~/dev_ws/

ros2 launch two_wheeled_robot hospital_world_v1.launch.py

Here is what the 5th floor of the hospital looks like:

Now, let’s assume the robot has done steps 1-10 in the list I posted earlier. It is now on the 2nd floor where the cafe is.

Let’s load a map of the cafe. We need to make a ROS 2 service call to the LoadMap service. ROS 2 services work well when you want a request-response interaction.

Let’s see what services we have.

ros2 service list

We could also have used the following command to see the type of each service.

ros2 service list -t

We can see that we have a service called /map_server/load_map. Let’s see the service type.

ros2 service type /map_server/load_map

The output is:

nav2_msgs/srv/LoadMap

Let’s see the structure of the input arguments for this service.

ros2 interface show nav2_msgs/srv/LoadMap

You will see the variable string map_url. Since this is above the —, this piece is the request structure. Everything below the —, is the response structure.

For a ros2 service call, everything above the — are the arguments needed to call the service.

Now we need to call the /map_server/load_map service. The syntax is as follows:

ros2 service call <service_name> <service_type> <arguments>

All of this below is a single command.

ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url: /home/focalfossa/dev_ws/src/two_wheeled_robot/maps/cafe_world/cafe_world.yaml}"

…where cafe_world.yaml is the new map file. Notice you need to provide the full path to the map file.

Here is what RViz looks like after running the command.

You can go in RViz and uncheck the local costmap.

The outline of the reception desk is due to the fact that the robot is still in the hospital world inside Gazebo, so its global costmap is updating accordingly.

Below is what the cafe world actually looks like in Gazebo. You will see that the new map above comes from this cafe world.

That’s how you load a new map at runtime using ROS 2. That’s it. Keep building!

How to Create an Indoor Delivery Robot – ROS 2 Navigation

In this tutorial, I will show you how to create an indoor delivery robot using the ROS 2 Navigation Stack (also known as Nav2) using Python code. Here is the final output you will be able to achieve after going through this tutorial:

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)
Offices
Restaurants
Warehouses
And more…

We will focus on offices in this tutorial. You can see how the office world looks by going to this post.

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 Python scripts. You can find an explanation of Nav2 here.

Directions

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/scripts

Open a new Python program called pick_and_deliver.py.

gedit pick_and_deliver.py

Add this code.

#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Modified by AutomaticAddison.com

import time # Time library
from copy import deepcopy # Modifying a deep copy does not impact the original

from geometry_msgs.msg import PoseStamped # Pose with ref frame and timestamp
from rclpy.duration import Duration # Handles time for ROS 2
import rclpy # Python client library for ROS 2

from robot_navigator import BasicNavigator, NavigationResult # Helper module

# Positions for picking up items
pick_positions = {
  "front_reception_desk": [-2.5, 1.4],
  "rear_reception_desk": [-0.36, 20.0],
  "main_conference_room": [18.75, 15.3],
  "main_break_room": [15.5, 5.4]}

# Positions for delivery of items
shipping_destinations = {
  "front_reception_desk": [-2.5, 1.4],
  "rear_reception_desk": [-0.36, 20.0],
  "main_conference_room": [18.75, 15.3],
  "main_break_room": [15.5, 5.4]}

'''
Pick up an item in one location and deliver it to another. 
The assumption is that there is a person at the pick and delivery location
to load and unload the item from the robot.
'''
def main():
  # Recieved virtual request for picking item at front_reception_desk and bring to
  # employee at main_conference_room. This request would
  # contain the pick position ("front_reception_desk") and shipping destination ("main_conference_room")
  ####################
  request_item_location = 'front_reception_desk'
  request_destination = 'main_conference_room'
  ####################

  # Start the ROS 2 Python Client Library
  rclpy.init()

  # Launch the ROS 2 Navigation Stack
  navigator = BasicNavigator()

  # Set the robot's initial pose if necessary
  # initial_pose = PoseStamped()
  # initial_pose.header.frame_id = 'map'
  # initial_pose.header.stamp = navigator.get_clock().now().to_msg()
  # initial_pose.pose.position.x = 0.0
  # initial_pose.pose.position.y = 1.0
  # initial_pose.pose.position.z = 0.0
  # initial_pose.pose.orientation.x = 0.0
  # initial_pose.pose.orientation.y = 0.0
  # initial_pose.pose.orientation.z = 0.0
  # initial_pose.pose.orientation.w = 1.0
  # navigator.setInitialPose(initial_pose)

  # Wait for navigation to fully activate
  navigator.waitUntilNav2Active()

  # Set the pick location  
  pick_item_pose = PoseStamped()
  pick_item_pose.header.frame_id = 'map'
  pick_item_pose.header.stamp = navigator.get_clock().now().to_msg()
  pick_item_pose.pose.position.x = pick_positions[request_item_location][0]
  pick_item_pose.pose.position.y = pick_positions[request_item_location][1]
  pick_item_pose.pose.position.z = 0.0
  pick_item_pose.pose.orientation.x = 0.0
  pick_item_pose.pose.orientation.y = 0.0
  pick_item_pose.pose.orientation.z = 0.0
  pick_item_pose.pose.orientation.w = 1.0
  print('Received request for item picking at ' + request_item_location + '.')
  navigator.goToPose(pick_item_pose)

  # Do something during our route
  # (e.g. queue up future tasks or detect person for fine-tuned positioning)
  # Simply print information for employees on the robot's distance remaining
  i = 0
  while not navigator.isNavComplete():
    i = i + 1
    feedback = navigator.getFeedback()
    if feedback and i % 5 == 0:
      print('Distance remaining: ' + '{:.2f}'.format(
            feedback.distance_remaining) + ' meters.')

  result = navigator.getResult()
  if result == NavigationResult.SUCCEEDED:
    print('Got product from ' + request_item_location +
          '! Bringing product to shipping destination (' + request_destination + ')...')
    shipping_destination = PoseStamped()
    shipping_destination.header.frame_id = 'map'
    shipping_destination.header.stamp = navigator.get_clock().now().to_msg()
    shipping_destination.pose.position.x = shipping_destinations[request_destination][0]
    shipping_destination.pose.position.y = shipping_destinations[request_destination][1]
    shipping_destination.pose.position.z = 0.0
    shipping_destination.pose.orientation.x = 0.0
    shipping_destination.pose.orientation.y = 0.0
    shipping_destination.pose.orientation.z = 0.0
    shipping_destination.pose.orientation.w = 1.0
    navigator.goToPose(shipping_destination)

  elif result == NavigationResult.CANCELED:
    print('Task at ' + request_item_location + ' was canceled. Returning to staging point...')
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    navigator.goToPose(initial_pose)

  elif result == NavigationResult.FAILED:
    print('Task at ' + request_item_location + ' failed!')
    exit(-1)

  while not navigator.isNavComplete():
    pass

  exit(0)

if __name__ == '__main__':
  main()

The orientation values are in quaternion format. You can use this calculator to convert from Euler angles (e.g. x = 0 radians, y = 0 radians, z = 1.57 radians) to quaternion format (e.g. x = 0, y, = 0, z = 0.707, w = 0.707).

Save the code and close the file.

Change the access permissions on the file.

chmod +x pick_and_deliver.py

Open a new Python program called robot_navigator.py.

gedit robot_navigator.py

Add this code.

Save the code and close the file.

Change the access permissions on the file.

chmod +x robot_navigator.py

Open CMakeLists.txt.

cd ~/dev_ws/src/two_wheeled_robot

gedit CMakeLists.txt

Add the Python executables.

scripts/robot_navigator.py
scripts/pick_and_deliver.py

Here is my full CMakeLists.txt file.

Add the launch file.

cd ~/dev_ws/src/two_wheeled_robot/launch/office_world

gedit office_world_v1.launch.py

Save and close.

Add the Nav2 parameters.

cd ~/dev_ws/src/two_wheeled_robot/params/office_world

gedit nav2_params.yaml

Save and close.

Now we build the package.

cd ~/dev_ws/

colcon build

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

ros2 launch two_wheeled_robot office_world_v1.launch.py

Now send the robot from the front desk to the main conference room by opening a new terminal window, and typing:

ros2 run two_wheeled_robot pick_and_deliver.py

The robot will go from the front desk to the main conference room.

A success message will print once the robot has reached the conference room.

That’s it! Keep building!