| Note: This tutorial assumes that you have completed the previous tutorials: building a ROS package. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Understanding the Example Solution
Description: This tutorial walks through the code of the servicesim example solution detailing the ROS interfaces used and the state machine of the competition.Tutorial Level: BEGINNER
Contents
The Code
The code is available on the servicesim bitbucket repository. Here is the full content of the file, we will go through it line by line below.
1 #! /usr/bin/env python
2
3 # Copyright (C) 2018 Open Source Robotics Foundation, Inc.
4 #
5 # Licensed under the Apache License, Version 2.0 (the "License");
6 # you may not use this file except in compliance with the License.
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10 #
11 # Unless required by applicable law or agreed to in writing, software
12 # distributed under the License is distributed on an "AS IS" BASIS,
13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 # See the License for the specific language governing permissions and
15 # limitations under the License.
16
17 import copy
18 import random
19 from enum import Enum
20
21 import actionlib
22
23 from actionlib_msgs.msg import GoalStatus
24
25 from geometry_msgs.msg import Point, Pose, PoseWithCovarianceStamped, Quaternion
26
27 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
28
29 import rospy
30
31 from sensor_msgs.msg import Image
32
33 from servicesim_competition.msg import ActorNames
34 from servicesim_competition.srv import DropOffGuest, DropOffGuestRequest, NewTask
35 from servicesim_competition.srv import PickUpGuest, PickUpGuestRequest, RoomInfo, RoomInfoRequest
36
37 from std_srvs.srv import Empty, EmptyRequest
38
39
40 class CompetitionState(Enum):
41 BeginTask = 0
42 Pickup = 1
43 DropOff = 2
44 ReturnToStart = 3
45
46
47 class ExampleNode(object):
48 def __init__(self):
49 self.robot_name = 'servicebot'
50 self.guest_name = ''
51 self.action_timeout = 10
52 self.actors_in_range = []
53
54 rospy.init_node('example_solution')
55 rospy.loginfo('node created')
56 # create service proxies
57 rospy.loginfo('wait for new task srv')
58 rospy.wait_for_service('/servicesim/new_task')
59 self.new_task_srv = rospy.ServiceProxy('/servicesim/new_task', NewTask)
60 rospy.loginfo('wait for pickup guest srv')
61 rospy.wait_for_service('/servicesim/pickup_guest')
62 self.pickup_guest_srv = rospy.ServiceProxy('/servicesim/pickup_guest', PickUpGuest)
63 rospy.loginfo('wait for drop off srv')
64 rospy.wait_for_service('/servicesim/dropoff_guest')
65 self.dropoff_guest_srv = rospy.ServiceProxy('/servicesim/dropoff_guest', DropOffGuest)
66 rospy.loginfo('wait for room info srv')
67 rospy.wait_for_service('/servicesim/room_info')
68 self.room_info_srv = rospy.ServiceProxy('/servicesim/room_info', RoomInfo)
69 # create a service proxy to clear the navigation costmaps
70 rospy.loginfo('wait for clear_costmaps')
71 rospy.wait_for_service('/servicebot/move_base/clear_costmaps')
72 self.clear_costmaps_srv = rospy.ServiceProxy(
73 '/servicebot/move_base/clear_costmaps', Empty)
74 # create subscribers
75 rospy.loginfo('create subs')
76 rospy.Subscriber('/servicebot/rfid', ActorNames, self.rfid_callback)
77 rospy.Subscriber('/servicebot/camera_front/image_raw', Image, self.image_callback)
78 rospy.loginfo('done creating subs')
79
80 # create publisher
81 # create a publisher to initialize the localization with the starting pose
82 # provided by the competition
83 self.initial_pose_pub = rospy.Publisher(
84 '/servicebot/initialpose', PoseWithCovarianceStamped, latch=True)
85 # create action client
86 self.move_base = actionlib.SimpleActionClient('/servicebot/move_base', MoveBaseAction)
87 self.move_base.wait_for_server(rospy.Duration(self.action_timeout))
88 rospy.loginfo('done waiting for move base action server')
89
90 def rfid_callback(self, msg):
91 # this function will be called periodically with the list of RFID tags
92 # around the robot
93 if msg.actor_names != self.actors_in_range:
94 rospy.loginfo(msg.actor_names)
95 self.actors_in_range = msg.actor_names
96 return True
97
98 def image_callback(self, msg):
99 # placeholder for competitor to add image processing
100 return True
101
102 def request_new_task(self):
103 # Calls the service to start the competition and receive a new task
104 resp = self.new_task_srv()
105 rospy.loginfo(resp)
106 return [
107 resp.pick_up_location, resp.drop_off_location, resp.guest_name, resp.robot_start_pose]
108
109 def request_follow(self):
110 # Calls the service to request a specific guest to follow the robot
111 req = PickUpGuestRequest()
112 req.robot_name = self.robot_name
113 req.guest_name = self.guest_name
114 resp = self.pickup_guest_srv(req)
115 rospy.loginfo(resp)
116 return [resp.success]
117
118 def request_unfollow(self):
119 # Calls the service to request a specific guest to stop followingthe robot
120 req = DropOffGuestRequest()
121 req.guest_name = self.guest_name
122 resp = self.dropoff_guest_srv(req)
123 rospy.loginfo(resp)
124 return [resp.success]
125
126 # returns a target pose from the room name
127 def pose_from_room_name(self, room_name):
128 # Calls the service to get pose infromations based on a room name
129 req = RoomInfoRequest()
130 req.name = room_name
131 resp = self.room_info_srv(req)
132
133 # To be replaced by competitors
134 # here we just return the center of the provided area
135 mid_pose = Pose(
136 Point(
137 (resp.min.x + resp.max.x) / 2., (resp.min.y + resp.max.y) / 2., 0),
138 Quaternion(0, 0, 1.0, 0.0))
139 return mid_pose
140
141 def construct_goal_from_pose(self, pose):
142 # Takes a 6D pose and returns a navigation goal
143 posecopy = copy.deepcopy(pose)
144 goal = MoveBaseGoal()
145 goal.target_pose.pose = posecopy
146 # As we need the guest to be at the target location and not the robot,
147 # we offset every goal of 1 meter
148 goal.target_pose.pose.position.x -= 1
149 goal.target_pose.header.frame_id = 'map'
150 goal.target_pose.header.stamp = rospy.Time.now()
151
152 # add randomness (square of 10cm) around the goal
153 dx = random.randrange(0, 20, 1) - 10
154 dy = random.randrange(0, 20, 1) - 10
155 goal.target_pose.pose.position.x += dx / 100.
156 goal.target_pose.pose.position.y += dy / 100.
157 return goal
158
159 def example_solution(self):
160 # This handles the competition logic
161 # A state maching that gives goals to the robot and calls the competition services
162 # to complete the checkpoints
163 state = CompetitionState.BeginTask
164
165 rate = rospy.Rate(1)
166
167 while not rospy.is_shutdown():
168 if state == CompetitionState.BeginTask:
169 rospy.loginfo('In BeginTask state')
170 # Request a new task
171 [pick_up_room, drop_off_room, self.guest_name, self.start_pose] = \
172 self.request_new_task()
173 self.start_pose.position.z = 0
174 # Initialize the localization with the provided starting position
175 init_pose_msg = PoseWithCovarianceStamped()
176 init_pose_msg.header.frame_id = 'map'
177 init_pose_msg.pose.pose = self.start_pose
178 self.initial_pose_pub.publish(init_pose_msg)
179 rospy.loginfo('published initial pose')
180 rospy.sleep(0.5)
181
182 # Switch to the Pickup state
183 state = CompetitionState.Pickup
184
185 elif state == CompetitionState.Pickup:
186 # Go to pick up point
187 rospy.loginfo('In Pickup state')
188 # Cancel previous navigation goals
189 self.move_base.cancel_all_goals()
190 # Clears the obstacle maps
191 rospy.loginfo('clearing costmaps')
192 self.clear_costmaps_srv(EmptyRequest())
193 rospy.sleep(0.5)
194 pickup_goal = self.construct_goal_from_pose(self.pose_from_room_name(pick_up_room))
195 # Ask the robot to go to the pickup location by sending it a navigation goal
196 self.move_base.send_goal(pickup_goal)
197 rospy.loginfo('sent goal')
198
199 self.move_base.wait_for_result(rospy.Duration(self.action_timeout))
200 rospy.loginfo('action timed out in state: %s' % self.move_base.get_state())
201 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
202 # If the robot succeeded to reach the pickup point
203 # check that the guest is in range
204 if self.guest_name in self.actors_in_range:
205 rospy.loginfo('requesting unfollow')
206 # if the guest is in range, ask the guest to follow the robot
207 if self.request_follow():
208 # the guest is following the robot
209 # Switch to the DropOff state
210 state = CompetitionState.DropOff
211 else:
212 # pickup point reached but the guest is not in range
213 # add custom room exploration logic here
214 rospy.logwarn('robot reached pickup point but guest is not in range!')
215 else:
216 rospy.loginfo('action timed out in state: %s' % self.move_base.get_state())
217
218 elif state == CompetitionState.DropOff:
219 # Go to drop off point
220 rospy.loginfo('In DropOff state')
221 # Cancel previous navigation goals
222 self.move_base.cancel_all_goals()
223 # Clears the obstacle maps
224 rospy.loginfo('clearing costmaps')
225 self.clear_costmaps_srv(EmptyRequest())
226 rospy.sleep(0.5)
227 # Ask the robot to go to the drop off location by sending it a navigation goal
228 dropoff_goal = self.construct_goal_from_pose(
229 self.pose_from_room_name(drop_off_room))
230 self.move_base.send_goal(dropoff_goal)
231 self.move_base.wait_for_result(rospy.Duration(self.action_timeout))
232 # If the robot succeeded to reach the dropoff point
233 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
234 # ask the guest to stop following the robot
235 self.request_unfollow()
236 state = CompetitionState.ReturnToStart
237 else:
238 rospy.loginfo('action timed out in state: %s' % self.move_base.get_state())
239
240 elif state == CompetitionState.ReturnToStart:
241 # Go to drop off point
242 rospy.loginfo('In ReturnToStart state')
243 # Cancel previous navigation goals
244 self.move_base.cancel_all_goals()
245 # Clears the obstacle maps
246 rospy.loginfo('clearing costmaps')
247 self.clear_costmaps_srv(EmptyRequest())
248 rospy.sleep(0.5)
249 # Ask the robot to return to the starting position
250 startgoal = self.construct_goal_from_pose(self.start_pose)
251 self.move_base.send_goal(startgoal)
252 self.move_base.wait_for_result(rospy.Duration(self.action_timeout))
253 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
254 # If the robot succeeded to reach the starting point
255 # the competition is over
256 rospy.loginfo('Hooray!')
257 rospy.signal_shutdown('Competition completed!')
258 else:
259 rospy.loginfo('action timed out in state: %s' % self.move_base.get_state())
260
261 rate.sleep()
262
263
264 if __name__ == '__main__':
265 node = ExampleNode()
266 try:
267 node.example_solution()
268 except rospy.ROSInterruptException:
269 pass
The Code Explained
Now, let's break the code down.
1 #! /usr/bin/env python
Every Python ROS Node will have this declaration at the top. The first line makes sure your script is executed as a Python script.
Importing the python modules
We will need these python builtin modules to perform random number generation in our script
19 from enum import Enum
This class will allow us to declare an enumeration of the states of our state machine
37 from std_srvs.srv import Empty, EmptyRequest
Import the ROS message and service types that we will use for our application
29 import rospy
Import rospy is necessary to create a python ROS Node
Import the servicesim_competition service and message type to be able to interact with the competition logic
Defining the comeptition's state machine states
Here we declare the states we will use in our state machine. These states match the Checkpoints of the competition.
The competition example class
47 class ExampleNode(object):
Define the class where we will implement our ROS Node
Initialisation of the class
48 def __init__(self):
In this function we will create all the variables and ROS interfaces we need to complete the competition
We initialize the robot_name to its value in the competition. action_timeout will be used to decide when to resend a goal to the navigation algorithm and p[lan a new path towards that goal. Here we set it to 10 seconds.
54 rospy.init_node('example_solution')
The next line, 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. In this case, your node will take on the name "example_solution". NOTE: the name must be a base name, i.e. it cannot contain any slashes "/"
58 rospy.wait_for_service('/servicesim/new_task')
59 self.new_task_srv = rospy.ServiceProxy('/servicesim/new_task', NewTask)
60 rospy.loginfo('wait for pickup guest srv')
61 rospy.wait_for_service('/servicesim/pickup_guest')
62 self.pickup_guest_srv = rospy.ServiceProxy('/servicesim/pickup_guest', PickUpGuest)
63 rospy.loginfo('wait for drop off srv')
64 rospy.wait_for_service('/servicesim/dropoff_guest')
65 self.dropoff_guest_srv = rospy.ServiceProxy('/servicesim/dropoff_guest', DropOffGuest)
66 rospy.loginfo('wait for room info srv')
67 rospy.wait_for_service('/servicesim/room_info')
68 self.room_info_srv = rospy.ServiceProxy('/servicesim/room_info', RoomInfo)
Here we make sure that all the service for the competition are available and we create Service Proxies to be able to use them later in the competition
This service will allow us to remove clear the obstacle map built by the robot. This will be helpful when the robot is stuck and cannot find a path to its destination
We subscribe to the sensor data from the robot we want to use. Note that the last argument provided is a function name. That callback function will be used everytime a message is received on that topic
81 # create a publisher to initialize the localization with the starting pose
82 # provided by the competition
83 self.initial_pose_pub = rospy.Publisher(
84 '/servicebot/initialpose', PoseWithCovarianceStamped, latch=True)
85 # create action client
86 self.move_base = actionlib.SimpleActionClient('/servicebot/move_base', MoveBaseAction)
87 self.move_base.wait_for_server(rospy.Duration(self.action_timeout))
Create the interfaces that will allow us to interact with the localization and the navigation algorithms of the robot
Methods of the class
This function will be used to process the RFID sensor data. This will receive messages of type ActorNames that is a list of all the actor names in range of the robot's sensor Everytime we receive the list we store it in the actor_in_range variable to be able to reuse it later. This callback also prints the list to the console if the list has changed.
This function will be used to process the robot's camera images. This is currently empty and should be where users can add their own image processing logic for the competition.
102 def request_new_task(self):
103 # Calls the service to start the competition and receive a new task
104 resp = self.new_task_srv()
105 rospy.loginfo(resp)
106 return [
107 resp.pick_up_location, resp.drop_off_location, resp.guest_name, resp.robot_start_pose]
108
109 def request_follow(self):
110 # Calls the service to request a specific guest to follow the robot
111 req = PickUpGuestRequest()
112 req.robot_name = self.robot_name
113 req.guest_name = self.guest_name
114 resp = self.pickup_guest_srv(req)
115 rospy.loginfo(resp)
116 return [resp.success]
117
118 def request_unfollow(self):
119 # Calls the service to request a specific guest to stop followingthe robot
120 req = DropOffGuestRequest()
121 req.guest_name = self.guest_name
122 resp = self.dropoff_guest_srv(req)
123 rospy.loginfo(resp)
124 return [resp.success]
These functions are used to call the competition services allowing to validate checkpoints.
127 def pose_from_room_name(self, room_name):
128 # Calls the service to get pose infromations based on a room name
129 req = RoomInfoRequest()
130 req.name = room_name
131 resp = self.room_info_srv(req)
132
133 # To be replaced by competitors
134 # here we just return the center of the provided area
135 mid_pose = Pose(
136 Point(
137 (resp.min.x + resp.max.x) / 2., (resp.min.y + resp.max.y) / 2., 0),
138 Quaternion(0, 0, 1.0, 0.0))
139 return mid_pose
As the task given by the competition are providing us room names, we need to convert these to cartesian locations in the world for the robot to navigate, This function requests the room information by calling the room_info service and than creates a Pose pointing to the center of the provided area. This is a place where competitors can add their own logic based on their knowledge of the simulation environment layout (for example to make sure this pose is not occupied by a known static obstacle).
141 def construct_goal_from_pose(self, pose):
142 # Takes a 6D pose and returns a navigation goal
143 posecopy = copy.deepcopy(pose)
144 goal = MoveBaseGoal()
145 goal.target_pose.pose = posecopy
146 # As we need the guest to be at the target location and not the robot,
147 # we offset every goal of 1 meter
148 goal.target_pose.pose.position.x -= 1
149 goal.target_pose.header.frame_id = 'map'
150 goal.target_pose.header.stamp = rospy.Time.now()
151
152 # add randomness (square of 10cm) around the goal
153 dx = random.randrange(0, 20, 1) - 10
154 dy = random.randrange(0, 20, 1) - 10
155 goal.target_pose.pose.position.x += dx / 100.
156 goal.target_pose.pose.position.y += dy / 100.
157 return goal
This function will create a navigation goal from a Pose. Competitors can add their own logic on how to modify a the provided pose to make a compelling navigation goal. Currently this function offsets the pose of 1 meter, this is to make sure that the guess following the robot will reach the destination area. It also adds some randomness (up to 10 centimeter in every direction), this is to make sur the planner will try to find a new path to the destination and should prevent the robot from getting stuck.
Main function
159 def example_solution(self):
Now that we have all the functions we need to perform the competition, it is time to go through our main function containing all the competition logic. This function will be a state machine going through all states of the competition. It will loop every second and update the states if necessary. The competition starts in the state BeginTask.
BeginTask state
168 if state == CompetitionState.BeginTask:
169 rospy.loginfo('In BeginTask state')
170 # Request a new task
171 [pick_up_room, drop_off_room, self.guest_name, self.start_pose] = \
172 self.request_new_task()
173 self.start_pose.position.z = 0
174 # Initialize the localization with the provided starting position
175 init_pose_msg = PoseWithCovarianceStamped()
176 init_pose_msg.header.frame_id = 'map'
177 init_pose_msg.pose.pose = self.start_pose
178 self.initial_pose_pub.publish(init_pose_msg)
179 rospy.loginfo('published initial pose')
180 rospy.sleep(0.5)
181
182 # Switch to the Pickup state
183 state = CompetitionState.Pickup
In the BeginTask state we start the competition by requesting a new task self.request_new_task(). Then we initialize the localisation of the robot with the starting position provided by the task Finally we trigger the transition to go to the Pickup state.
PickUp state
185 elif state == CompetitionState.Pickup:
In the Pickup state we will ask to robot to go to the PickUp location.
We first make sure that the robot is not moving by cancelling all the goals and clear the obstacle map.
194 pickup_goal = self.construct_goal_from_pose(self.pose_from_room_name(pick_up_room))
195 # Ask the robot to go to the pickup location by sending it a navigation goal
196 self.move_base.send_goal(pickup_goal)
197 rospy.loginfo('sent goal')
198
199 self.move_base.wait_for_result(rospy.Duration(self.action_timeout))
Then we construct a navigation goal and send it to the navigation algorithm. We wait up to 10 seconds to let the robot perform the motion.
201 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
Once the action is over, or 10 seconds have passed, we check if the robot reached the destination
If it succeeds we make sure that the guest is in range of the RFID sensor of the robot.
If that's the case we request the guest to follow the robot and switch to the DropOff state.
If the guest is not in range of the RFID sensor, the competitors should add their own logic to explore the surrounding and identify the guest.
DropOff state
Like before we start by cancelling all goals and clear the obstacle maps.
We construct a navigation goal for the drop off location and send it to the navigation algorithm
If the robot succeeds to reach the goal we notify the guest that the destination has been reached by calling self.request_unfollow. Otherwise we try againg with a slightly different goal. Here competitors can add their own logic to pick a new goal to make sure the guest reaches the drop off area. If we successfully dropped off the guest to destination we switch to the ReturnToStart state
ReturnToStart state
240 elif state == CompetitionState.ReturnToStart:
241 # Go to drop off point
242 rospy.loginfo('In ReturnToStart state')
243 # Cancel previous navigation goals
244 self.move_base.cancel_all_goals()
245 # Clears the obstacle maps
246 rospy.loginfo('clearing costmaps')
247 self.clear_costmaps_srv(EmptyRequest())
248 rospy.sleep(0.5)
249 # Ask the robot to return to the starting position
250 startgoal = self.construct_goal_from_pose(self.start_pose)
251 self.move_base.send_goal(startgoal)
252 self.move_base.wait_for_result(rospy.Duration(self.action_timeout))
253 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
254 # If the robot succeeded to reach the starting point
255 # the competition is over
256 rospy.loginfo('Hooray!')
257 rospy.signal_shutdown('Competition completed!')
258 else:
259 rospy.loginfo('action timed out in state: %s' % self.move_base.get_state())
Similarly to the previous state, we clear the maps and ask the robot to navigate to its' starting position. If the robot succeeds the node exits by calling rospy.signal_shutdown(), otherwise we try again with a slightly different goal. Competitors can add their own logic to ensure the robot reaches the starting position.
