Example 13
In this example, we will be utilizing the move_base ROS node, a component of the ROS navigation stack to send base goals to the Stretch robot.
Build a map
First, begin by building a map of the space the robot will be navigating in. If you need a refresher on how to do this, then check out the Navigation Stack tutorial.
Getting Started
Next, with your created map, we can navigate the robot around the mapped space. Run:
roslaunch stretch_navigation navigation.launch map_yaml:=${HELLO_FLEET_PATH}/maps/<map_name>.yaml
${HELLO_FLEET_PATH}
is the path of the <map_name>.yaml
file.
IMPORTANT NOTE: It's likely that the robot's location in the map does not match the robot's location in the real space. In the top bar of Rviz, use 2D Pose Estimate to lay an arrow down roughly where the robot is located in the real space. Below is a gif for reference.
Now we are going to use a node to send a a move base goal half a meter in front of the map's origin. run the following command to execute the navigation.py node.
# Terminal 2
cd catkin_ws/src/stretch_tutorials/src/
python navigation.py
The Code
#!/usr/bin/env python
import rospy
import actionlib
import sys
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import Quaternion
from tf import transformations
class StretchNavigation:
"""
A simple encapsulation of the navigation stack for a Stretch robot.
"""
def __init__(self):
"""
Create an instance of the simple navigation interface.
:param self: The self reference.
"""
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
rospy.loginfo('{0}: Made contact with move_base server'.format(self.__class__.__name__))
self.goal = MoveBaseGoal()
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time()
self.goal.target_pose.pose.position.x = 0.0
self.goal.target_pose.pose.position.y = 0.0
self.goal.target_pose.pose.position.z = 0.0
self.goal.target_pose.pose.orientation.x = 0.0
self.goal.target_pose.pose.orientation.y = 0.0
self.goal.target_pose.pose.orientation.z = 0.0
self.goal.target_pose.pose.orientation.w = 1.0
def get_quaternion(self,theta):
"""
A function to build Quaternians from Euler angles. Since the Stretch only
rotates around z, we can zero out the other angles.
:param theta: The angle (radians) the robot makes with the x-axis.
"""
return Quaternion(*transformations.quaternion_from_euler(0.0, 0.0, theta))
def go_to(self, x, y, theta):
"""
Drive the robot to a particular pose on the map. The Stretch only needs
(x, y) coordinates and a heading.
:param x: x coordinate in the map frame.
:param y: y coordinate in the map frame.
:param theta: heading (angle with the x-axis in the map frame)
"""
rospy.loginfo('{0}: Heading for ({1}, {2}) at {3} radians'.format(self.__class__.__name__,
x, y, theta))
self.goal.target_pose.pose.position.x = x
self.goal.target_pose.pose.position.y = y
self.goal.target_pose.pose.orientation = self.get_quaternion(theta)
self.client.send_goal(self.goal, done_cb=self.done_callback)
self.client.wait_for_result()
def done_callback(self, status, result):
"""
The done_callback function will be called when the joint action is complete.
:param self: The self reference.
:param status: status attribute from MoveBaseActionResult message.
:param result: result attribute from MoveBaseActionResult message.
"""
if status == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo('{0}: SUCCEEDED in reaching the goal.'.format(self.__class__.__name__))
else:
rospy.loginfo('{0}: FAILED in reaching the goal.'.format(self.__class__.__name__))
if __name__ == '__main__':
rospy.init_node('navigation', argv=sys.argv)
nav = StretchNavigation()
nav.go_to(0.5, 0.0, 0.0)
The Code Explained
Now let's break the code down.
#!/usr/bin/env python
import rospy
import actionlib
import sys
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import Quaternion
from tf import transformations
rospy
if you are writing a ROS Node.
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
rospy.loginfo('{0}: Made contact with move_base server'.format(self.__class__.__name__))
move_base
, and has type MoveBaseAction
. Once we make the client, we wait for the server to be ready.
self.goal = MoveBaseGoal()
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time()
self.goal.target_pose.pose.position.x = 0.0
self.goal.target_pose.pose.position.y = 0.0
self.goal.target_pose.pose.position.z = 0.0
self.goal.target_pose.pose.orientation.x = 0.0
self.goal.target_pose.pose.orientation.y = 0.0
self.goal.target_pose.pose.orientation.z = 0.0
self.goal.target_pose.pose.orientation.w = 1.0
def get_quaternion(self,theta):
"""
A function to build Quaternians from Euler angles. Since the Stretch only
rotates around z, we can zero out the other angles.
:param theta: The angle (radians) the robot makes with the x-axis.
"""
return Quaternion(*transformations.quaternion_from_euler(0.0, 0.0, theta))
def go_to(self, x, y, theta, wait=False):
"""
Drive the robot to a particular pose on the map. The Stretch only needs
(x, y) coordinates and a heading.
:param x: x coordinate in the map frame.
:param y: y coordinate in the map frame.
:param theta: heading (angle with the x-axis in the map frame)
"""
rospy.loginfo('{0}: Heading for ({1}, {2}) at {3} radians'.format(self.__class__.__name__,
x, y, theta))
go_to()
function takes in the 3 arguments, the x and y coordinates in the map frame, and the heading.
self.goal.target_pose.pose.position.x = x
self.goal.target_pose.pose.position.y = y
self.goal.target_pose.pose.orientation = self.get_quaternion(theta)
MoveBaseGoal()
data structure has three goal positions (in meters), along each of the axes. For Stretch, it will only pay attention to the x and y coordinates, since it can't move in the z direction.
self.client.send_goal(self.goal, done_cb=self.done_callback)
self.client.wait_for_result()
done_callback()
function in one of the arguments in send_goal()
.
def done_callback(self, status, result):
"""
The done_callback function will be called when the joint action is complete.
:param self: The self reference.
:param status: status attribute from MoveBaseActionResult message.
:param result: result attribute from MoveBaseActionResult message.
"""
if status == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo('{0}: SUCCEEDED in reaching the goal.'.format(self.__class__.__name__))
else:
rospy.loginfo('{0}: FAILED in reaching the goal.'.format(self.__class__.__name__))
MoveBaseActionResult
succeeded or failed.
rospy.init_node('navigation', argv=sys.argv)
nav = StretchNavigation()
rospy.init_node(NAME, ...)
, is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Declare the StretchNavigation
object.
nav.go_to(0.5, 0.0, 0.0)