In this tutorial, I will show you how to send waypoints to a mobile robot and 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:
- Ground Delivery
- Hospitals and Medical Centers
- Hotels (e.g. Room Service)
- Offices
- Restaurants (e.g. Delivering Food and Drink From the Kitchen)
- Warehouses
- And more…
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 waypoint_follower.py.
gedit waypoint_follower.py
Add this code. NOTE: In the latest version of ROS 2 (Galactic and newer), you will need to change “NavigationResult” to “TaskResult”.
#! /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 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
'''
Follow waypoints using the ROS 2 Navigation Stack (Nav2)
'''
def main():
# 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 = 0.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)
# Activate navigation, if not autostarted. This should be called after setInitialPose()
# or this will initialize at the origin of the map and update the costmap with bogus readings.
# If autostart, you should `waitUntilNav2Active()` instead.
# navigator.lifecycleStartup()
# Wait for navigation to fully activate. Use this line if autostart is set to true.
navigator.waitUntilNav2Active()
# If desired, you can change or load the map as well
# navigator.changeMap('/path/to/map.yaml')
# You may use the navigator to clear or obtain costmaps
# navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap()
# global_costmap = navigator.getGlobalCostmap()
# local_costmap = navigator.getLocalCostmap()
# Set the robot's goal poses
goal_poses = []
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = 1.3
goal_pose.pose.position.y = 6.0
goal_pose.pose.position.z = 0.0
goal_pose.pose.orientation.x = 0.0
goal_pose.pose.orientation.y = 0.0
goal_pose.pose.orientation.z = 0.23
goal_pose.pose.orientation.w = 0.97
goal_poses.append(goal_pose)
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = 2.0
goal_pose.pose.position.y = -3.5
goal_pose.pose.position.z = 0.0
goal_pose.pose.orientation.x = 0.0
goal_pose.pose.orientation.y = 0.0
goal_pose.pose.orientation.z = 0.707
goal_pose.pose.orientation.w = -0.707
goal_poses.append(goal_pose)
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = 1.5
goal_pose.pose.position.y = -7.7
goal_pose.pose.position.z = 0.0
goal_pose.pose.orientation.x = 0.0
goal_pose.pose.orientation.y = 0.0
goal_pose.pose.orientation.z = 0.92
goal_pose.pose.orientation.w = -0.38
goal_poses.append(goal_pose)
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = -1.4
goal_pose.pose.position.y = -7.8
goal_pose.pose.position.z = 0.0
goal_pose.pose.orientation.x = 0.0
goal_pose.pose.orientation.y = 0.0
goal_pose.pose.orientation.z = 0.92
goal_pose.pose.orientation.w = 0.38
goal_poses.append(goal_pose)
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = -2.6
goal_pose.pose.position.y = -4.5
goal_pose.pose.position.z = 0.0
goal_pose.pose.orientation.x = 0.0
goal_pose.pose.orientation.y = 0.0
goal_pose.pose.orientation.z = 0.38
goal_pose.pose.orientation.w = 0.92
goal_poses.append(goal_pose)
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = 0.0
goal_pose.pose.position.y = 0.0
goal_pose.pose.position.z = 0.0
goal_pose.pose.orientation.x = 0.0
goal_pose.pose.orientation.y = 0.0
goal_pose.pose.orientation.z = 0.0
goal_pose.pose.orientation.w = 1.0
goal_poses.append(goal_pose)
# sanity check a valid path exists
# path = navigator.getPathThroughPoses(initial_pose, goal_poses)
nav_start = navigator.get_clock().now()
navigator.followWaypoints(goal_poses)
i = 0
while not navigator.isNavComplete():
################################################
#
# Implement some code here for your application!
#
################################################
# Do something with the feedback
i = i + 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print('Executing current waypoint: ' +
str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses)))
now = navigator.get_clock().now()
# Some navigation timeout to demo cancellation
if now - nav_start > Duration(seconds=100000000.0):
navigator.cancelNav()
# Some follow waypoints request change to demo preemption
if now - nav_start > Duration(seconds=500000.0):
goal_pose_alt = PoseStamped()
goal_pose_alt.header.frame_id = 'map'
goal_pose_alt.header.stamp = now.to_msg()
goal_pose_alt.pose.position.x = -6.5
goal_pose_alt.pose.position.y = -4.2
goal_pose_alt.pose.position.z = 0.0
goal_pose_alt.pose.orientation.x = 0.0
goal_pose_alt.pose.orientation.y = 0.0
goal_pose_alt.pose.orientation.z = 0.0
goal_pose_alt.pose.orientation.w = 1.0
goal_poses = [goal_pose_alt]
nav_start = now
navigator.followWaypoints(goal_poses)
# Do something depending on the return code
result = navigator.getResult()
if result == NavigationResult.SUCCEEDED:
print('Goal succeeded!')
elif result == NavigationResult.CANCELED:
print('Goal was canceled!')
elif result == NavigationResult.FAILED:
print('Goal failed!')
else:
print('Goal has an invalid return status!')
navigator.lifecycleShutdown()
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 waypoint_follower.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. Here is my full CMakeLists.txt file.
scripts/robot_navigator.py
scripts/waypoint_follower.py
Add the launch file.
cd ~/dev_ws/src/two_wheeled_robot/launch/cafe_world
gedit cafe_world_v1.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/cafe_world/cafe_world.yaml'
nav2_params_path = 'params/cafe_world/nav2_params.yaml'
robot_localization_file_path = 'config/ekf.yaml'
rviz_config_file_path = 'rviz/cafe_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/cafe.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 Nav2 parameters.
cd ~/dev_ws/src/two_wheeled_robot/params/cafe_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. I chose to use my cafe_world that has several tables. A good use case for this robot would be delivering food and drinks from the kitchen to the tables.
ros2 launch two_wheeled_robot cafe_world_v1.launch.py
Now send the robot to the waypoints by opening a new terminal window, and typing:
ros2 run two_wheeled_robot waypoint_follower.py
The robot will follow the waypoints.
A success message will print once the robot has reached the final waypoint.
That’s it! Keep building!
