Tutorial 5: Move AGVs to Stations
Tutorial 5
Prerequisites: Introduction to Tutorials and Tutorial 4
Source Code: https://github.com/jaybrecht/ariac_tutorials/tree/tutorial_5
Switch Branch:
cd ~/ariac_ws/src/ariac_tutorials git switch tutorial_5
This tutorial shows how to move AGVs to assembly stations.
Parse received orders to identify assembly tasks,
Identify the AGVs that need to be moved
Identify the assembly station where the AGVs need to be moved,
Lock the trays to prevent parts from moving during transport,
Move the AGVs to the assembly station.
Package Structure
Updates and additions that are specific to Tutorial 5 are highlighted in the tree below.
Updated/Created Files
Competition Interface
The competition interface for Tutorial 5 is shown in Listing 45.
1import rclpy
2from rclpy.node import Node
3from rclpy.qos import qos_profile_sensor_data
4from rclpy.parameter import Parameter
5
6from ariac_msgs.msg import (
7 CompetitionState as CompetitionStateMsg,
8 BreakBeamStatus as BreakBeamStatusMsg,
9 AdvancedLogicalCameraImage as AdvancedLogicalCameraImageMsg,
10 Part as PartMsg,
11 PartPose as PartPoseMsg,
12 Order as OrderMsg,
13 AssemblyPart as AssemblyPartMsg,
14 AGVStatus as AGVStatusMsg,
15 AssemblyTask as AssemblyTaskMsg,
16)
17
18from ariac_msgs.srv import (
19 MoveAGV)
20
21from std_srvs.srv import Trigger
22
23from ariac_tutorials.utils import (
24 multiply_pose,
25 rpy_from_quaternion,
26 rad_to_deg_str,
27 AdvancedLogicalCameraImage,
28 Order,
29 KittingTask,
30 CombinedTask,
31 AssemblyTask,
32 KittingPart,
33)
34
35class CompetitionInterface(Node):
36 '''
37 Class for a competition interface node.
38
39 Args:
40 Node (rclpy.node.Node): Parent class for ROS nodes
41
42 Raises:
43 KeyboardInterrupt: Exception raised when the user uses Ctrl+C to kill a process
44 '''
45 _competition_states = {
46 CompetitionStateMsg.IDLE: 'idle',
47 CompetitionStateMsg.READY: 'ready',
48 CompetitionStateMsg.STARTED: 'started',
49 CompetitionStateMsg.ORDER_ANNOUNCEMENTS_DONE: 'order_announcements_done',
50 CompetitionStateMsg.ENDED: 'ended',
51 }
52 '''Dictionary for converting CompetitionState constants to strings'''
53
54 _part_colors = {
55 PartMsg.RED: 'red',
56 PartMsg.BLUE: 'blue',
57 PartMsg.GREEN: 'green',
58 PartMsg.ORANGE: 'orange',
59 PartMsg.PURPLE: 'purple',
60 }
61 '''Dictionary for converting Part color constants to strings'''
62
63 _part_colors_emoji = {
64 PartMsg.RED: '🟥',
65 PartMsg.BLUE: '🟦',
66 PartMsg.GREEN: '🟩',
67 PartMsg.ORANGE: '🟧',
68 PartMsg.PURPLE: '🟪',
69 }
70 '''Dictionary for converting Part color constants to emojis'''
71
72 _part_types = {
73 PartMsg.BATTERY: 'battery',
74 PartMsg.PUMP: 'pump',
75 PartMsg.REGULATOR: 'regulator',
76 PartMsg.SENSOR: 'sensor',
77 }
78 '''Dictionary for converting Part type constants to strings'''
79
80 _destinations = {
81 AGVStatusMsg.KITTING: 'kitting station',
82 AGVStatusMsg.ASSEMBLY_FRONT: 'front assembly station',
83 AGVStatusMsg.ASSEMBLY_BACK: 'back assembly station',
84 AGVStatusMsg.WAREHOUSE: 'warehouse',
85 }
86 '''Dictionary for converting AGVDestination constants to strings'''
87
88 _stations = {
89 AssemblyTaskMsg.AS1: 'assembly station 1',
90 AssemblyTaskMsg.AS2: 'assembly station 2',
91 AssemblyTaskMsg.AS3: 'assembly station 3',
92 AssemblyTaskMsg.AS4: 'assembly station 4',
93 }
94 '''Dictionary for converting AssemblyTask constants to strings'''
95
96 def __init__(self):
97 super().__init__('competition_interface')
98
99 sim_time = Parameter(
100 "use_sim_time",
101 rclpy.Parameter.Type.BOOL,
102 True
103 )
104
105 self.set_parameters([sim_time])
106
107 # Service client for starting the competition
108 self._start_competition_client = self.create_client(Trigger, '/ariac/start_competition')
109
110 # Subscriber to the competition state topic
111 self._competition_state_sub = self.create_subscription(
112 CompetitionStateMsg,
113 '/ariac/competition_state',
114 self._competition_state_cb,
115 10)
116
117 # Store the state of the competition
118 self._competition_state: CompetitionStateMsg = None
119
120 # Subscriber to the break beam status topic
121 self._break_beam0_sub = self.create_subscription(
122 BreakBeamStatusMsg,
123 '/ariac/sensors/breakbeam_0/status',
124 self._breakbeam0_cb,
125 qos_profile_sensor_data)
126
127 # Store the number of parts that crossed the beam
128 self._conveyor_part_count = 0
129
130 # Store whether the beam is broken
131 self._object_detected = False
132
133 # Subscriber to the logical camera topic
134 self._advanced_camera0_sub = self.create_subscription(
135 AdvancedLogicalCameraImageMsg,
136 '/ariac/sensors/advanced_camera_0/image',
137 self._advanced_camera0_cb,
138 qos_profile_sensor_data)
139
140 # Store each camera image as an AdvancedLogicalCameraImage object
141 self._camera_image: AdvancedLogicalCameraImage = None
142
143 # Subscriber to the order topic
144 self.orders_sub = self.create_subscription(
145 OrderMsg,
146 '/ariac/orders',
147 self._orders_cb,
148 10)
149
150 # Flag for parsing incoming orders
151 self._parse_incoming_order = False
152
153 # List of orders
154 self._orders = []
155
156 @property
157 def orders(self):
158 return self._orders
159
160 @property
161 def camera_image(self):
162 return self._camera_image
163
164 @property
165 def conveyor_part_count(self):
166 return self._conveyor_part_count
167
168 @property
169 def parse_incoming_order(self):
170 return self._parse_incoming_order
171
172 @parse_incoming_order.setter
173 def parse_incoming_order(self, value):
174 self._parse_incoming_order = value
175
176 def _orders_cb(self, msg: Order):
177 '''Callback for the topic /ariac/orders
178 Arguments:
179 msg -- Order message
180 '''
181 order = Order(msg)
182 self._orders.append(order)
183 if self._parse_incoming_order:
184 self.get_logger().info(self._parse_order(order))
185
186 def _advanced_camera0_cb(self, msg: AdvancedLogicalCameraImageMsg):
187 '''Callback for the topic /ariac/sensors/advanced_camera_0/image
188
189 Arguments:
190 msg -- AdvancedLogicalCameraImage message
191 '''
192 self._camera_image = AdvancedLogicalCameraImage(msg.part_poses,
193 msg.tray_poses,
194 msg.sensor_pose)
195
196 def _breakbeam0_cb(self, msg: BreakBeamStatusMsg):
197 '''Callback for the topic /ariac/sensors/breakbeam_0/status
198
199 Arguments:
200 msg -- BreakBeamStatusMsg message
201 '''
202 if not self._object_detected and msg.object_detected:
203 self._conveyor_part_count += 1
204
205 self._object_detected = msg.object_detected
206
207 def _competition_state_cb(self, msg: CompetitionStateMsg):
208 '''Callback for the topic /ariac/competition_state
209 Arguments:
210 msg -- CompetitionState message
211 '''
212 # Log if competition state has changed
213 if self._competition_state != msg.competition_state:
214 state = CompetitionInterface._competition_states[msg.competition_state]
215 self.get_logger().info(f'Competition state is: {state}', throttle_duration_sec=1.0)
216
217 self._competition_state = msg.competition_state
218
219 def start_competition(self):
220 '''Function to start the competition.
221 '''
222 self.get_logger().info('Waiting for competition to be ready')
223
224 if self._competition_state == CompetitionStateMsg.STARTED:
225 return
226 # Wait for competition to be ready
227 while self._competition_state != CompetitionStateMsg.READY:
228 try:
229 rclpy.spin_once(self)
230 except KeyboardInterrupt:
231 return
232
233 self.get_logger().info('Competition is ready. Starting...')
234
235 # Check if service is available
236 if not self._start_competition_client.wait_for_service(timeout_sec=3.0):
237 self.get_logger().error('Service \'/ariac/start_competition\' is not available.')
238 return
239
240 # Create trigger request and call starter service
241 request = Trigger.Request()
242 future = self._start_competition_client.call_async(request)
243
244 # Wait until the service call is completed
245 rclpy.spin_until_future_complete(self, future)
246
247 if future.result().success:
248 self.get_logger().info('Started competition.')
249 else:
250 self.get_logger().warn('Unable to start competition')
251
252 def parse_advanced_camera_image(self, image: AdvancedLogicalCameraImage) -> str:
253 '''
254 Parse an AdvancedLogicalCameraImage message and return a string representation.
255 '''
256
257 if len(image._part_poses) == 0:
258 return 'No parts detected'
259
260 output = '\n\n'
261 for i, part_pose in enumerate(image._part_poses):
262 part_pose: PartPoseMsg
263 output += '==========================\n'
264 part_color = CompetitionInterface._part_colors[part_pose.part.color].capitalize()
265 part_color_emoji = CompetitionInterface._part_colors_emoji[part_pose.part.color]
266 part_type = CompetitionInterface._part_types[part_pose.part.type].capitalize()
267 output += f'Part {i+1}: {part_color_emoji} {part_color} {part_type}\n'
268 output += '--------------------------\n'
269 output += 'Camera Frame\n'
270 output += '--------------------------\n'
271
272 output += ' Position:\n'
273 output += f' x: {part_pose.pose.position.x:.3f} (m)\n'
274 output += f' y: {part_pose.pose.position.y:.3f} (m)\n'
275 output += f' z: {part_pose.pose.position.z:.3f} (m)\n'
276
277 roll, pitch, yaw = rpy_from_quaternion(part_pose.pose.orientation)
278 output += ' Orientation:\n'
279 output += f' roll: {rad_to_deg_str(roll)}\n'
280 output += f' pitch: {rad_to_deg_str(pitch)}\n'
281 output += f' yaw: {rad_to_deg_str(yaw)}\n'
282
283 part_world_pose = multiply_pose(image._sensor_pose, part_pose.pose)
284 output += '--------------------------\n'
285 output += 'World Frame\n'
286 output += '--------------------------\n'
287
288 output += ' Position:\n'
289 output += f' x: {part_world_pose.position.x:.3f} (m)\n'
290 output += f' y: {part_world_pose.position.y:.3f} (m)\n'
291 output += f' z: {part_world_pose.position.z:.3f} (m)\n'
292
293 roll, pitch, yaw = rpy_from_quaternion(part_world_pose.orientation)
294 output += ' Orientation:\n'
295 output += f' roll: {rad_to_deg_str(roll)}\n'
296 output += f' pitch: {rad_to_deg_str(pitch)}\n'
297 output += f' yaw: {rad_to_deg_str(yaw)}\n'
298
299 output += '==========================\n\n'
300
301 return output
302
303 def _parse_kitting_task(self, kitting_task: KittingTask):
304 '''
305 Parses a KittingTask object and returns a string representation.
306
307 Args:
308 kitting_task (KittingTask): KittingTask object to parse
309
310 Returns:
311 str: String representation of the KittingTask object
312 '''
313 output = 'Type: Kitting\n'
314 output += '==========================\n'
315 output += f'AGV: {kitting_task.agv_number}\n'
316 output += f'Destination: {CompetitionInterface._destinations[kitting_task.destination]}\n'
317 output += f'Tray ID: {kitting_task.tray_id}\n'
318 output += 'Products:\n'
319 output += '==========================\n'
320
321 quadrants = {1: "Quadrant 1: -",
322 2: "Quadrant 2: -",
323 3: "Quadrant 3: -",
324 4: "Quadrant 4: -"}
325
326 for i in range(1, 5):
327 product: KittingPart
328 for product in kitting_task.parts:
329 if i == product.quadrant:
330 part_color = CompetitionInterface._part_colors[product.part.color].capitalize()
331 part_color_emoji = CompetitionInterface._part_colors_emoji[product.part.color]
332 part_type = CompetitionInterface._part_types[product.part.type].capitalize()
333 quadrants[i] = f'Quadrant {i}: {part_color_emoji} {part_color} {part_type}'
334 output += f'\t{quadrants[1]}\n'
335 output += f'\t{quadrants[2]}\n'
336 output += f'\t{quadrants[3]}\n'
337 output += f'\t{quadrants[4]}\n'
338
339 return output
340
341 def _parse_assembly_task(self, assembly_task: AssemblyTask):
342 '''
343 Parses an AssemblyTask object and returns a string representation.
344
345 Args:
346 assembly_task (AssemblyTask): AssemblyTask object to parse
347
348 Returns:
349 str: String representation of the AssemblyTask object
350 '''
351 output = 'Type: Assembly\n'
352 output += '==========================\n'
353 if len(assembly_task.agv_numbers) == 1:
354 output += f'AGV: {assembly_task.agv_number[0]}\n'
355 elif len(assembly_task.agv_numbers) == 2:
356 output += f'AGV(s): [{assembly_task.agv_numbers[0]}, {assembly_task.agv_numbers[1]}]\n'
357 output += f'Station: {self._stations[assembly_task.station].title()}\n'
358 output += 'Products:\n'
359 output += '==========================\n'
360
361 product: AssemblyPartMsg
362 for product in assembly_task.parts:
363 part_color = CompetitionInterface._part_colors[product.part.color].capitalize()
364 part_color_emoji = CompetitionInterface._part_colors_emoji[product.part.color]
365 part_type = CompetitionInterface._part_types[product.part.type].capitalize()
366
367 output += f'Part: {part_color_emoji} {part_color} {part_type}\n'
368
369 output += ' Position:\n'
370 output += f' x: {product.assembled_pose.pose.position.x:.3f} (m)\n'
371 output += f' y: {product.assembled_pose.pose.position.y:.3f} (m)\n'
372 output += f' z: {product.assembled_pose.pose.position.z:.3f} (m)\n'
373
374 roll, pitch, yaw = rpy_from_quaternion(product.assembled_pose.pose.orientation)
375 output += ' Orientation:\n'
376 output += f' roll: {rad_to_deg_str(roll)}\n'
377 output += f' pitch: {rad_to_deg_str(pitch)}\n'
378 output += f' yaw: {rad_to_deg_str(yaw)}\n'
379
380 output += f' Install direction:\n'
381 output += f' x: {product.install_direction.x:.1f}\n'
382 output += f' y: {product.install_direction.y:.1f}\n'
383 output += f' z: {product.install_direction.z:.1f}\n'
384
385 return output
386
387 def _parse_combined_task(self, combined_task: CombinedTask):
388 '''
389 Parses a CombinedTask object and returns a string representation.
390
391 Args:
392 combined_task (CombinedTask): CombinedTask object to parse
393
394 Returns:
395 str: String representation of the CombinedTask object
396 '''
397
398 output = 'Type: Combined\n'
399 output += '==========================\n'
400 output += f'Station: {self._stations[combined_task.station].title()}\n'
401 output += 'Products:\n'
402 output += '==========================\n'
403
404 product: AssemblyPartMsg
405 for product in combined_task.parts:
406 part_color = CompetitionInterface._part_colors[product.part.color].capitalize()
407 part_color_emoji = CompetitionInterface._part_colors_emoji[product.part.color]
408 part_type = CompetitionInterface._part_types[product.part.type].capitalize()
409
410 output += f'Part: {part_color_emoji} {part_color} {part_type}\n'
411
412 output += ' Position:\n'
413 output += f' x: {product.assembled_pose.pose.position.x:.3f} (m)\n'
414 output += f' y: {product.assembled_pose.pose.position.y:.3f} (m)\n'
415 output += f' z: {product.assembled_pose.pose.position.z:.3f} (m)\n'
416
417 roll, pitch, yaw = rpy_from_quaternion(product.assembled_pose.pose.orientation)
418 output += ' Orientation:\n'
419 output += f' roll: {rad_to_deg_str(roll)}\n'
420 output += f' pitch: {rad_to_deg_str(pitch)}\n'
421 output += f' yaw: {rad_to_deg_str(yaw)}\n'
422
423 output += f' Install direction:\n'
424 output += f' x: {product.install_direction.x:.1f}\n'
425 output += f' y: {product.install_direction.y:.1f}\n'
426 output += f' z: {product.install_direction.z:.1f}\n'
427
428 return output
429
430 def _parse_order(self, order: Order):
431 '''Parse an order message and return a string representation.
432
433 Args:
434 order (Order) -- Order message
435
436 Returns:
437 String representation of the order message
438 '''
439 output = '\n\n==========================\n'
440 output += f'Received Order: {order.order_id}\n'
441 output += f'Priority: {order.order_priority}\n'
442
443 if order.order_type == OrderMsg.KITTING:
444 output += self._parse_kitting_task(order.order_task)
445 elif order.order_type == OrderMsg.ASSEMBLY:
446 output += self._parse_assembly_task(order.order_task)
447 elif order.order_type == OrderMsg.COMBINED:
448 output += self._parse_combined_task(order.order_task)
449 else:
450 output += 'Type: Unknown\n'
451 return output
452
453 def lock_agv_tray(self, num):
454 '''
455 Lock the tray of an AGV and parts on the tray. This will prevent tray and parts from moving during transport.
456
457 Args:
458 num (int): AGV number
459
460 Raises:
461 KeyboardInterrupt: Exception raised when the user presses Ctrl+C
462 '''
463
464 # Create a client to send a request to the `/ariac/agv{num}_lock_tray` service
465 tray_locker = self.create_client(
466 Trigger,
467 f'/ariac/agv{num}_lock_tray'
468 )
469
470 # Build the request
471 request = Trigger.Request()
472 # Send the request
473 future = tray_locker.call_async(request)
474
475 # Wait for the response
476 try:
477 rclpy.spin_until_future_complete(self, future)
478 except KeyboardInterrupt as kb_error:
479 raise KeyboardInterrupt from kb_error
480
481 # Check the response
482 if future.result().success:
483 self.get_logger().info(f'Locked AGV{num}\'s tray')
484 else:
485 self.get_logger().warn('Unable to lock tray')
486
487 def move_agv_to_station(self, num, station):
488 '''
489 Move an AGV to an assembly station.
490
491 Args:
492 num (int): AGV number
493 station (int): Assembly station number
494
495 Raises:
496 KeyboardInterrupt: Exception raised when the user presses Ctrl+C
497 '''
498
499 # Create a client to send a request to the `/ariac/move_agv` service.
500 mover = self.create_client(
501 MoveAGV,
502 f'/ariac/move_agv{num}')
503
504 # Create a request object.
505 request = MoveAGV.Request()
506
507 # Set the request location.
508 if station in [AssemblyTaskMsg.AS1, AssemblyTaskMsg.AS3]:
509 request.location = MoveAGV.Request.ASSEMBLY_FRONT
510 else:
511 request.location = MoveAGV.Request.ASSEMBLY_BACK
512
513 # Send the request.
514 future = mover.call_async(request)
515
516 # Wait for the server to respond.
517 try:
518 rclpy.spin_until_future_complete(self, future)
519 except KeyboardInterrupt as kb_error:
520 raise KeyboardInterrupt from kb_error
521
522 # Check the result of the service call.
523 if future.result().success:
524 self.get_logger().info(f'Moved AGV{num} to {self._stations[station]}')
525 else:
526 self.get_logger().warn(future.result().message)
Code Explanation
The competition interface from Tutorial 4 was augmented with the components described below.
Imports
MoveAGV: Service class which is used to move an AGV to a preset location (ariac_msgs/srv/MoveAGV).
Instance Methods
lock_agv_tray(self, num): Method to lock the tray of an AGV. This method creates a client to the /ariac/agv{num}_lock_tray service and calls it. The AGV number is passed as an argument to the method.move_agv_to_station(self, num, station): Method to move an AGV to a station. This method creates a client to the /ariac/move_agv{num} service and calls it. The AGV number and station are passed as arguments to the method.
Executable
#!/usr/bin/env python3
'''
To test this script, run the following commands in separate terminals:
- ros2 launch ariac_gazebo ariac.launch.py trial_name:=tutorial competitor_pkg:=ariac_tutorials
- ros2 run ariac_tutorials tutorial_5.py
'''
import rclpy
from ariac_msgs.msg import Order as OrderMsg
from ariac_tutorials.competition_interface import CompetitionInterface
def main(args=None):
rclpy.init(args=args)
interface = CompetitionInterface()
interface.start_competition()
while not interface.orders:
try:
rclpy.spin_once(interface)
except KeyboardInterrupt:
break
for order in interface.orders:
if order.order_type == OrderMsg.ASSEMBLY:
for agv in order.order_task.agv_numbers:
interface.lock_agv_tray(agv)
interface.move_agv_to_station(agv, order.order_task.station)
interface.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Code Explanation
This executable does the following:
Initialize the ROS client library.
Create an instance of the class
CompetitionInterfaceas a ROS node.Start the competition.
Iterate through the list of orders and retrieve orders with assembly tasks.
Note: See Tutorial 4 for more information on retrieving orders.
Retrieve AGVs for the assembly tasks.
For each AGV:
Lock the tray of the AGVs.
Move the AGVs to the assembly station.
Build Instructions
cmake_minimum_required(VERSION 3.8)
project(ariac_tutorials)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(ariac_msgs REQUIRED)
find_package(orocos_kdl REQUIRED)
# Install the config directory to the package share directory
install(DIRECTORY
config
DESTINATION share/${PROJECT_NAME}
)
# Install Python modules
ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME})
# Install Python executables
install(PROGRAMS
scripts/tutorial_1.py
scripts/tutorial_2.py
scripts/tutorial_3.py
scripts/tutorial_4.py
scripts/tutorial_5.py
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
Run the Executable
In terminal 1, run the following commands:
cd ~/ariac_ws colcon build . install/setup.bash ros2 run ariac_tutorials tutorial_5.py
The node will wait until the competition is ready.
In terminal 2, run the following commands:
cd ~/ariac_ws . install/setup.bash ros2 launch ariac_gazebo ariac.launch.py trial_name:=tutorial competitor_pkg:=ariac_tutorials
Once the environment is loaded and the competition state is ready, the interface node running in terminal 1 will start the competition and move AGVs 1 and 2 to station 1.