In this tutorial, I will show you how to send goals 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 (Room Service)
- Offices
- Restaurants
- 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 nav_to_pose.py
gedit nav_to_pose.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 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
'''
Navigates a robot from an initial pose to a goal pose.
'''
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 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 = 10.0
goal_pose.pose.position.y = -2.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
# sanity check a valid path exists
# path = navigator.getPath(initial_pose, goal_pose)
# Go to the goal pose
navigator.goToPose(goal_pose)
i = 0
# Keep doing stuff as long as the robot is moving towards the goal
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('Distance remaining: ' + '{:.2f}'.format(
feedback.distance_remaining) + ' meters.')
# Some navigation timeout to demo cancellation
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
navigator.cancelNav()
# Some navigation request change to demo preemption
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=120.0):
goal_pose.pose.position.x = -3.0
navigator.goToPose(goal_pose)
# 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!')
# Shut down the ROS 2 Navigation Stack
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 nav_to_pose.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 what my CMakeLists.txt looks like.
scripts/nav_to_pose.py
scripts/robot_navigator.py
Now we build the package.
cd ~/dev_ws/
colcon build
Open a new terminal and launch the robot in a Gazebo world. Here is my launch file. I chose to use my car_world that has some houses, a fast food restaurant, and a gas station:
ros2 launch two_wheeled_robot car_world_v1.launch.py
Wait for the simulation to load, and then send the goal by opening a new terminal window, and typing:
ros2 run two_wheeled_robot nav_to_pose.py
You will see the distance remaining to the goal 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 pose printed on the screen.
The robot will move to the goal.
A success message will print once the robot has reached the goal.
That’s it! Keep building!
