Autonomous navigation with PAL’s robots#
🏁 Goal of this tutorial
By the end of this tutorial, you will know how to make PAL’s robots navigate from point A to B through a Python script.
You can send PAL’s robots from one point to another either through a GUI such as rviz or PAL’s robots’s Web User Interface, or via a programming interface.
Pre-requisites#
You should first have completed Laser-based SLAM and path planning on the robot.
You must have already done the mapping process, i.e. the robot has a map and it is properfly localized.
Note
Before continuing with the instructions of this section make sure that the
robot computer is able to resolve the development computer hostname. Set
the right ROS_MASTER_URI
and ROS_IP
environment variables in the
development computer.
Also, do not forget to undock your robot before running the code. Otherwiswe the robot will not move.
There are four ways to make your robot navigate in a map:
Using a graphical interface, like rviz
Using absolute metric coordinates: the robot will navigate to an absolute coordinate in the map.
Using POI (point of interest): the robot will navigate to a predefinied POI in the map.
Using waypoints: the robot will follow a pre-fixed sequence of points of interest.
Let’s have a look at the three options.
Navigating with rviz#
From rviz
, click on the 2D Nav Goal of the upper tool-bar, and
select the target location in the map that the robot has to reach while you drag
towards the desired orientation.
This will send a navigation goal to move_base
, which will plan and execute
the goal, as illustrated in the sequence of snapshots.
In the event that the robot does not move, make sure that the ROS_IP
variable
has been set with the right IP address of the development computer.
Navigation goals can be sent programmatically to the Action Server /move_base_simple/goal.
Navigating with metric coordinates#
In this code, the robot will move to a given coordinate in the map.
Example of navigation with explicit coordinates#
#!/usr/bin/env python
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
def nav_move_base():
move_client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
move_client.wait_for_server()
rospy.loginfo("Move base client ready")
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.5
goal.target_pose.pose.orientation.w = 1.0
rospy.loginfo("Sending goal")
move_client.send_goal(goal)
wait = move_client.wait_for_result()
return move_client.get_result()
if __name__ == '__main__':
try:
rospy.init_node('nav_move_base_client')
result = nav_move_base()
if result:
rospy.loginfo("Arrived at target!")
except rospy.ROSInterruptException:
rospy.loginfo("Finished.")
Explicit coordinates - code explained#
#!/usr/bin/env python
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
def nav_move_base():
move_client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
move_client.wait_for_server()
rospy.loginfo("Move base client ready")
In this example, we are using the move_base
action, which moves the robot to
a specific metric coordinate in the map.
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.5
goal.target_pose.pose.orientation.w = 1.0
We set the goal to 0.5 metres along the x
axis of the map without rotating
the robot base.
rospy.loginfo("Sending goal")
move_client.send_goal(goal)
wait = move_client.wait_for_result()
return move_client.get_result()
We send the goal and wait for a result.
Tip: if you want to get map coordinates from the map, you can do as follows:
Select the Publish Point option in the
rviz
top menu:
Move the mouse around the map and you’ll see the map coordinates at the bottom bar of
rviz
.
Navigating to a POI#
You can use Point of Interest (POI) to send the robot to labelled existing points in the map.
Note
The following code assumes we have already set POI called P1
.
Check Defining Points of Interest to learn how to define POIs.
Example of POI navigation#
#!/usr/bin/env python
import rospy
import actionlib
from pal_navigation_msgs.msg import GoToPOIAction, GoToPOIGoal
def nav_go_to_poi():
# Connect to the poi navigation action server
move_client = actionlib.SimpleActionClient(
'/poi_navigation_server/go_to_poi',
GoToPOIAction)
move_client.wait_for_server()
rospy.loginfo("POI navigation client ready")
goal = GoToPOIGoal()
goal.poi.data = "P1"
rospy.loginfo("Sending goal")
move_client.send_goal(goal)
wait = move_client.wait_for_result()
return move_client.get_result()
if __name__ == '__main__':
try:
rospy.init_node('nav_go_to_poi_client')
result = nav_go_to_poi()
if result:
rospy.loginfo("Arrived at target!")
except rospy.ROSInterruptException:
rospy.loginfo("Finished.")
POI navigation - code explained#
#!/usr/bin/env python
import rospy
import actionlib
from pal_navigation_msgs.msg import GoToPOIAction, GoToPOIGoal
def nav_go_to_poi():
# Connect to the poi navigation action server
move_client = actionlib.SimpleActionClient(
'/poi_navigation_server/go_to_poi',
GoToPOIAction)
move_client.wait_for_server()
rospy.loginfo("POI navigation client ready")
This time we use the /poi_navigation_server/go_to_poi
action to achieve our
goal.
goal = GoToPOIGoal()
goal.poi.data = "P1"
rospy.loginfo("Sending goal")
move_client.send_goal(goal)
wait = move_client.wait_for_result()
return move_client.get_result()
We then set the goal, in this case, P1
, send the goal and wait for a result.
Navigating through a sequence of POIs#
It might be the case that you want the robot to follow a sequence of points in
the map. If so, you can use waypoints. The sequence of points is defined as a
Group
of points, which has to be previously defined.
Note
The following code assumes we have already set a group called
sequence_points
, which is composed of points P1
and P2
. Check
Defining Points of Interest to learn how to define POIs and groups of points.
Example of navigation through multiple POIs#
#!/usr/bin/env python
import rospy
import actionlib
from pal_waypoint_msgs.msg import DoWaypointNavigationAction, DoWaypointNavigationGoal
def nav_waypoint():
# Connect to the poi navigation action server
move_client = actionlib.SimpleActionClient(
'/pal_waypoint/navigate',
DoWaypointNavigationAction)
move_client.wait_for_server()
rospy.loginfo("Waypoint navigation client ready")
goal = DoWaypointNavigationGoal()
goal.group = "sequence_points"
rospy.loginfo("Sending goal")
move_client.send_goal(goal)
wait = move_client.wait_for_result()
return move_client.get_result()
if __name__ == '__main__':
try:
rospy.init_node('nav_waypoint_client')
result = nav_waypoint()
if result:
rospy.loginfo("Arrived at target!")
except rospy.ROSInterruptException:
rospy.loginfo("Finished.")
Multiple POIs - code explained#
#!/usr/bin/env python
import rospy
import actionlib
from pal_waypoint_msgs.msg import DoWaypointNavigationAction, DoWaypointNavigationGoal
def nav_waypoint():
# Connect to the poi navigation action server
move_client = actionlib.SimpleActionClient(
'/pal_waypoint/navigate',
DoWaypointNavigationAction)
move_client.wait_for_server()
rospy.loginfo("Waypoint navigation client ready")
Now we use the /pal_waypoint/navigate
action to move the robot along a
set of points.
goal = DoWaypointNavigationGoal() goal.group = "sequence_points" rospy.loginfo("Sending goal") move_client.send_goal(goal) wait = move_client.wait_for_result() return move_client.get_result()
We then set the goal, in this case, the group name sequence_points
, send the
goal and wait for a result.