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.
(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.
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.
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)
(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.
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.
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.
In this tutorial, I will show you how to send a goal path 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 we will develop in this tutorial has a number of real-world use cases.
For example, you can build an autonomous agricultural robot that can do the following jobs:
Precision Weeding
Counting
Fruit Picking
Harvesting
Irrigation
Monitoring
Mowing
Planting
Pruning
Seeding
Spraying
You could also use this application for an autonomous robotic lawn mower.
(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.
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 nav_through_poses.py
Open a new Python program called robot_navigator.py.
Wait for the simulation to fully load. I like to wait three minutes so the pose of the robot can set to the correct location within RViz.
If the global costmap doesn’t load, try launching again.
Now send the goal path by opening a new terminal window, and typing:
ros2 run two_wheeled_robot nav_through_poses.py
You will see the distance remaining to the goal pose printed on the screen. You can also choose to print other information to the screen by getting the appropriate message type.
You will also see the path from the initial pose to the goal poses printed on the screen.
The robot will move through the goal poses.
A success message will print once the robot has reached the final goal pose.
