From b3106d60f0315b6fda3676fa34ef97ff6be2bf2c Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 27 Dec 2023 15:15:13 +0800 Subject: [PATCH 01/50] EFC fleet adapter Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 127 ++- .../fleet_adapter_mir/MiRClientAPI.py | 179 ---- .../fleet_adapter_mir/MiRCommandHandle.py | 967 ------------------ .../fleet_adapter_mir/fleet_adapter_mir.py | 416 +++----- .../fleet_adapter_mir/mir_api.py | 775 ++++++++++++++ .../fleet_adapter_mir/rmf_cart_missions.json | 559 ++++++++++ .../fleet_adapter_mir/rmf_missions.json | 215 ++++ .../fleet_adapter_mir/robot_adapter_mir.py | 470 +++++++++ 8 files changed, 2205 insertions(+), 1503 deletions(-) delete mode 100644 fleet_adapter_mir/fleet_adapter_mir/MiRClientAPI.py delete mode 100755 fleet_adapter_mir/fleet_adapter_mir/MiRCommandHandle.py create mode 100644 fleet_adapter_mir/fleet_adapter_mir/mir_api.py create mode 100644 fleet_adapter_mir/fleet_adapter_mir/rmf_cart_missions.json create mode 100644 fleet_adapter_mir/fleet_adapter_mir/rmf_missions.json create mode 100644 fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index 389f243..6db6795 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -1,73 +1,60 @@ -# ROBOT CONFIG ================================================================= -# To init MiR robot REST APIs and RMF states - -robots: - # NOTE(CH3): This robot name is different from the MiR robot name - # for some reason - # - # I don't have access to the MiR REST server to test it out - # Doesn't seem very configurable from the code side? - # - # So for now, this name is the name used in the script only. - my_test_robot: - mir_config: - base_url: "http://some.ip.address/api/v2.0.0/" - user: "application/json" - password: "Basic HASH_OF_PASSWORD_OBTAINED_AT_API_DOCUMENTATION_PAGE" - rmf_move_mission: "rmf_default_move_mission" # This mission must be predefined - dock_and_charge_mission: "dock_to_charger" # This mission must be predefined - - rmf_config: - robot_state_update_frequency: 1 - start: - map_name: "L1" - max_merge_waypoint_distance: 3.0 - max_merge_lane_distance: 3.0 - # NOTE(CH3): - # If you leave these empty, the robot will try to figure it out on init - # from the MiR reported location. - # - # Otherwise, fill BOTH of these! And make sure that the robot starts - # ON the waypoint! - waypoint_index: 0 # Optional - orientation: 0.0 # Optional, radians - charger: - waypoint: "charger_waypoint" - - -# NODE CONFIG ================================================================== # To init the ROS2 node names used in the script - node_names: robot_command_handle: "rmf_mir_fleet_command_handler" fleet_state_publisher: "rmf_mir_fleet_state_publisher" - rmf_fleet_adapter: "TestDeliveryAdapter" + rmf_fleet_adapter: "DeliveryAdapter" -# TRANSFORM CONFIG ============================================================= -# For computing transforms between MiR and RMF coordinate systems -# -# NOTE(CH3): I am assuming that the transform is the same across maps -# Otherwise... Future me or future someone else, PLEASE KEY THIS PER MAP -# And also remember to edit the robot.transforms dictionary accordingly!! +# Conversions between RMF data and MiR data +conversions: + reference_coordinates: + L1: # Map name + rmf: [[0.0, 0.0], + [0.0, 1.0], + [1.0, 1.0], + [1.0, 0.0]] + mir: [[0.0, 0.0], + [0.0, 1.0], + [1.0, 1.0], + [1.0, 0.0]] + L2: + rmf: [[0.0, 0.0], + [0.0, 1.0], + [1.0, 1.0], + [1.0, 0.0]] + mir: [[0.0, 0.0], + [0.0, 1.0], + [1.0, 1.0], + [1.0, 0.0]] -reference_coordinates: - rmf: [[26.95, -20.23], - [29.26, -22.38], - [11.4, -16.48], - [12.46, -16.99]] + # RMF to MIR map names + maps: + L1: "Level 1" # Map name stored on MiR + L2: "Level 2" - mir: [[7.2, 16.6], - [5.15, 18.35], - [23, 12.35], - [22.05, 12.95]] + missions: + move: "rmf_move" # This mission must be predefined + dock_and_charge: "rmf_dock_and_charge" # This mission must be predefined + localize: "rmf_localize" + # dock_to_cart: "rmf_dock_to_cart" + # pickup: "rmf_pickup_cart" + # dropoff: "rmf_dropoff_cart" + # exit_lot: "rmf_exit_lot" + go_to: "rmf_move_to_position" + # check_footprint: "rmf_update_footprint" + lift_positions: + lift_1: # Name of lift matching the navigation graph + L1: + name: "Level_1_Lift" # Name of lift position stored on the robot on this map + # tolerance: 0.3 + L2: + name: "Level_2_Lift" -# FLEET CONFIG ================================================================= -# RMF Fleet parameters +# RMF Fleet parameters rmf_fleet: - name: "test_fleet" + name: "mir" limits: linear: [0.7, 0.3] # velocity, acceleration angular: [1.0, 0.45] @@ -85,17 +72,27 @@ rmf_fleet: friction_coefficient: 0.20 ambient_system: power: 20.0 - cleaning_system: + tool_system: power: 650.0 recharge_threshold: 0.01 recharge_soc: 1.0 account_for_battery_drain: True - publish_fleet_state: True - fleet_state_topic: "fleet_states" - fleet_state_publish_frequency: 1 + publish_fleet_state: 1.0 # Publish frequency (Hz) for the fleet state task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: True - delivery: False + delivery: True clean: False - finishing_request: "nothing" # [park, charge, nothing] - action_categories: ["custom_mission_1"] + finishing_request: "charge" # [park, charge, nothing] + actions: [] + robots: + mir_1: + charger: "Charger_1" + max_merge_waypoint_distance: 0.25 + max_merge_lane_distance: 1.0 + mir_config: + base_url: "http://some.ip.address/api/v2.0.0/" + user: "application/json" + password: "Basic HASH_OF_PASSWORD_OBTAINED_AT_API_DOCUMENTATION_PAGE" + + robot_state_update_frequency: 1 + debug: False # whether to enable debug printouts from EasyFullControl diff --git a/fleet_adapter_mir/fleet_adapter_mir/MiRClientAPI.py b/fleet_adapter_mir/fleet_adapter_mir/MiRClientAPI.py deleted file mode 100644 index f3a6cad..0000000 --- a/fleet_adapter_mir/fleet_adapter_mir/MiRClientAPI.py +++ /dev/null @@ -1,179 +0,0 @@ -import requests -import json -from urllib.error import HTTPError - - -__all__ = [ - "MirAPI" -] - - -class MirAPI: - def __init__(self, prefix, headers, timeout=10.0, debug=False): - #HTTP connection - self.prefix = prefix - self.debug = debug - self.headers = headers - self.timeout = timeout - self.connected = False - - # Test connectivity - try: - response = requests.get(self.prefix + 'wifi/connections', headers=self.headers, timeout=self.timeout) - self.connected = True - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - except Exception as err: - print(f"Other error: {err}") - - def status_get(self): - if not self.connected: - return - try: - response = requests.get(self.prefix + f'status', headers=self.headers, timeout=self.timeout) - return response.json() - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - except Exception as err: - print(f"Other error: {err}") - - def missions_get(self): - if not self.connected: - return - try: - response = requests.get(self.prefix + 'missions', headers = self.headers, timeout = 1.0) - if self.debug: - print(f"Response: {response.json()}") - return response.json() - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - except Exception as err: - print(f"Other error: {err}") - - def positions_get(self): - if not self.connected: - return - try: - response = requests.get(self.prefix + 'positions' , headers=self.headers, timeout=self.timeout) - if self.debug: - print(f"Response: {response.json()}") - return response.json() - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - except Exception as err: - print(f"Other error: {err}") - - def mission_queue_id_get(self, mission_queue_id): - if not self.connected: - return - try: - response = requests.get(self.prefix + f'mission_queue/{mission_queue_id}', headers = self.headers, timeout=self.timeout) - return response.json() - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - except Exception as err: - print(f"Other here error: {err}") - - def mission_queue_post(self, mission_id, full_mission_description=None): - if not self.connected: - return - data = {'mission_id': mission_id} - if full_mission_description is not None: - data = full_mission_description - if mission_id != full_mission_description['mission_id']: - print(f'Inconsistent mission id, provided [{mission_id}], full_mission_description: [{full_mission_description}]') - return - - try: - response = requests.post(self.prefix + 'mission_queue' , headers = self.headers, data=json.dumps(data), timeout = self.timeout) - if self.debug: - print(f"Response: {response.json()}") - return response.json() - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - except Exception as err: - print(f"Other here error: {err}") - - def missions_mission_id_actions_post(self,mission_id,body): - if not self.connected: - return - try: - response = requests.post(self.prefix + 'missions/' + mission_id +'/actions' , headers = self.headers, data=json.dumps(body), timeout = self.timeout) - if self.debug: - print(f"Response: {response.json()}") - print(response.status_code) - return response.json() - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - except Exception as err: - print(f"Other error: {err}") - - def missions_post(self, mission): - if not self.connected: - return - try: - response = requests.post(self.prefix + 'missions' , headers = self.headers, data=mission, timeout = self.timeout) - if self.debug: - print(f"Response: {response.json()}") - return response.json() - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - except Exception as err: - print(f"Other error: {err}") - - def positions_guid_get(self, guid): - if not self.connected: - return - try: - response = requests.get(self.prefix + 'positions/'+ guid, headers=self.headers, timeout=self.timeout) - if self.debug: - print(f"Response: {response.json()}") - return response.json() - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - except Exception as err: - print(f"Other error: {err}") - - def status_put(self, state_id): - if not self.connected: - return - data = {"state_id": state_id} - try: - response = requests.put(self.prefix + 'status', headers = self.headers, data=data, timeout=self.timeout) - if self.debug: - print(f"Response: {response.json()}") - return response.json() - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - except Exception as err: - print(f"Other error: {err}") - - def mission_queue_delete(self): - if not self.connected: - return - try: - response = requests.delete(self.prefix + 'missions' , headers = self.headers, timeout = self.timeout) - if self.debug: - print(f"Response: {response.headers}") - return True - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - return False - except Exception as err: - print(f"Other error: {err}") - return False - - def missions_guid_delete(self, guid): - if not self.connected: - return - try: - response = requests.delete(self.prefix + 'missions/' +guid , headers = self.headers, timeout = self.timeout) - if self.debug: - print(f"Response: {response.headers}") - return True - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - return False - except Exception as err: - print(f"Other error: {err}") - return False diff --git a/fleet_adapter_mir/fleet_adapter_mir/MiRCommandHandle.py b/fleet_adapter_mir/fleet_adapter_mir/MiRCommandHandle.py deleted file mode 100755 index 7be617d..0000000 --- a/fleet_adapter_mir/fleet_adapter_mir/MiRCommandHandle.py +++ /dev/null @@ -1,967 +0,0 @@ -from rmf_fleet_msgs.msg import Location, RobotMode, RobotState - -import rmf_adapter as adpt - -from collections import namedtuple -from typing import Any -import threading -import urllib3 -import copy -import enum -import math -import argparse -import json - -from datetime import timedelta -from dataclasses import dataclass - -__all__ = [ - "MiRLocation", - "MiRState", - "MiRPositionTypes", - "MiRCommandHandle", - #"MiRRetryContext" -] - - - -############################################################################### -# TYPES -############################################################################### - -MiRLocation = namedtuple("MiRLocation", ['x', 'y', 'yaw']) - - -class MiRState(enum.IntEnum): - READY = 3 - PAUSE = 4 - EXECUTING = 5 - MANUAL_CONTROL = 11 - ERROR = 12 - - -class MiRPositionTypes(enum.IntEnum): - ROBOT = 0 - CHARGING_STATION = 7 - CHARGING_STATION_ENTRY = 8 - - -############################################################################### -# CLASSES -############################################################################### -@dataclass -class Action: - category: str - description: str - start_time: int - execution: Any - check_task_completion: Any - - -class MiRCommandHandle(adpt.RobotCommandHandle): - def __init__(self, - name, - node, - rmf_graph, - robot_traits, - robot_state_update_frequency=1, - dry_run=False): - adpt.RobotCommandHandle.__init__(self) - - # DEBUG VARIABLES - self.last_update_type = None - # - - self.name = name # Name of robot object in config yaml - self.node = node - self.robot_traits = robot_traits - self.linear_velocity = robot_traits.linear.nominal_velocity - self.angular_velocity = robot_traits.rotational.nominal_velocity - self.dry_run = dry_run # For testing only. Disables REST calls. - - self.paused = False - self.paused_path = [] - - self.next_arrival_updater = None - - # Robot State ========================================================= - self.robot_state = RobotState() - self.last_robot_state_update = -1 - self.robot_state_update_frequency = robot_state_update_frequency - - # NOTE(CH3): This is a naively monotonically increasing task counter. - # - # There is no interface to get the task request message ID! - # Instead I am just following the behaviour from rmf_core's - # full_control adapter. - self.current_task_id = 0 - - self.transforms = {'rmf_to_mir': None, - 'mir_to_rmf': None} - - # RMF Variables ======================================================= - self.rmf_updater = None - self.rmf_graph = rmf_graph - self.rmf_lane_dict = {} # Maps entry, exit to lane index - - # TODO(MXG): We should initialize this to something sensible, based on - # the config file information - self.rmf_map_name = "" - - # This is made out of RMF Plan Waypoints - self.rmf_remaining_path_waypoints = [] - - # NOTE(CH3): This is required for fleet state publishing - # The path is in reverse order! (i.e. [last, ... first]) - # This is made out of RMF Location messages - self.rmf_robot_state_path_locations = [] - - # Populate lane dict - for i in range(self.rmf_graph.num_lanes): - graph_lane = self.rmf_graph.get_lane(i) - id_tuple = ( - graph_lane.entry.waypoint_index, - graph_lane.exit.waypoint_index - ) - self.rmf_lane_dict.get(id_tuple, []).append(i) - - reverse_id_tuple = ( - graph_lane.exit.waypoint_index, - graph_lane.entry.waypoint_index - ) - self.rmf_lane_dict.get(reverse_id_tuple, []).append(i) - - - # RMF Location Trackers =============================================== - self.rmf_current_lane_index = None # None when moving - self.rmf_current_waypoint_index = None - self.rmf_target_waypoint_index = None # None when not moving - - # RMF Execution Flags ================================================= - self.rmf_docking_executed = False - self.rmf_docking_requested = False - self.rmf_path_requested = False - - # RMF perform action=================================================== - self.action = None - - # MiR Variables ======================================================= - self.mir_name = "" # Name of robot on MiR REST server - self.mir_missions = {} # MiR Mission Name-GUID Dict - self.mir_positions = {} # MiR Place Name-GUID Dict - self.mir_api = None # MiR REST API - self.mir_state = MiRState.PAUSE - self.mir_rmf_move_mission = None - self.mir_dock_and_charge_mission = None - - # Thread Management =================================================== - # Path queue execution thread - self._path_following_thread = None - self._path_quit_event = threading.Event() - self._path_quit_cv = threading.Condition() - self._update_mutex = threading.Lock() - - # Dock queue execution thread - self._docking_thread = None - self._docking_quit_event = threading.Event() - self._docking_quit_cv = threading.Condition() - - # Start State Update Timer ============================================ - self.state_update_timer = self.node.create_timer( - self.robot_state_update_frequency, - self.lock_and_execute_updates - ) - - ############################################################################ - # Init RobotUpdateHandle class member - def init_updater(self, updater): - self.rmf_updater = updater - self.rmf_updater.set_action_executor(self._action_executor) - - ########################################################################## - # ROBOTCOMMANDHANDLE OVERLOADS (DO NOT CHANGE METHOD SIGNATURES) - ########################################################################## - # Pause and resume are not technically overrides... - # But it's neater to put them here - def pause(self): - """Set pause flag and hold on to any requested paths.""" - self.paused = True - - if self.rmf_remaining_path_waypoints: - self.node.get_logger().info( - '[PAUSE] {self.name}: Current path saved!' - ) - - self.paused_path = self.rmf_remaining_path_waypoints - self.rmf_remaining_path_waypoints = [] - - def resume(self): - """Unset pause flag and substitute paused paths if no paths exist.""" - if self.paused: - self.paused = False - - if self.rmf_remaining_path_waypoints: - return - elif self.paused_path: - self.rmf_remaining_path_waypoints = self.paused_path - self.paushed_path = [] - - self.node.get_logger().info( - '[RESUME] {self.name}: Saved path restored!' - ) - else: - return - - def clear(self): - """Clear all pending action information""" - self.rmf_remaining_path_waypoints.clear() - self.next_arrival_updater = None - self.rmf_path_requested = False - self.rmf_target_waypoint_index = None - - self.rmf_docking_requested = False - self.rmf_docking_executed = False - - def stop(self): - """Stop all path following and docking commands.""" - self.clear() - - if self._path_following_thread is not None: - self._path_quit_event.set() - self._path_quit_cv.acquire() - self._path_quit_cv.notify_all() - self._path_quit_cv.release() - self._path_following_thread.join() - self._path_following_thread = None - - if self._docking_thread is not None: - self._docking_quit_event.set() - self._docking_quit_cv.acquire() - self._docking_quit_cv.notify_all() - self._docking_quit_cv.release() - self._docking_thread.join() - self._docking_thread = None - - if not self.dry_run: - self.mir_api.mission_queue_delete() - - old_state = self.mir_state - self.mir_state = MiRState.PAUSE - - # Prevent repeat and needless logs - if (old_state != MiRState.PAUSE - and self.robot_state.mode.mode != RobotMode.MODE_IDLE): - self.node.get_logger().info( - '[ABORT] {self.name}: Robot stop called!' - ) - - def follow_new_path(self, - waypoints, - next_arrival_estimator, # function! - path_finished_callback): - waypoint_indices = ' '.join([str(x.graph_index) for x in waypoints]) - print(f"Following new Path: {waypoint_indices}") - - self.stop() - self.current_task_id += 1 - - self.rmf_path_requested = True - - # Obtain plan waypoints =============================================== - waypoints = copy.copy(waypoints) - - self.rmf_remaining_path_waypoints = [ - (i, waypoints[i]) for i in range(len(waypoints)) - ] - - # We reverse this list so that we can pop it instead of traversing - # it using an index (which is more Pythonic) - self.rmf_remaining_path_waypoints.reverse() - - # Construct robot state path list ===================================== - self.rmf_robot_state_path_locations = [] - - for (_, waypoint) in self.rmf_remaining_path_waypoints: - # Split timestamp into decimal and whole second portions - _sub_seconds, _seconds = math.modf(waypoint.time.timestamp()) - - _msg = Location() - _msg.x, _msg.y, _msg.yaw = waypoint.position - _msg.t.sec, _msg.t.nanosec = int(_seconds), int(_sub_seconds * 1e9) - - self.rmf_robot_state_path_locations.append(_msg) - - if not self.dry_run: - state_id = MiRState.READY - self.mir_api.status_put(state_id) - - def path_following_closure(): - _current_waypoint = None - _next_waypoint = None - - # LOOP ============================================================ - # Kept alive if paused - while ((self.rmf_remaining_path_waypoints or self.paused) - or _current_waypoint): - - # DEBUG PRINTOUT - # print([str(x[1].graph_index) for x in self.rmf_remaining_path_waypoints]) - next_mission_wait = 0 - if not self.paused: # Skipped if paused - if _current_waypoint is None: - _, _current_waypoint = ( - self.rmf_remaining_path_waypoints.pop() - ) - self.rmf_path_requested = True - - waypoint_leave_msg = _current_waypoint.time - ros_waypoint_leave_time = waypoint_leave_msg - - ros_now = self.node.get_clock().now().nanoseconds / 1e9 - next_mission_wait = ( - ros_waypoint_leave_time.timestamp() - ros_now - ) - - else: - print("Paused") - # Prevent spinning out of control when paused - self._path_quit_cv.acquire() - self._path_quit_cv.wait(1) - self._path_quit_cv.release() - - # CHECK FOR PRE-EMPT ========================================== - if self._path_quit_event.is_set(): - self.clear() - self.node.get_logger().info( - '[ABORT] {self.name}: Pre-empted path following!' - ) - return - - # EXECUTE NEXT COMMAND ======================================== - # Wait for time to leave and robot to finish moving - if (next_mission_wait <= 0 - and self.mir_state == MiRState.READY - and not self.paused): # Skipped if paused - - with self._update_mutex: - # END ================================================== - if not self.rmf_remaining_path_waypoints: # We're done! - self.rmf_path_requested = False - self.next_arrival_updater = None - - print("Path Finished Callback") - path_finished_callback() - self.execute_updates() - - return - - # ASSIGN NEXT TARGET =================================== - else: - _next_path_index, _next_waypoint = ( - self.rmf_remaining_path_waypoints[-1] - ) - - self.next_arrival_updater = ( - lambda p : next_arrival_estimator( - _next_path_index, self.estimate_arrival( - _next_waypoint.position, p - ) - ) - ) - - # Grab graph indices - if _next_waypoint.graph_index is not None: - _next_index = _next_waypoint.graph_index - self.rmf_map_name = self.get_map_name( - _next_index - ) - else: - _next_index = None - - if _current_waypoint.graph_index is not None: - _current_index = ( - _current_waypoint.graph_index - ) - else: - _current_index = None - - _current_waypoint = None - - # Update Internal Location Trackers ================ - # [IdleAtWaypoint -> RMF_Move] - # [IdleAtLane -> RMF_Move] - # [IdleAtUnknown -> RMF_Move] - - # Set target index - self.rmf_target_waypoint_index = _next_index - - # Infer and set lane index - if not self.rmf_current_lane_index: - if _current_index is not None: - self.rmf_current_lane_index = ( - self.rmf_lane_dict.get( - (_current_index, _next_index) - ) - ) - # DEBUG VARS - print(f"Current Lane Index: {self.rmf_current_lane_index}, Current Index: {_current_index}, Next Index : {_next_index}") - - # Unset current index - self.rmf_current_waypoint_index = None - - # SEND NEXT TARGET ===================================== - _mir_pos = self.transforms['rmf_to_mir'].transform( - [ - _next_waypoint.position[0], - _next_waypoint.position[1] - ] - ) - _mir_ori_rad = \ - _next_waypoint.position[2] + self.transforms['rmf_to_mir'].get_rotation() - _mir_ori = math.degrees(_mir_ori_rad % (2 * math.pi)) - - if _mir_ori > 180.0: - _mir_ori = _mir_ori - 360.0 - elif _mir_ori <= -180.0: - _mir_ori = _mir_ori + 360.0 - - mir_location = MiRLocation( - x=_mir_pos[0], - y=_mir_pos[1], - yaw=_mir_ori - ) - - print(f"RMF location x:{_next_waypoint.position[0]}" - f"y:{_next_waypoint.position[1]} " - f'yaw:{_next_waypoint.position[2]}') - print(f'MiR location: {mir_location}') - - print(f"RMF Index: {_next_waypoint.graph_index}") - - self.mir_state = None - - self.queue_rmf_move_coordinate_mission(mir_location) - self.execute_updates() - - # DEBUGGING - print(f'MiR state: {self.mir_state}') - - continue - - if not self.paused: # Skipped if paused - # Prevent spinning out of control - if next_mission_wait <= 0: - next_mission_wait = 0.1 - - self._path_quit_cv.acquire() - self._path_quit_cv.wait(next_mission_wait) - self._path_quit_cv.release() - - self._path_quit_event.clear() - - if self._path_following_thread is not None: - self._path_following_thread.join() - - self._path_following_thread = threading.Thread( - target=path_following_closure - ) - - self._path_following_thread.start() - - - def dock(self, dock_name, docking_finished_callback): - """Start thread to invoke MiR docking mission, then notify rmf_core.""" - self.stop() - - self.current_task_id += 1 - - self.rmf_docking_requested = True - self.rmf_docking_executed = False - - if not self.dry_run: - state_id = MiRState.READY - self.mir_api.status_put(state_id) - - def dock_closure(): - if not self.dry_run: - response = self.queue_dock_and_charge_mission() - if response is None: - self.rmf_docking_requested = False - self.rmf_docking_executed = False - - if not self.rmf_docking_requested: - self.node.get_logger().info( - '[ERROR] Could not queue dock mission for dock at: "{dock_name}"!' - ) - docking_finished_callback() - - # Check for docking complete: - # Once the robot begins executing a queued docking mission, it's API - # response's `mission_text` will change to `Docking...` and with - # `state_text`:`Executing`. But once the docking is completed, the - # `mission_text` will revert to `Waiting for new missions...` and - # `state_text`:`Ready...`. - while self.rmf_docking_requested: - if not self.dry_run: - api_response = self.mir_api.status_get() - if ('docking' in api_response['mission_text'].lower()): - self.rmf_docking_executed = True - else: - api_response = None - self.rmf_docking_executed = False - - self.execute_updates(api_response) - - # Docking completed - if not self.dry_run: - # Below we try to catch the change in `mission_text` to `Docking...` - # first and then wait till the `state_text` is `Ready...` before - # considering the docking complete. - if (self.rmf_docking_executed - and api_response['state_id'] == MiRState.READY): - self.rmf_docking_requested = False - docking_finished_callback() - - self.node.get_logger().info( - '[COMPLETE] Completed dock at: "{dock_name}"!' - ) - return - else: - self.rmf_docking_requested = False - self.rmf_docking_executed = True - self.node.get_logger().info( - '[COMPLETE-DRYRUN] Completed dock at: "{dock_name}"!' - ) - docking_finished_callback() - return - - # Docking pre-empted - if self._docking_quit_event.is_set(): - self.rmf_docking_requested = False - self.rmf_docking_executed = False - - self.clear() - - self.node.get_logger().info( - '[ABORT] Pre-empted dock at: "{dock_name}"!' - ) - docking_finished_callback() - return - - self._docking_quit_cv.acquire() - self._docking_quit_cv.wait(1) - self._docking_quit_cv.release() - - self._docking_quit_event.clear() - - if self._docking_thread is not None: - self._docking_thread.join() - - self._docking_thread = threading.Thread(target=dock_closure) - self._docking_thread.start() - - - def estimate_arrival(self, goal, current_position): - # We do a very rough estimate here which ignores acceleration and - # deceleration rates - dx = goal[0] - current_position[0] - dy = goal[1] - current_position[1] - translation = math.sqrt(dx*dx + dy*dy) - t_translation = translation/self.linear_velocity - - rotation = math.fabs(math.fmod(goal[2] - current_position[2], math.pi)) - t_rotation = rotation/self.angular_velocity - - return t_translation + t_rotation - - def _action_executor(self, - category: str, - description: dict, - execution: adpt.robot_update_handle.ActionExecution): - with self._update_mutex: - # Get list of missions, get guid of mission - missions = self.mir_api.missions_get() - mission_guid = None - for m in missions: - if m['name'] == category: - mission_guid = m['guid'] - if mission_guid is None: - error_str = f'Action category {category} not supported, ignoring' - self.node.get_logger().error(error_str) - execution.error(error_str) - return - - # Start mission - response = self.mir_api.mission_queue_post(mission_guid) - mission_queue_id = response['id'] - - def _check_task_completion(): - mission_status = self.mir_api.mission_queue_id_get(mission_queue_id) - if 'finished' in mission_status and mission_status['finished'] is not None: - return True - return False - - # Keep track of perform action - self.action = Action( - category, - description, - self.node.get_clock().now(), - execution, - _check_task_completion) - self.node.get_logger().info(f"Robot [{self.name}] starts [{category}] action") - - def check_perform_action(self): - self.node.get_logger().info(f'Executing perform action: {self.action.category}') - action_ok = self.action.execution.okay() - if self.action.check_task_completion() or not action_ok: - if action_ok: - self.node.get_logger().info( - f"action [{self.action.category}] is completed") - self.action.execution.finished() - else: - self.node.get_logger().warn( - f"action [{self.action.category}] is killed/canceled") - self.action = None - return - - # Still executing perform action - # TODO(AA): Update accurate remaining time - self.action.execution.update_remaining_time(timedelta(seconds=10.0)) - # TODO(AA): Update self.action.execution state - - ########################################################################## - # INIT METHODS - ########################################################################## - def load_mir_missions(self): - if self.dry_run: - self.node.get_logger().info('{self.name}: DRY_RUN LOAD MISSIONS') - return - - self.node.get_logger().info('{self.name}: Retrieving MiR Missions...') - robot_missions_ls = self.mir_api.missions_get() - for i in robot_missions_ls: - if i['name'] not in self.mir_missions: - self.mir_missions[i['name']] = i - else: - if "move_coordinate" in i['name']: - print("removing {}".format(i['name'])) - self.mir_api.missions_guid_delete(i['guid']) - - self.node.get_logger().info( - f'retrieved {len(self.mir_missions)} missions' - ) - - def load_mir_positions(self): - if self.dry_run: - self.node.get_logger().info('{self.name}: DRY_RUN LOAD POSITIONS') - return - - self.node.get_logger().info('{self.name}: Retrieving MiR Positions...') - count = 0 - - for pos in self.mir_api.positions_get(): - if ( - pos['name'] not in self.mir_positions - or pos['guid'] != self.mir_positions[pos['name']]['guid'] - ): - if ( - pos['type_id'] == MiRPositionTypes.ROBOT - or pos['type_id'] == MiRPositionTypes.CHARGING_STATION_ENTRY - ): - self.mir_positions[pos['name']] = ( - self.mir_api.positions_guid_get(pos['guid']) - ) - count += 1 - self.node.get_logger().info(f'updated {count} positions') - - ########################################################################## - # MISSION METHODS - ########################################################################## - def queue_rmf_move_coordinate_mission(self, mir_location): - rmf_move_mission_guid = \ - self.mir_missions[self.mir_rmf_move_mission]['guid'] - mission = { - 'mission_id': rmf_move_mission_guid, - 'parameters': [ - {'id': 'x', 'value': mir_location.x, 'label': f'{mir_location.x:.3f}'}, - {'id': 'y', 'value': mir_location.y, 'label': f'{mir_location.y:.3f}'}, - {'id': 'yaw', 'value': mir_location.yaw, 'label': f'{mir_location.yaw:.3f}'} - ], - "priority": 0, - "description": "variable move mission to be called by open-rmf" - } - try: - response = self.mir_api.mission_queue_post(rmf_move_mission_guid, mission) - except Exception: - self.node.get_logger().error( - '{self.name}: Failed to call rmf_move_mission to ' - '[{mir_location.x:3f}_{mir_location.y:.3f}]!' - ) - - def queue_dock_and_charge_mission(self): - if self.mir_dock_and_charge_mission is None: - self.node.get_logger().error('No dock and charge mission defined in MiR config.') - return - - dock_and_charge_mission_guid = \ - self.mir_missions[self.mir_dock_and_charge_mission]['guid'] - try: - response = self.mir_api.mission_queue_post(dock_and_charge_mission_guid) - return response - except Exception: - self.node.get_logger().error( - f'{self.name}: Failed to call dock and charge mission ' - f'{self.mir_dock_and_charge_mission}' - ) - - ########################################################################## - # RMF CORE INTERACTION METHODS - ########################################################################## - def get_position(self, rmf=True, api_response=None, as_dimensions=False): - """Get MiR or RMF robot location from the MiR REST API.""" - if api_response is None: - if not self.dry_run: - api_response = self.mir_api.status_get() - else: - if as_dimensions: - return [[0.0], [0.0], [0.0]] - else: - return [0.0, 0.0, 0.0] - - mir_pos = [api_response['position']['x'], api_response['position']['y']] - mir_ori = api_response['position']['orientation'] - - # Output is [x, y, yaw] - if rmf: - rmf_pos = self.transforms['mir_to_rmf'].transform(mir_pos) - rmf_ori = (math.radians(mir_ori % 360) - + self.transforms['mir_to_rmf'].get_rotation()) - output = [*rmf_pos, rmf_ori] - else: - output = [*mir_pos, mir_ori] - - if as_dimensions: - return [[x] for x in output] - else: - return output - - # Priority... - # 1. update_position(waypoint, orientation) [At waypoint] - # 2. update_position(position, lanes) [In transit] - # 3. update_position(position, target_waypoint) [In transit, unknown lane] - # 4. update_position(map_name, position) [Lost] - def update_position(self, api_response=None): - """Update position using the MiR status location.""" - if api_response is None: - if not self.dry_run: - api_response = self.mir_api.status_get() - else: - self.rmf_updater.update_lost_position( - self.rmf_map_name, [0.0, 0.0, 0.0] - ) - self.node.get_logger().info("[DRYRUN] Updated Position: " - "pos: [0, 0] | ori: [0]") - return - - mir_pos = [api_response['position']['x'], api_response['position']['y']] - mir_ori = api_response['position']['orientation'] - - rmf_pos = self.transforms['mir_to_rmf'].transform(mir_pos) - rmf_ori = (math.radians(mir_ori % 360) - + self.transforms['mir_to_rmf'].get_rotation()) - - rmf_3d_pos = [*rmf_pos, rmf_ori] - - print("Updating") - - # At waypoint - # States: (0, 1, 0) - if self.rmf_current_waypoint_index is not None: - self.rmf_updater.update_current_waypoint( - self.rmf_current_waypoint_index, rmf_ori - ) - # DEBUG VARIABLES - update_type = 0 - if update_type != self.last_update_type: - print(f"Update Type: 0 Current Waypoint: {self.rmf_current_waypoint_index}, Orientation: {rmf_ori}") - - # In Transit or Idle in Lane - # States: (1, 0, 0), (1, 0, 1) - elif self.rmf_current_lane_index is not None: - self.rmf_updater.update_current_lanes( - rmf_3d_pos, self.rmf_current_lane_index - ) - - # DEBUG VARIABLES - update_type = 1 - if update_type != self.last_update_type: - print(f"Update Type: 1 Current Location: {rmf_3d_pos}, Current Lane Index: {self.rmf_current_lane_index}") - - # In Transit, Unknown Lane - # States: (0, 0, 1) - elif self.rmf_target_waypoint_index is not None: # In Unknown Lane - self.rmf_updater.update_off_grid_position( - rmf_3d_pos, self.rmf_target_waypoint_index - ) - - # DEBUG VARIABLES - update_type = 2 - if update_type != self.last_update_type: - print(f"Update Type: 2 Current Location: {rmf_3d_pos}, Target Waypoint Index: {self.rmf_target_waypoint_index}") - - # Lost or MiR Commanded - # States: (0, 0, 0) - else: - self.rmf_updater.update_lost_position( - self.rmf_map_name, rmf_3d_pos - ) - - # DEBUG VARIABLES - update_type = 3 - if update_type != self.last_update_type: - print(f"Update Type: 3 Current Map Name: {self.rmf_map_name}, Current Location: {rmf_3d_pos}") - - if self.next_arrival_updater: - self.next_arrival_updater(rmf_3d_pos) - - self.node.get_logger().info(f"Updated Position: pos: {rmf_pos} | " - f"ori: {rmf_ori}") - - def update_internal_location_trackers(self): - """Traverses the state machine to help manage robot location.""" - state_tuple = (self.rmf_current_lane_index is not None, - self.rmf_current_waypoint_index is not None, - self.rmf_target_waypoint_index is not None) - - # In the absence of a state, treat it as paused - if self.robot_state: - robot_mode = self.robot_state.mode.mode - else: - robot_mode = RobotMode.MODE_PAUSED - - # SEMANTIC STATE INFERENCE AND ADJUSTMENT ============================= - # See docs for more information on the state transitions - - # MiR_Move: Non-RMF Commanded Move-To-Coordinate - # (0, 1, 0) and (1, 0 ,0) --> (0, 0, 0) - # When robot is done moving, robot will be IdleAtUnknown - if not self.rmf_path_requested and robot_mode == RobotMode.MODE_MOVING: - # Unset all - self.rmf_current_lane_index = None - self.rmf_current_waypoint_index = None - self.rmf_target_waypoint_index = None - - # RMF_ReachedWaypoint -> IdleAtWaypoint - # Set current to target's value, unset target and lane - # (0, 0, 1) and (1, 0, 1) --> (0, 1, 0) - if (state_tuple == (0, 0, 1) or state_tuple == (1, 0, 1) - and robot_mode == RobotMode.MODE_IDLE - and not self.rmf_path_requested): - self.rmf_current_waypoint_index = self.rmf_target_waypoint_index - self.rmf_target_waypoint_index = None - self.rmf_current_lane_index = None - - # IdleAtWaypoint/Lane/Unknown -> RMF_Move - # - # Defined in self.follow_new_path's path_following_closure - # and called during path following execution - - def update_internal_robot_state(self, api_response=None): - """Update internal robot state message. Does not publish!""" - - # NOTE(CH3): You might need to use robot.mir_name depending - # on whether you want to use the config yaml name or MiR server - # name whereever the FleetState message is intended to be used - robot_state = RobotState() # Temporary msg to avoid race conditions - robot_state.name = self.name - - # NOTE(CH3): Presuming model here means robot model, not sim model - robot_state.model = "MiR100" - - if self.dry_run: - self.robot_state = robot_state - return - - try: - if api_response is None: - api_response = self.mir_api.status_get() - - #now_sec, now_ns = math.modf( - # self.node.get_clock().now().seconds_nanoseconds()) - now_sec, now_ns = self.node.get_clock().now().seconds_nanoseconds() - - # Populate Location message - rmf_location = Location() - rmf_location.x, rmf_location.y, rmf_location.yaw = ( - self.get_position(rmf=True, api_response=api_response) - ) - - rmf_location.level_name = self.rmf_map_name - self.node.get_logger().info(f'The rmf map name: {self.rmf_map_name}') - # Populate RobotState message - robot_state.task_id = str(self.current_task_id) - robot_state.battery_percent = api_response['battery_percentage'] - - robot_state.location = rmf_location - robot_state.path = self.rmf_robot_state_path_locations - robot_state.location.t.sec = now_sec - robot_state.location.t.nanosec = now_ns - - if api_response['mission_text'].startswith('Charging'): # Charging - robot_state.mode.mode = RobotMode.MODE_CHARGING - self.mir_state = MiRState.READY - elif api_response['state_id'] == MiRState.PAUSE: # Paused/Pre-empted - self.pause() - robot_state.mode.mode = RobotMode.MODE_PAUSED - self.mir_state = MiRState.PAUSE - elif (api_response['state_id'] == MiRState.EXECUTING # Moving - and not api_response['mission_text'].startswith('Charging')): - self.resume() - robot_state.mode.mode = RobotMode.MODE_MOVING - self.mir_state = MiRState.EXECUTING - elif api_response['state_id'] == MiRState.READY: # Idle/Moved - self.resume() - robot_state.mode.mode = RobotMode.MODE_IDLE - self.mir_state = MiRState.READY - - if self.rmf_docking_requested: - if self.rmf_docking_executed: # Docked - if api_response['state_id'] == MiRState.READY: - robot_state.mode.mode = RobotMode.MODE_IDLE - else: # Docking - robot_state.mode.mode = RobotMode.MODE_DOCKING - - # Update internal RobotState - self.robot_state = robot_state - - self.last_robot_state_update = ( - self.node.get_clock().now().nanoseconds / 1e9 - ) - except Exception as e: - self.node.get_logger().warn(f'Exception: {e}') - - ########################################################################## - # INTERNAL UPDATE LOOP - ########################################################################## - def execute_updates(self, api_response=None): - if api_response is None: - api_response = self.mir_api.status_get() - - self.update_internal_robot_state(api_response=api_response) - self.update_internal_location_trackers() - self.update_position(api_response=api_response) - - if (self.action): - self.check_perform_action() - - self.state_update_timer.reset() - - - def lock_and_execute_updates(self): - with self._update_mutex: - self.execute_updates() - - - def get_map_name(self, graph_index): - self.node.get_logger().info(str(self.rmf_graph.get_waypoint(graph_index))) - return self.rmf_graph.get_waypoint(graph_index).map_name diff --git a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py index 85f680e..f772154 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py @@ -1,24 +1,21 @@ +import asyncio import sys import yaml import nudged import argparse +import time +import threading from pprint import pprint from functools import partial import rclpy import rclpy.node -from rmf_fleet_msgs.msg import FleetState -from rmf_task_msgs.msg import TaskProfile, TaskType +from rclpy.duration import Duration -import rmf_adapter as adpt -import rmf_adapter.vehicletraits as traits -import rmf_adapter.battery as battery -import rmf_adapter.geometry as geometry -import rmf_adapter.graph as graph -import rmf_adapter.plan as plan - -from .MiRCommandHandle import MiRCommandHandle -from .MiRClientAPI import MirAPI +import rmf_adapter +import rmf_adapter.easy_full_control as rmf_easy +from rmf_adapter import Transformation +from .robot_adapter_mir import RobotAdapterMiR ############################################################################### # HELPER FUNCTIONS AND CLASSES @@ -55,229 +52,115 @@ def sanitise_dict(dictionary, inplace=False, recursive=False): return output -def compute_transforms(rmf_coordinates, mir_coordinates, node=None): +def compute_transforms(level, coords, node=None): """Get transforms between RMF and MIR coordinates.""" - transforms = { - 'rmf_to_mir': nudged.estimate(rmf_coordinates, mir_coordinates), - 'mir_to_rmf': nudged.estimate(mir_coordinates, rmf_coordinates) - } - + rmf_coords = coords['rmf'] + mir_coords = coords['mir'] + tf = nudged.estimate(rmf_coords, mir_coords) if node: - mse = nudged.estimate_error(transforms['rmf_to_mir'], - rmf_coordinates, - mir_coordinates) + mse = nudged.estimate_error(tf, rmf_coords, mir_coords) + node.get_logger().info( + f"Transformation error estimate for {level}: {mse}" + ) - node.get_logger().info(f"Transformation estimate error: {mse}") - return transforms + return Transformation( + tf.get_rotation(), + tf.get_scale(), + tf.get_translation() + ) -def create_fleet(config,nav_graph_path, mock): - """Create RMF Adapter, FleetUpdateHandle, and parse navgraph.""" - profile = traits.Profile( - geometry.make_final_convex_circle(config['rmf_fleet']['profile']['footprint']), - geometry.make_final_convex_circle(config['rmf_fleet']['profile']['vicinity']) - ) - robot_traits = traits.VehicleTraits( - linear=traits.Limits(*config['rmf_fleet']['limits']['linear']), - angular=traits.Limits(*config['rmf_fleet']['limits']['angular']), - profile=profile - ) - robot_traits.differential.reversible = config['rmf_fleet']['reversible'] +class FleetAdapterMiR: + def __init__( + self, + cmd_node, + adapter, + fleet_handle, + robot_handles: list[RobotAdapterMiR], + frequency, + event_loop, + ): + self.event_loop = event_loop + self.adapter = adapter + self.fleet_handle = fleet_handle + self.robot_handles: list[RobotAdapterMiR] = robot_handles + self.node = cmd_node + if frequency > 0.0: + self.period = 1.0/frequency + else: + raise Exception(f'Invalid robot update frequency: {frequency}') + self.robot_update_jobs = {} - voltage = config['rmf_fleet']['battery_system']['voltage'] - capacity = config['rmf_fleet']['battery_system']['capacity'] - charging_current = config['rmf_fleet']['battery_system']['charging_current'] - battery_sys = battery.BatterySystem.make(voltage,capacity,charging_current) + async def state_updates(self): + robot_updaters = [] + for robot in self.robot_handles: + robot_updaters.append(robot.update_loop(self.period)) + await asyncio.gather(*robot_updaters) + + def update_loop(self): + asyncio.set_event_loop(self.event_loop) + self.event_loop.run_until_complete(self.state_updates()) - mass = config['rmf_fleet']['mechanical_system']['mass'] - moment = config['rmf_fleet']['mechanical_system']['moment_of_inertia'] - friction = config['rmf_fleet']['mechanical_system']['friction_coefficient'] - mech_sys = battery.MechanicalSystem.make(mass,moment,friction) + def start(self): + update_thread = threading.Thread(target=self.update_loop) + update_thread.start() - ambient_power_sys = battery.PowerSystem.make( - config['rmf_fleet']['ambient_system']['power']) - tool_power_sys = battery.PowerSystem.make( - config['rmf_fleet']['cleaning_system']['power']) - motion_sink = battery.SimpleMotionPowerSink(battery_sys,mech_sys) - ambient_sink = battery.SimpleDevicePowerSink(battery_sys, ambient_power_sys) - tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys) + # Create the executor for the logger node + rclpy_executor = rclpy.executors.SingleThreadedExecutor() + rclpy_executor.add_node(self.node) - nav_graph = graph.parse_graph(nav_graph_path, robot_traits) + self.adapter.start() + rclpy_executor.spin() + + self.node.destroy_node() + rclpy_executor.shutdown() + rclpy.shutdown() - # RMF_CORE Fleet Adapter: Manages delivery or loop requests - if mock: - adapter = adpt.MockAdapter(config['node_names']['rmf_fleet_adapter']) - else: - adapter = adpt.Adapter.make(config['node_names']['rmf_fleet_adapter']) +def create_fleet(fleet_config, config_yaml, cmd_node) -> FleetAdapterMiR: + """Create RMF Adapter and fleet handle""" + for level, coords in config_yaml['conversions']['reference_coordinates'].items(): + tf = compute_transforms(level, coords, cmd_node) + fleet_config.add_robot_coordinates_transformation(level, tf) + + # RMF_CORE Fleet Adapter: Manages delivery or loop requests + adapter = rmf_adapter.Adapter.make(config_yaml['node_names']['rmf_fleet_adapter']) assert adapter, ("Adapter could not be init! " "Ensure a ROS2 scheduler node is running") - fleet_name = config['rmf_fleet']['name'] - fleet = adapter.add_fleet(fleet_name, robot_traits, nav_graph) - - if config['rmf_fleet']['publish_fleet_state']: - fleet.fleet_state_publish_period(None) - - drain_battery = config['rmf_fleet']['account_for_battery_drain'] - recharge_threshold = config['rmf_fleet']['recharge_threshold'] - recharge_soc = config['rmf_fleet']['recharge_soc'] - finishing_request = config['rmf_fleet']['task_capabilities']['finishing_request'] - # Set task planner params - ok = fleet.set_task_planner_params( - battery_sys, - motion_sink, - ambient_sink, - tool_sink, - recharge_threshold, - recharge_soc, - drain_battery, - finishing_request) - assert ok, ("Unable to set task planner params") - - task_capabilities_config = config['rmf_fleet']['task_capabilities'] - finishing_request = task_capabilities_config['finishing_request'] - # Set task planner params - ok = fleet.set_task_planner_params( - battery_sys, - motion_sink, - ambient_sink, - tool_sink, - recharge_threshold, - recharge_soc, - drain_battery, - finishing_request) - assert ok, ("Unable to set task planner params") - - # Accept Standard RMF Task which are defined in config.yaml - always_accept = adpt.fleet_update_handle.Confirmation() - always_accept.accept() - if task_capabilities_config['loop']: - print(f"Fleet [{fleet_name}] is configured to perform Loop tasks") - fleet.consider_patrol_requests(lambda desc: always_accept) - if task_capabilities_config['delivery']: - print(f"Fleet [{fleet_name}] is configured to perform Delivery tasks") - fleet.consider_delivery_requests(lambda desc: always_accept, lambda desc: always_accept) - - # Whether to accept custom RMF action tasks - def _consider(description: dict): - confirm = adpt.fleet_update_handle.Confirmation() - confirm.accept() - return confirm - - # TODO(AA): To check if the MiR fleet supports these missions names, before - # confirming. - # Configure this fleet to perform action category - if 'action_categories' in task_capabilities_config: - for cat in task_capabilities_config['action_categories']: - print(f"Fleet [{fleet_name}] is configured to perform action of category [{cat}]") - fleet.add_performable_action(cat, _consider) - - return adapter, fleet, fleet_name, robot_traits, nav_graph - -def create_robot_command_handles(config, handle_data, dry_run=False): - robots = {} - - for robot_name, robot_config in config['robots'].items(): - # CONFIGURE MIR ======================================================= - mir_config = robot_config['mir_config'] - rmf_config = robot_config['rmf_config'] - - prefix = mir_config['base_url'] - headers = {} - headers['Content-Type'] = mir_config['user'] - headers['Authorization'] = mir_config['password'] - - # CONFIGURE HANDLE ==================================================== - robot = MiRCommandHandle( - name=robot_name, - node=handle_data['node'], - rmf_graph=handle_data['graph'], - robot_traits=handle_data['robot_traits'], - robot_state_update_frequency=rmf_config.get('robot_state_update_frequency', 1), - dry_run=dry_run - ) - robot.mir_api = MirAPI(prefix,headers) - robot.transforms = handle_data['transforms'] - robot.rmf_map_name = rmf_config['start']['map_name'] - - if not dry_run: - # with MiRRetryContext(robot): - _mir_status = robot.mir_api.status_get() - robot.mir_name = _mir_status["robot_name"] - - robot.load_mir_missions() - robot.load_mir_positions() - - # Check that the MiR fleet has defined the RMF move mission, - # that this adapter will use repeatedly with varying parameters. - rmf_move_mission = mir_config['rmf_move_mission'] - assert rmf_move_mission in robot.mir_missions, \ - (f'RMF move mission [{rmf_move_mission}] not yet defined as a mission in MiR fleet') - robot.mir_rmf_move_mission = rmf_move_mission - - if 'dock_and_charge_mission' in mir_config: - dock_and_charge_mission = mir_config['dock_and_charge_mission'] - assert dock_and_charge_mission in robot.mir_missions, \ - (f'Dock and charge mission [{dock_and_charge_mission}] not yet defined as a mission in MiR fleet') - robot.mir_dock_and_charge_mission = dock_and_charge_mission + cmd_node.declare_parameter('server_uri', '') + server_uri = cmd_node.get_parameter( + 'server_uri' + ).get_parameter_value().string_value + if server_uri == '': + server_uri = None - else: - robot.mir_name = "DUMMY_ROBOT_FOR_DRY_RUN" + fleet_config.server_uri = server_uri + fleet_handle = adapter.add_easy_fleet(fleet_config) - robots[robot.name] = robot + conversions = config_yaml['conversions'] + update_frequency = config_yaml['rmf_fleet']['robot_state_update_frequency'] + debug = config_yaml['rmf_fleet']['debug'] - # OBTAIN PLAN STARTS ================================================== - start_config = rmf_config['start'] + event_loop = asyncio.new_event_loop() + + robots_handles = [] + for robot_name, rmf_config in fleet_config.known_robot_configurations.items(): + mir_config = config_yaml['rmf_fleet']['robots'][robot_name]['mir_config'] + robots_handles.append(RobotAdapterMiR( + robot_name, + rmf_config, + mir_config, + conversions, + fleet_handle, + fleet_config, + cmd_node, + event_loop, + debug, + )) + + return FleetAdapterMiR(cmd_node, adapter, fleet_handle, robots_handles, update_frequency, event_loop) - # If the plan start is configured, use it, otherwise, grab it - waypoint_index = start_config.get('waypoint_index') - orientation = start_config.get('orientation') - if (waypoint_index is not None) and (orientation is not None): - pprint(type(handle_data['adapter'])) - starts = [plan.Start(handle_data['adapter'].now(), - start_config.get('waypoint_index'), - start_config.get('orientation'))] - else: - starts = plan.compute_plan_starts( - handle_data['graph'], - start_config['map_name'], - robot.get_position(rmf=True, as_dimensions=True), - handle_data['adapter'].now(), - max_merge_waypoint_distance = start_config["max_merge_waypoint_distance"], - max_merge_lane_distance = start_config["max_merge_lane_distance"] - ) - assert starts, ("Robot %s can't be placed on the nav graph!" - % robot_name) - assert len(starts) != 0, (f'No StartSet found for robot: {robot_name}') - - # Insert start data into robot - start = starts[0] - - if start.lane is not None: # If the robot is in a lane - robot.rmf_current_lane_index = start.lane - robot.rmf_current_waypoint_index = None - robot.rmf_target_waypoint_index = None - else: # Otherwise, the robot is on a waypoint - robot.rmf_current_lane_index = None - robot.rmf_current_waypoint_index = start.waypoint - robot.rmf_target_waypoint_index = None - - print("MAP_NAME:", start_config['map_name']) - robot.rmf_map_name = start_config['map_name'] - - handle_data['fleet_handle'].add_robot( - robot, - robot.name, - handle_data['robot_traits'].profile, - starts, - lambda rmf_updater: robot.init_updater(rmf_updater)) - - handle_data['node'].get_logger().info( - f"successfully initialized robot {robot.name}" - f" (MiR name: {robot.mir_name})" - ) - return robots ############################################################################### # MAIN @@ -286,7 +169,7 @@ def main(argv=sys.argv): # INIT RCL ================================================================ rclpy.init(args=argv) - adpt.init_rclcpp() + rmf_adapter.init_rclcpp() args_without_ros = rclpy.utilities.remove_ros_args(argv) parser = argparse.ArgumentParser( @@ -310,90 +193,39 @@ def main(argv=sys.argv): config_path = args.config_file nav_graph_path = args.nav_graph - mock = args.mock - dry_run = args.dry_run # For testing - - if dry_run: - mock = True - - with open(config_path, "r") as f: - config = yaml.safe_load(f) - sanitise_dict(config, inplace=True, recursive=True) - - print("\n== Initialising MiR Robot Command Handles with Config ==") - pprint(config) - print() - - # INIT FLEET ============================================================== - adapter, fleet, fleet_name, robot_traits, nav_graph = create_fleet( - config,nav_graph_path, mock=mock) - - # INIT TRANSFORMS ========================================================= - rmf_coordinates = config['reference_coordinates']['rmf'] - mir_coordinates = config['reference_coordinates']['mir'] - transforms = compute_transforms(rmf_coordinates, mir_coordinates) - - # INIT ROBOT HANDLES ====================================================== - cmd_node = rclpy.node.Node(config['node_names']['robot_command_handle']) - - handle_data = { - 'fleet_handle': fleet, - 'fleet_name': fleet_name, - 'adapter': adapter, - 'node': cmd_node, - - 'graph': nav_graph, - 'robot_traits': robot_traits, - 'transforms': transforms - } - - robots = create_robot_command_handles(config, handle_data, dry_run=dry_run) - - # CREATE NODE EXECUTOR ==================================================== - rclpy_executor = rclpy.executors.SingleThreadedExecutor() - rclpy_executor.add_node(cmd_node) - - # INIT FLEET STATE PUB ==================================================== - if config['rmf_fleet']['publish_fleet_state']: - fleet_state_node = rclpy.node.Node( - config['node_names']['fleet_state_publisher']) - fleet_state_pub = fleet_state_node.create_publisher( - FleetState, - config['rmf_fleet']['fleet_state_topic'], - 1 - ) - rclpy_executor.add_node(fleet_state_node) + fleet_config = rmf_easy.FleetConfiguration.from_config_files( + config_path, nav_graph_path + ) + assert fleet_config, f'Failed to parse config file [{config_path}]' - def create_fleet_state_pub_fn(fleet_state_pub, fleet_name, robots): - def f(): - fleet_state = FleetState() - fleet_state.name = fleet_name + with open(config_path, 'r') as f: + config_yaml = yaml.safe_load(f) - for robot in robots.values(): - fleet_state.robots.append(robot.robot_state) + dry_run = args.dry_run # For testing - fleet_state_pub.publish(fleet_state) - return f + if dry_run: + print('Dry run finished') + # We don't have a mock adapter for the Easy API, so for now we should + # just exit as long as the config was parsed without an error. + # TODO(@mxgrey): Think of a meaningful way to do "dry runs" with the + # Easy API. + exit() - fleet_state_timer = fleet_state_node.create_timer( - config['rmf_fleet']['fleet_state_publish_frequency'], - create_fleet_state_pub_fn(fleet_state_pub, fleet_name, robots) - ) + sanitise_dict(config_yaml, inplace=True, recursive=True) - # GO! ===================================================================== - adapter.start() - rclpy_executor.spin() + print("\n== MiR Adapter Configuration ==") + pprint(config_yaml) + print() - # CLEANUP ================================================================= - cmd_node.destroy_node() + # Create a node to use inside of the Python code for logging + cmd_node = rclpy.node.Node(config_yaml['node_names']['robot_command_handle']) - if config['rmf_fleet']['publish_fleet_state']: - fleet_state_node.destroy_timer(fleet_state_timer) - fleet_state_node.destroy_node() + # Create the fleet, including the robots that are in the config file + fleet = create_fleet(fleet_config, config_yaml, cmd_node) - rclpy_executor.shutdown() - rclpy.shutdown() + # GO! + fleet.start() if __name__ == "__main__": diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py new file mode 100644 index 0000000..7d330cc --- /dev/null +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py @@ -0,0 +1,775 @@ +from collections import namedtuple +import copy +import enum +import math +import requests +import json +from icecream import ic +from urllib.error import HTTPError + +from rmf_adapter.easy_full_control import RobotState + +__all__ = [ + "MirAPI" +] + +MiRLocation = namedtuple("MiRLocation", ['x', 'y', 'yaw']) + + +class MiRStateCode(enum.IntEnum): + READY = 3 + PAUSE = 4 + EXECUTING = 5 + MANUAL_CONTROL = 11 + ERROR = 12 + + +class MiRPositionTypes(enum.IntEnum): + ROBOT = 0 + SHELF = 5 + CHARGING_STATION = 7 + CHARGING_STATION_ENTRY = 8 + CART = 22 + LIFT = 25 + LIFT_ENTRY = 26 + + +LocalizationParamPosition = 'position_estimate' + +class MapConversions: + def __init__(self, rmf_to_mir: dict): + self.rmf_to_mir = rmf_to_mir + self.mir_to_rmf = {v: k for k, v in rmf_to_mir.items()} + + +class MirStatus: + def __init__(self, response: dict, map_conversions: MapConversions, map_name: str): + self.response = response + p = response['position'] + self.state = RobotState( + map_conversions.mir_to_rmf[map_name], + [p['x'], p['y'], math.radians(p['orientation'])], + response['battery_percentage']/100, + ) + + +class MirAPI: + def __init__(self, prefix, headers, conversions, timeout=10.0, debug=False): + #HTTP connection + self.prefix = prefix + self.debug = False + self.headers = headers + self.timeout = timeout + self.connected = False + self.map_conversions = MapConversions(conversions['maps']) + self.known_missions = {} + self.known_positions = {} + self.known_maps = {} + self.mission_keys: dict = conversions['missions'] + self.mission_actions: dict = {} + self.mission_action_types: dict = {} + self.attempt_connection() + + def attempt_connection(self): + try: + requests.get(self.prefix + 'wifi/connections', headers=self.headers, timeout=self.timeout) + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + return + except Exception as err: + print(f"Other error: {err}") + return + + self.connected = True + + self.load_missions() + self.load_maps() + + move_key = 'move' + assert move_key in self.mission_keys, ( + f'{move_key} mission must be specified in fleet config file under ' + f'conversions -> missions' + ) + self.move_mission: str = self.mission_keys[move_key] + assert self.move_mission in self.known_missions, ( + f'RMF move mission [{self.move_mission}] has not yet been ' + f'defined as a mission in the MiR robot [{self.prefix}]' + ) + + dock_and_charge_key = 'dock_and_charge' + assert dock_and_charge_key in self.mission_keys, ( + f'{dock_and_charge_key} mission must be specified in fleet config ' + f'file under conversions -> missions' + ) + self.dock_and_charge_mission: str = self.mission_keys[dock_and_charge_key] + assert self.dock_and_charge_mission in self.known_missions, ( + f'RMF dock and charge mission [{self.dock_and_charge_mission}] ' + f'has not yet been defined as a mission in the MiR robot ' + f'[{self.prefix}]' + ) + + def make_optional_mission(key): + target_mission = None + if key in self.mission_keys: + target_mission = self.mission_keys[key] + assert target_mission in self.known_missions, ( + f'RMF {key} mission [{target_mission}] has not yet been ' + f'defined as a mission in the MiR robot [{self.prefix}]' + ) + return target_mission + + self.localize_mission: str | None = make_optional_mission('localize') + self.dock_to_cart_mission: str | None = make_optional_mission('dock_to_cart') + self.pickup_mission: str | None = make_optional_mission('pickup') + self.dropoff_mission: str | None = make_optional_mission('dropoff') + self.exit_mission: str | None = make_optional_mission('exit_lot') + self.footprint_mission: str | None = make_optional_mission('check_footprint') + self.go_to: str | None = make_optional_mission('go_to') + + if self.localize_mission is not None: + localize_actions = self.missions_mission_id_actions_get( + self.known_missions[self.localize_mission]['guid'] + ) + assert localize_actions is not None, ( + f'{self.localize_mission} mission does not have any actions' + ) + self.localize_params = None + for action in localize_actions: + if action.get('action_type') == 'switch_map': + self.localize_params = action['parameters'] + break + assert self.localize_params is not None, ( + f'No switch_map action in the mission {self.localize_mission}:\n' + f'{localize_actions}' + ) + found_position_estimate_param = False + for param in self.localize_params: + if param.get('input_name') == LocalizationParamPosition: + found_position_estimate_param = True + assert found_position_estimate_param, ( + f'No {LocalizationParamPosition} parameter found in the ' + f'mission {self.localize_mission}:\n{localize_actions}' + ) + + # Retrieve mission parameters + for mission, mission_data in self.known_missions.items(): + self.mission_actions[mission] = self.missions_mission_id_actions_get( + mission_data['guid']) + + self.created_by_id = self.me_get()['guid'] + + + def update_known_positions(self): + self.known_positions = {} + for pos in self.positions_get(): + if pos['name'] in self.known_positions and \ + pos['type_id'] == self.known_positions[pos['name']]['type_id'] and \ + ('rmf_localize' in pos['name'] or 'cgh_localize' in pos['name']): + # Delete any duplicate positions + self.positions_guid_delete(pos['guid']) + elif pos['type_id'] == MiRPositionTypes.ROBOT or \ + pos['type_id'] == MiRPositionTypes.SHELF or \ + pos['type_id'] == MiRPositionTypes.CHARGING_STATION or \ + pos['type_id'] == MiRPositionTypes.LIFT or \ + pos['type_id'] == MiRPositionTypes.LIFT_ENTRY: + self.known_positions[pos['name']] = ( + self.positions_guid_get(pos['guid']) + ) + + def create_rmf_missions(self, rmf_missions: dict): + rmf_mission_group_id = 'some_id' #TODO create a group id + for mission_name, mission_json in rmf_missions.items(): + if mission_name not in self.known_missions: + # Create the relevant mission on MiR + mission_id = self.missions_post(mission_name, rmf_mission_group_id) + # Fill in mission actions + self.create_rmf_mission_actions(mission_name, mission_id, mission_json) + + def create_rmf_mission_actions(self, mission_name: str, mission_id: str, mission_json: list[dict]): + mission_actions = [] + + for action in mission_json: + # Find schema for action type + action_type = action.get('action_type') + action_type_schema = self.mission_action_types.get(action_type) + if action_type_schema is None: + action_type_schema = self.action_type_get(action_type) + self.mission_action_types[action_type] = action_type_schema + + action_body_param = [] + id_tracker = [] + # First, add the parameters that we have customized for RMF + for param in action['parameters']: + id_tracker.append(param['id']) + param_body = {} + param_body['id'] = param['id'] + param_body['input_name'] = param['input_name'] + param_body['value'] = param['value'] + + # If the input type is a position, we would need a valid GUID for the value field + # Let's find a placeholder position GUID from the list of known positions + if param_body['id'] == 'position' or param_body['id'] == 'entry_position': + default_pos = list(self.known_positions.items())[0]['guid'] + param_body['value'] = default_pos + + action_body_param.append(param_body) + + # Next, add the default parameters that come with this action + for param in action_type_schema['parameters']: + # TODO: skip if this parameter has already been included in the json provided in rmf missions + # e.g. "position" in rmf_move_to_position + if param['id'] in id_tracker: + continue + id_tracker.append(param['id']) + + param_body = {} + param_body['id'] = param['id'] + param_body['input_name'] = param.get('input_name') + + default_value = None + if 'constraints' in param and 'default' in param['constraints']: + default_value = param['constraints']['default'] + param_body['value'] = default_value + action_body_param.append(param_body) + + action_body = {} + action_body['action_type'] = action_type + action_body['priority'] = action.get('priority') + action_body['parameters'] = action_body_param + + mission_actions.append(action_body) + + action_guid = self.missions_mission_id_actions_post(mission_id, mission_actions) + + def navigate(self, position): + pos_x = round(position[0], 3) + pos_y = round(position[1], 3) + ori_deg = position[2]*180/math.pi + mission_params = [ + {'id': 'X', 'value': pos_x, 'label': f'{pos_x}'}, + {'id': 'Y', 'value': pos_y, 'label': f'{pos_y}'}, + {'id': 'Orientation', 'value': ori_deg, 'label': f'{ori_deg:.3f}'} + ] + return self.queue_mission_by_name(self.move_mission, mission_params) + + def go_to_known_position(self, position_name): + if not position_name in self.known_positions: + return None + return self.dock(self.go_to, None, position_name) + + def dock(self, mission_name, start_waypoint, end_waypoint): + mission_params = None + + # Get parameters for start and end waypoints + end_wp = self.known_positions.get(end_waypoint) + if not end_wp: + self.update_known_positions() + end_wp = self.known_positions.get(end_waypoint) + assert end_wp is not None + end_wp_guid = end_wp.get('guid') + assert end_wp_guid is not None + end_param = self.get_mission_params_with_value(mission_name, 'move', 'end_waypoint', end_wp_guid) + dock_param = self.get_mission_params_with_value(mission_name, 'docking', 'end_waypoint', end_wp_guid) + + # Start waypoint is optional + if start_waypoint is not None: + start_wp = self.known_positions.get(start_waypoint) + if not start_wp: + self.update_known_positions() + start_wp = self.known_positions.get(start_waypoint) + assert start_wp is not None + start_wp_guid = start_wp.get('guid') + assert start_wp_guid is not None + start_param = self.get_mission_params_with_value(mission_name, 'move', 'start_waypoint', start_wp_guid) + mission_params = start_param + end_param + + # Check whether we should dock into this end waypoint or not (for charging) + elif dock_param: + mission_params = end_param + dock_param + else: + mission_params = end_param + return self.queue_mission_by_name(mission_name, mission_params) + + def localize(self, map, estimate, index): + if self.localize_mission is None: + raise Exception( + 'The fleet was not configured with a localize mission' + ) + + if index is not None: + position_name = f'cgh_1810_localize_{index}' + else: + p = estimate + position_name = f'cgh_1810_localize_{map}_{p[0]:.2f}_{p[1]:.2f}' + mir_map = self.map_conversions.rmf_to_mir[map] + map_id = self.known_maps[mir_map] + position_guid = self.get_position_guid(position_name, map_id, estimate) + if position_guid is None: + raise Exception('Unable to set a localization position on the MiR') + mission_params = copy.copy(self.localize_params) + for param in mission_params: + if param.get('input_name') == LocalizationParamPosition: + param['value'] = position_guid + + return self.queue_mission_by_name( + self.localize_mission, + mission_params=mission_params + ) + + + def get_position_guid(self, name, map_id, location): + attempts = 0 + max_attempts = 10 + while True: + attempts +=1 + if attempts >= max_attempts: + print( + f'Too many attempts [{max_attempts}] to set a localization ' + 'position.' + ) + return None + + # We keep cycling through these attempts until we can confirm that + # the MiR server has an acceptable position for us to use for + # localization + position = self.known_positions.get(name) + + def position_matches(pos): + if abs(pos['pos_x'] - location[0]) > 0.01: + return False + if abs(pos['pos_y'] - location[1]) > 0.01: + return False + if pos['map_id'] != map_id: + return False + return True + + if position is None: + # The position does not exist so we need to create a new one + position_guid = self.positions_post(name, map_id, location) + if position_guid is not None: + return position_guid + # For some reason posting the new position failed. Maybe there + # was a timeout or an argument error. We will update the known + # positions and retry this loop. + self.update_known_positions() + elif not position_matches(position): + if self.positions_put(position['guid'], name, map_id, location): + return position['guid'] + else: + return position['guid'] + + + def queue_mission_by_name(self, mission_name, mission_params=None): + mir_mission = self.known_missions.get(mission_name) + if mir_mission is None: + return None + mission_guid = mir_mission['guid'] + mission_description = None + if mission_params: + mission_description = { + 'mission_id': mission_guid, + 'message': 'string', + 'parameters': mission_params, + 'priority': 0, + 'description': 'string' + } + + response = self.mission_queue_post(mission_guid, mission_description) + if response is None or 'id' not in response: + return None + mission_queue_id = response['id'] + return mission_queue_id + + def positions_post(self, name, map_id, location): + data = { + 'created_by_id': self.created_by_id, + 'map_id': map_id, + 'name': name, + 'orientation': math.degrees(location[2]), + 'pos_x': location[0], + 'pos_y': location[1], + 'type_id': MiRPositionTypes.ROBOT + } + + try: + response = requests.post( + self.prefix + "positions", + headers=self.headers, + data=json.dumps(data), + timeout=self.timeout + ).json() + if response is None or 'guid' not in response: + return None + if self.debug: + print(f'Response: {response}') + self.known_positions[response['name']] = response + return response['guid'] + except Exception as err: + print(f'Position post failed: {err}') + + def positions_put(self, guid, name, map_id, location): + data = { + 'map_id': map_id, + 'name': name, + 'orientation': math.degrees(location[2]), + 'pos_x': location[0], + 'pos_y': location[1], + 'type_id': MiRPositionTypes.ROBOT + } + + try: + response = requests.put( + self.prefix + f'positions/{guid}', + headers=self.headers, + data=json.dumps(data), + timeout=self.timeout + ).json() + if self.debug: + print(f'Response: {response}') + self.known_positions[response['name']] = response + return response['guid'] + except: + pass + + def positions_delete(self): + new_known_positions = {} + for position in self.known_positions: + if 'rmf_localize' in position or 'cgh_localize' in position: + pos_guid = self.known_positions[position] + try: + response = requests.delete( + self.prefix + f'positions/{pos_guid}', + headers=self.headers, + timeout=self.timeout + ).json() + return response['guid'] + except: + pass + else: + new_known_positions[position] = copy.deepcopy(self.known_positions[position]) + self.known_positions = {} + self.known_positions = new_known_positions + + def positions_guid_delete(self, guid): + try: + response = requests.delete( + self.prefix + f'positions/{guid}', + headers=self.headers, + timeout=self.timeout + ).json() + if self.debug: + print(f'Response: {response}') + except Exception as err: + print(f'Failed to delete position guid [{guid}]: {err}') + + def status_get(self) -> MirStatus: + if not self.connected: + return + try: + response = requests.get(self.prefix + f'status', headers=self.headers, timeout=self.timeout) + # To prevent adapter crashing in case of error + if response.json() is None or 'position' not in response.json(): + return None + map_id = response.json()['map_id'] + map_name = None + for mname, mid in self.known_maps.items(): + if mid == map_id: + map_name = mname + break + + return MirStatus(response.json(), self.map_conversions, map_name) + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def load_missions(self): + robot_missions = self.missions_get() + for mission in robot_missions: + if 'move_coordinate' in mission['name']: + self.missions_guid_delete(mission['guid']) + else: + self.known_missions[mission['name']] = mission + + def load_maps(self): + robot_maps = self.maps_get() + for map in robot_maps: + self.known_maps[map['name']] = map['guid'] + + def me_get(self): + try: + response = requests.get(self.prefix + 'users/me', headers=self.headers, timeout=self.timeout) + if self.debug: + print(f'Response: {response.json()}') + return response.json() + except HTTPError as http_err: + print(f'HTTP error: {http_err}') + except Exception as err: + print(f'Other error: {err}') + + def actions_get(self): + if not self.connected: + return + try: + response = requests.get(self.prefix + 'actions', headers=self.headers, timeout=self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def action_type_get(self, action_type: str): + if not self.connected: + return + try: + response = requests.get(self.prefix + f'actions/{action_type}', headers=self.headers, timeout=self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def missions_get(self): + if not self.connected: + return + try: + response = requests.get(self.prefix + 'missions', headers=self.headers, timeout=self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def positions_get(self): + if not self.connected: + return + try: + response = requests.get(self.prefix + 'positions' , headers=self.headers, timeout=self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def mission_queue_get(self): + if not self.connected: + return + try: + response = requests.get(self.prefix + f'mission_queue', headers = self.headers, timeout=self.timeout) + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other here error: {err}") + + def mission_queue_id_get(self, mission_queue_id): + if not self.connected: + return + try: + response = requests.get(self.prefix + f'mission_queue/{mission_queue_id}', headers = self.headers, timeout=self.timeout) + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other here error: {err}") + + def mission_queue_post(self, mission_id, full_mission_description=None): + if not self.connected: + return + data = {'mission_id': mission_id} + if full_mission_description is not None: + # print(f'---------->>> {full_mission_description}') + data = full_mission_description + if mission_id != full_mission_description['mission_id']: + print(f'Inconsistent mission id, provided [{mission_id}], full_mission_description: [{full_mission_description}]') + return + + try: + response = requests.post(self.prefix + 'mission_queue' , headers = self.headers, data=json.dumps(data), timeout = self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other here error: {err}") + + def missions_mission_id_actions_post(self,mission_id,body): + if not self.connected: + return + try: + response = requests.post(self.prefix + 'missions/' + mission_id +'/actions' , headers = self.headers, data=json.dumps(body), timeout = self.timeout) + if self.debug: + print(f"Response: {response.json()}") + print(response.status_code) + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def missions_mission_id_actions_get(self, mission_id): + if not self.connected: + return + try: + response = requests.get(self.prefix + 'missions/' + str(mission_id) +'/actions' , headers = self.headers, timeout = self.timeout) + if self.debug: + print(f"Response: {response.json()}") + print(response.status_code) + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def missions_post(self, mission): + if not self.connected: + return + try: + response = requests.post(self.prefix + 'missions' , headers = self.headers, data=mission, timeout = self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def positions_guid_get(self, guid): + if not self.connected: + return + try: + response = requests.get(self.prefix + 'positions/'+ guid, headers=self.headers, timeout=self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def status_put(self, state_id): + if not self.connected: + return + data = {"state_id": state_id} + try: + response = requests.put(self.prefix + 'status', headers = self.headers, data=json.dumps(data), timeout=self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def clear_error(self): + if not self.connected: + return + data = {"clear_error": True} + try: + response = requests.put(self.prefix + 'status', headers = self.headers, data=json.dumps(data), timeout=self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def mission_queue_delete(self): + if not self.connected: + return + try: + response = requests.delete(self.prefix + 'mission_queue' , headers = self.headers, timeout = self.timeout) + if self.debug: + print(f"Response: {response.headers}") + return True + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + return False + except Exception as err: + print(f"Other error: {err}") + return False + + def mission_queue_id_delete(self, mission_queue_id): + if not self.connected: + return + try: + response = requests.delete( + self.prefix + 'mission_queue/' + str(mission_queue_id), + headers = self.headers, + timeout = self.timeout + ) + if self.debug: + print(f"Response: {response.headers}") + return True + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + return False + except Exception as err: + print(f"Other error: {err}") + return False + + def missions_guid_delete(self, guid): + if not self.connected: + return + try: + response = requests.delete(self.prefix + 'missions/' +guid , headers = self.headers, timeout = self.timeout) + if self.debug: + print(f"Response: {response.headers}") + return True + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + return False + except Exception as err: + print(f"Other error: {err}") + return False + + def maps_get(self): + if not self.connected: + return [] + try: + response = requests.get(self.prefix + 'maps', headers = self.headers, timeout = self.timeout) + if self.debug: + print(f"Response: {response.headers}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + return [] + except Exception as err: + print(f"Other error: {err}") + return [] + + def mission_completed(self, mission_queue_id): + mission_status = self.mission_queue_id_get(mission_queue_id) + if not mission_status: + return False + if 'finished' in mission_status and mission_status['finished'] is not None: + # if 'state' in mission_status and mission_status['state'] == 'Done': + return True + return False + + def get_mission_params_with_value(self, mission_name: str, action_type: str, param_name: str, value: str): + mission_actions = self.mission_actions.get(mission_name) + mission_params = None + for d in mission_actions: + if 'action_type' in d and d['action_type'] == action_type: + mission_params = d['parameters'] + for p in mission_params: + if 'input_name' in p and p['input_name'] == param_name: + p['value'] = value + # Let's only include this mission param to avoid error + mission_params = [p] + return mission_params + return mission_params + + def update_footprint(self): + return self.queue_mission_by_name(self.footprint_mission) diff --git a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_missions.json b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_missions.json new file mode 100644 index 0000000..b7d49ba --- /dev/null +++ b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_missions.json @@ -0,0 +1,559 @@ +{ + "rmf_exit_lot": [ + { + "priority": 1, + "parameters": [ + { + "value": "mirconst-guid-0000-0001-footprint000", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + }, + { + "priority": 2, + "parameters": [ + { + "value": -1.0, + "input_name": null, + "id": "x" + }, + { + "value": 0.0, + "input_name": null, + "id": "y" + }, + { + "value": 0.0, + "input_name": null, + "id": "orientation" + }, + { + "value": 0.25, + "input_name": null, + "id": "max_linear_speed" + }, + { + "value": 0.25, + "input_name": null, + "id": "max_angular_speed" + }, + { + "value": true, + "input_name": null, + "id": "collision_detection" + } + ], + "action_type": "relative_move" + }, + { + "priority": 3, + "parameters": [ + { + "value": "plc_register", + "input_name": null, + "id": "compare" + }, + { + "value": "e2fc134a-56bb-11ee-a409-00012998f454", + "input_name": null, + "id": "module" + }, + { + "value": "0", + "input_name": null, + "id": "io_port" + }, + { + "value": "8", + "input_name": null, + "id": "register" + }, + { + "value": "==", + "input_name": null, + "id": "operator" + }, + { + "value": 1.0, + "input_name": null, + "id": "value" + }, + { + "value": "", + "input_name": null, + "id": "true" + }, + { + "value": "", + "input_name": null, + "id": "false" + } + ], + "action_type": "if" + }, + { + "priority": 4, + "parameters": [ + { + "value": "f65abbec-ea4e-11ed-af33-00012983ef2c", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + }, + { + "priority": 5, + "parameters": [ + { + "value": "mirconst-guid-0000-0001-footprint000", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + } + ], + "rmf_dock_to_cart": [ + { + "priority": 1, + "parameters": [ + { + "value": "mirconst-guid-0000-0001-footprint000", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + }, + { + "priority": 2, + "parameters": [ + { + "value": null, + "input_name": "cart_marker", + "id": "marker" + }, + { + "value": "cbd0de8c-467c-11ee-899f-00012983ef2c", + "input_name": null, + "id": "marker_type" + }, + { + "value": "10", + "input_name": null, + "id": "retries" + }, + { + "value": 0.25, + "input_name": null, + "id": "max_linear_speed" + } + ], + "action_type": "docking" + } + ], + "rmf_dropoff_cart": [ + { + "priority": 2, + "parameters": [ + { + "value": "mirconst-guid-0000-0001-footprint000", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + }, + { + "priority": 3, + "parameters": [ + { + "value": -1.0, + "input_name": null, + "id": "x" + }, + { + "value": 0.0, + "input_name": null, + "id": "y" + }, + { + "value": 0.0, + "input_name": null, + "id": "orientation" + }, + { + "value": 0.25, + "input_name": null, + "id": "max_linear_speed" + }, + { + "value": 0.25, + "input_name": null, + "id": "max_angular_speed" + }, + { + "value": true, + "input_name": null, + "id": "collision_detection" + } + ], + "action_type": "relative_move" + }, + { + "priority": 1, + "parameters": [ + { + "value": "mirconst-guid-0000-0011-actionlist00", + "input_name": null, + "id": "mission_id" + } + ], + "action_type": "load_mission" + } + ], + "rmf_follow_line": [ + { + "priority": 1, + "parameters": [], + "action_type": "adjust_localization" + }, + { + "priority": 2, + "parameters": [ + { + "value": "plc_register", + "input_name": null, + "id": "compare" + }, + { + "value": "e2fc134a-56bb-11ee-a409-00012998f454", + "input_name": null, + "id": "module" + }, + { + "value": "0", + "input_name": null, + "id": "io_port" + }, + { + "value": "8", + "input_name": null, + "id": "register" + }, + { + "value": "==", + "input_name": null, + "id": "operator" + }, + { + "value": 1.0, + "input_name": null, + "id": "value" + }, + { + "value": "", + "input_name": null, + "id": "true" + }, + { + "value": "", + "input_name": null, + "id": "false" + } + ], + "action_type": "if" + }, + { + "priority": 3, + "parameters": [ + { + "value": "f65abbec-ea4e-11ed-af33-00012983ef2c", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + }, + { + "priority": 4, + "parameters": [ + { + "value": "mirconst-guid-0000-0001-footprint000", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + }, + { + "priority": 5, + "parameters": [ + { + "value": "to be replaced with a value guid in the fleet adapter", + "input_name": "start_waypoint", + "id": "position" + }, + { + "value": 10, + "input_name": null, + "id": "retries" + }, + { + "value": 0.2, + "input_name": null, + "id": "distance_threshold" + } + ], + "action_type": "move" + }, + { + "priority": 6, + "parameters": [ + { + "value": "plc_register", + "input_name": null, + "id": "compare" + }, + { + "value": "e2fc134a-56bb-11ee-a409-00012998f454", + "input_name": null, + "id": "module" + }, + { + "value": "0", + "input_name": null, + "id": "io_port" + }, + { + "value": "8", + "input_name": null, + "id": "register" + }, + { + "value": "==", + "input_name": null, + "id": "operator" + }, + { + "value": 1.0, + "input_name": null, + "id": "value" + }, + { + "value": "", + "input_name": null, + "id": "true" + }, + { + "value": "", + "input_name": null, + "id": "false" + } + ], + "action_type": "if" + }, + { + "priority": 7, + "parameters": [ + { + "value": "454e65f9-06b9-11ee-acbc-00012983ef2c", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + }, + { + "priority": 8, + "parameters": [ + { + "value": "mirconst-guid-0000-0001-footprint000", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + }, + { + "priority": 9, + "parameters": [ + { + "value": "to be replaced with a value guid in the fleet adapter", + "input_name": "end_waypoint", + "id": "position" + }, + { + "value": 10, + "input_name": null, + "id": "retries" + }, + { + "value": 0.2, + "input_name": null, + "id": "distance_threshold" + } + ], + "action_type": "move" + }, + { + "priority": 10, + "parameters": [], + "action_type": "adjust_localization" + } + ], + "rmf_pickup_cart": [ + { + "priority": 1, + "parameters": [ + { + "value": "13", + "input_name": null, + "id": "register" + }, + { + "value": "set", + "input_name": null, + "id": "action" + }, + { + "value": 0.0, + "input_name": null, + "id": "value" + } + ], + "action_type": "set_plc_register" + }, + { + "priority": 2, + "parameters": [ + { + "value": "14", + "input_name": null, + "id": "register" + }, + { + "value": 1.0, + "input_name": null, + "id": "value" + }, + { + "value": 0.0, + "input_name": null, + "id": "reset_value" + }, + { + "value": "", + "input_name": null, + "id": "content" + } + ], + "action_type": "set_reset_plc" + }, + { + "priority": 3, + "parameters": [ + { + "value": "8", + "input_name": null, + "id": "register" + }, + { + "value": 1.0, + "input_name": null, + "id": "value" + }, + { + "value": "00:01:00.000000", + "input_name": null, + "id": "timeout" + } + ], + "action_type": "wait_for_plc_register" + }, + { + "priority": 4, + "parameters": [ + { + "value": "f65abbec-ea4e-11ed-af33-00012983ef2c", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + } + ], + "rmf_update_footprint": [ + { + "priority": 1, + "parameters": [ + { + "value": "plc_register", + "input_name": null, + "id": "compare" + }, + { + "value": "e2fc134a-56bb-11ee-a409-00012998f454", + "input_name": null, + "id": "module" + }, + { + "value": "0", + "input_name": null, + "id": "io_port" + }, + { + "value": "8", + "input_name": null, + "id": "register" + }, + { + "value": "==", + "input_name": null, + "id": "operator" + }, + { + "value": 1.0, + "input_name": null, + "id": "value" + }, + { + "value": "", + "input_name": null, + "id": "true" + }, + { + "value": "", + "input_name": null, + "id": "false" + } + ], + "action_type": "if" + }, + { + "priority": 2, + "parameters": [ + { + "value": "f65abbec-ea4e-11ed-af33-00012983ef2c", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + }, + { + "priority": 3, + "parameters": [ + { + "value": "mirconst-guid-0000-0001-footprint000", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + } + ] +} \ No newline at end of file diff --git a/fleet_adapter_mir/fleet_adapter_mir/rmf_missions.json b/fleet_adapter_mir/fleet_adapter_mir/rmf_missions.json new file mode 100644 index 0000000..9ba25b3 --- /dev/null +++ b/fleet_adapter_mir/fleet_adapter_mir/rmf_missions.json @@ -0,0 +1,215 @@ +{ + "rmf_dock_and_charge": [ + { + "priority": 1, + "parameters": [ + { + "value": "to be replaced with a value guid in the fleet adapter", + "input_name": "end_waypoint", + "id": "position" + }, + { + "value": 10, + "input_name": null, + "id": "retries" + }, + { + "value": 0.2, + "input_name": null, + "id": "distance_threshold" + } + ], + "action_type": "move" + }, + { + "priority": 2, + "parameters": [ + { + "value": null, + "input_name": "end_waypoint", + "id": "marker" + }, + { + "value": "mirconst-guid-0000-0001-marker000001", + "input_name": null, + "id": "marker_type" + }, + { + "value": "10", + "input_name": null, + "id": "retries" + }, + { + "value": 0.25, + "input_name": null, + "id": "max_linear_speed" + } + ], + "action_type": "docking" + }, + { + "priority": 3, + "parameters": [ + { + "value": "00:10:00.000000", + "input_name": null, + "id": "minimum_time" + }, + { + "value": 25.0, + "input_name": null, + "id": "minimum_percentage" + }, + { + "value": true, + "input_name": null, + "id": "charge_until_new_mission" + } + ], + "action_type": "charging" + } + ], + "rmf_follow_line": [ + { + "priority": 1, + "parameters": [], + "action_type": "adjust_localization" + }, + { + "priority": 2, + "parameters": [ + { + "value": "to be replaced with a value guid in the fleet adapter", + "input_name": "start_waypoint", + "id": "position" + }, + { + "value": 10, + "input_name": null, + "id": "retries" + }, + { + "value": 0.2, + "input_name": null, + "id": "distance_threshold" + } + ], + "action_type": "move" + }, + { + "priority": 3, + "parameters": [ + { + "value": "to be replaced with a value guid in the fleet adapter", + "input_name": "end_waypoint", + "id": "position" + }, + { + "value": 10, + "input_name": null, + "id": "retries" + }, + { + "value": 0.2, + "input_name": null, + "id": "distance_threshold" + } + ], + "action_type": "move" + }, + { + "priority": 4, + "parameters": [], + "action_type": "adjust_localization" + } + ], + "rmf_localize": [ + { + "priority": 1, + "parameters": [], + "action_type": "adjust_localization" + }, + { + "priority": 2, + "parameters": [ + { + "value": "to be replaced with a value guid in the fleet adapter", + "input_name": "position_estimate", + "id": "entry_position" + } + ], + "action_type": "switch_map" + }, + { + "priority": 3, + "parameters": [], + "action_type": "adjust_localization" + }, + { + "priority": 4, + "parameters": [], + "action_type": "adjust_localization" + }, + { + "priority": 5, + "parameters": [], + "action_type": "adjust_localization" + } + ], + "rmf_move": [ + { + "priority": 1, + "parameters": [ + { + "value": 0.0, + "input_name": "X", + "id": "x" + }, + { + "value": 0.0, + "input_name": "Y", + "id": "y" + }, + { + "value": 0.0, + "input_name": "Orientation", + "id": "orientation" + }, + { + "value": 10, + "input_name": null, + "id": "retries" + }, + { + "value": 0.2, + "input_name": null, + "id": "distance_threshold" + } + ], + "action_type": "move_to_position" + } + ], + "rmf_move_to_position": [ + { + "action_type": "move", + "priority": 1, + "parameters": [ + { + "value": "to be replaced with a value guid in the fleet adapter", + "input_name": "end_waypoint", + "id": "position" + }, + { + "value": 10, + "input_name": null, + "id": "retries" + }, + { + "value": 0.2, + "input_name": null, + "id": "distance_threshold" + } + ] + } + ] +} \ No newline at end of file diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py new file mode 100644 index 0000000..ea5ad7b --- /dev/null +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -0,0 +1,470 @@ +import math +import time +from typing import Any +from dataclasses import dataclass +import json +from rmf_adapter.robot_update_handle import Tier +import rmf_adapter.easy_full_control as rmf_easy +import rclpy +import rclpy.node as Node +from rclpy.duration import Duration +from .mir_api import MirAPI, MirStatus, MiRStateCode +from threading import Lock + + +# Parallel processing solution derived from +# https://stackoverflow.com/a/59385935 +def parallel(f): + def run_in_parallel(*args, **kwargs): + + return args[0].event_loop.run_in_executor( + None, f, *args, **kwargs + ) + + return run_in_parallel + + +class MissionHandle: + def __init__(self, execution, docking=False, localize=False, charger=None): + self.execution = execution + self.docking = docking + self.localize = localize + self.charger = charger + self.done = False + # mission_queue_id gets set asynchronously + self.mission_queue_id = None + self.do_not_cancel = False + self.mutex = Lock() + # Block before beginning the request to guarantee that a call to stop() + # cannot possibly lock it first + self.mutex.acquire(blocking=True) + + def set_mission_queue_id(self, mission_queue_id): + self.mission_queue_id = mission_queue_id + self.mutex.release() + + @property + def activity(self): + # Move the execution reference into a separate variable just in case + # another thread modifies self.execution while we're still using it. + execution = self.execution + if execution is not None and not self.done: + return execution.identifier + return None + +class RobotAdapterMiR: + def __init__( + self, + name: str, + rmf_config: rmf_easy.RobotConfiguration, + mir_config: dict, + conversions: dict, + fleet_handle, + fleet_config, + node: Node, + event_loop, + debug=False + ): + self.name = name + self.node = node + self.mission: MissionHandle | None = None + self.event_loop = event_loop + self.debug = debug + + self.disconnect = False + + prefix = mir_config['base_url'] + headers = { + 'Content-Type': mir_config['user'], + 'Authorization': mir_config['password'], + } + self.api: MirAPI = MirAPI(prefix, headers, conversions) + + status = self.api.status_get() + while self.api.status_get() is None: + status = self.api.status_get() + time.sleep(0.1) + self.last_known_status: MirStatus = status + self.current_map = status.state.map + self.fleet_config = fleet_config + + self.api.update_known_positions() + self.api.positions_delete() + + # This must be done after the API is made + self.lift_positions: dict = conversions.get('lift_positions', {}) + for lift, levels in self.lift_positions.items(): + for level, position in levels.items(): + position_name = position.get('name') + assert position_name is not None, ( + f'Missing "name" field for level [{level}] in lift ' + f'[{lift}] for lift_positions in configuration file' + ) + known = self.api.known_positions.get(position_name) + assert known is not None, ( + f'MiR is missing the position [{position_name}] needed as ' + f'a lift position on level [{level}] for lift [{lift}]' + ) + self.nav_issue_ticket = None # We should only have one issue ticket at a time to manage unsuccessful missions + self.requested_replan = False + self.replan_counts = 0 + + self.fleet_handle = fleet_handle + self.update_handle = fleet_handle.add_robot( + self.name, + status.state, + rmf_config, + self._make_callbacks(), + ) + + @property + def activity(self): + # Move the mission reference into a separate variable just in case + # another thread modifies self.mission while we're still using it. + mission = self.mission + if mission is not None: + return self.mission.activity + return None + + async def update_loop(self, period): + while rclpy.ok(): + now = self.node.get_clock().now() + next_wakeup = now + Duration(nanoseconds = period * 1e9) + await self.request_update() + while self.node.get_clock().now() < next_wakeup: + time.sleep(0.01) + + @parallel + def request_update(self): + # Retrieve the latest MiR status from robot + status = self.api.status_get() + if status is None: + self.disconnect = True + self.node.get_logger().warn(f'Unable to retrieve status from robot [{self.name}]!') + return + if self.disconnect: + self.node.get_logger().info(f'Robot [{self.name}] connectivity resumed, received status from robot successfully.') + self.disconnect = False + more = self.update_handle.more() + if more is not None: + more.unstable_debug_positions(self.debug) + + # Update the stored mission status from MiR + mission = self.mission + self.update_mission_status(status, mission) + + # Update RMF with the latest RobotState + if mission is None or (mission.localize and mission.done) or not mission.localize: + self.update_handle.update(status.state, self.activity) + self.last_known_status = status + else: + self.node.get_logger().info( + f'Mission is None / Robot is localizing, ignore status update') + + # Update RMF to mark the ActionExecution as finished + if mission is not None and mission.done: + self.update_rmf_finished(mission) + + # Clear error on updates + if status is not None and 'errors' in status.response and len(status.response['errors']) > 0: + self.api.clear_error() + if status is not None and (status.response['state_id'] == MiRStateCode.PAUSE or status.response['state_id'] == MiRStateCode.ERROR): + self.api.status_put(MiRStateCode.READY) + + def is_charging(self, status: MirStatus): + # TODO(XY): check if we can verify that a robot is charging via mode_id instead + if status.response.get('mission_text') == "Charging... Waiting for new mission...": + return True + return False + + def update_rmf_finished(self, mission: MissionHandle): + if not mission.execution: + return + mission.execution.finished() + mission.execution = None + + def update_mission_status(self, status: MirStatus, mission: MissionHandle): + if mission is None or mission.mission_queue_id is None: + # Either we don't have a mission or we don't know the + # mission_queue_id yet so just continue as normal for now + return + + if mission.execution is None: + # This mission is already finished, so we return early + return + + mission_status = self.api.mission_queue_id_get(mission.mission_queue_id) + executing_mission = status.response.get('mission_queue_id') + if executing_mission == mission.mission_queue_id: + # The current mission is still being executed, so we don't + # need to change anything + if mission.charger is not None and self.is_charging(status): + self.node.get_logger().info(f'Robot [{self.name}] has begun charging...') + mission.docking = False + mission.done = True + return + + # The mission is not being executed according to the status, so either + # the mission has finished or it has not started yet. We need a second + # API call to figure out which it is. + if mission_status is None or mission_status.get('state') is None: + # It shouldn't come to here, but we'll prevent any potential crashes + # with a check here + return + if mission_status['state'] == 'Done': + # The mission has finished so let's trigger execution.finished() and + # then clear it out + self.node.get_logger().info(f'MiR [{self.name}] has completed mission {mission.mission_queue_id}, ' + f'marking ActionExecution object as finished.') + mission.docking = False + mission.done = True + # If the mission previously issued a ticket, let's resolve that ticket + if self.nav_issue_ticket is not None: + msg = {} + self.nav_issue_ticket.resolve(msg) + self.nav_issue_ticket = None + self.node.get_logger().info(f'Issue ticket is resolved!') + self.replan_counts = 0 + elif mission_status['state'] == 'Aborted': + # The robot is unable to perform the mission for some reason, so we + # raise an issue and re-attempt the mission. + tier = Tier.Error + category = mission_status['state'] + detail = { + 'mission_queue_id': mission.mission_queue_id, + 'message': 'Mission has been aborted.' + } + # Save the issue ticket somewhere so that we can resolve it later + self.nav_issue_ticket = self.update_handle.more().create_issue(tier, category, detail) + self.node.get_logger().info(f'Created [{category}] issue ticket for mission queue ID [{mission.mission_queue_id}]') + + # After issuing a ticket, let's request a replan + mission.done = True + self.update_handle.more().replan() + # We keep track of the number of times we are replanning for the same mission + self.replan_counts += 1 + self.node.get_logger().info(f'[{self.name}] Replan count: {self.replan_counts}') + + # Ensure that robot is charging + if mission.done and mission.charger is not None and not self.is_charging(status): + # Charging mission is marked as done but robot is not charging. Let's send + # the robot to the charger again. + mission.done = False + self.update_handle.more().replan() + self.replan_counts += 1 + self.node.get_logger().info(f'[{self.name}] Replan count: {self.replan_counts}') + + def _make_callbacks(self): + callbacks = rmf_easy.RobotCallbacks( + lambda destination, execution: self.navigate( + destination, execution + ), + lambda activity: self.stop(activity), + lambda category, description, execution: self.perform_action( + category, description, execution + ) + ) + + callbacks.localize = lambda estimate, execution: self.localize( + estimate, execution + ) + + return callbacks + + def navigate(self, destination, execution): + # If this is a cancellation behavior command, we check if we're meant to ignore it. + if self.ignore_action: + self.node.get_logger().info(f'Ignoring navigation command, ignore action flag is ' + f'raised after checking latch.') + execution.finished() + return + + # If the nav command coming in is to bring the robot to a charger, but the robot is + # already charging at the same charger, we ignore this nav command + status = self.last_known_status + if status and self.is_charging(status): + ignore_charging_cmd = False + mission = self.mission + # If the robot's previous mission was a charging mission to the same charger + if mission and mission.charger is not None and destination.name == mission.charger: + ignore_charging_cmd = True + # If the robot was already charging at the same charger before any missions were + # sent/stored + elif destination.map == self.current_map and \ + (self.dist(destination.position, status.state.position) < + self.update_handle.max_merge_waypoint_distance()): + ignore_charging_cmd = True + + if ignore_charging_cmd: + self.node.get_logger().info(f'[{self.name}] Received navigation command to dock to ' + f'charger {destination.name} when robot is already charging ' + f'at the same charger, ignoring command and marking it as ' + f'finished.') + execution.finished() + return + + # If the robot already has a mission, cancel it + self.node.get_logger().info(f'[{self.name}] Calling stop for new navigation command.') + self.request_stop(self.mission) + + # If this is a docking task, send the appropriate docking mission + if destination.dock is not None: + dock_json = json.loads(destination.dock) + if dock_json.get('mission_name') == self.api.dock_and_charge_mission: + self.mission = MissionHandle(execution, charger=destination.name) + else: + self.mission = MissionHandle(execution) + self.request_dock(dock_json, self.mission) + return + # TODO(XY): remove this hack and make sure dock missions are passed properly to + # the nav command + elif destination.name and 'Charger' in destination.name: + self.mission = MissionHandle(execution, charger=destination.name) + dock_json = {'description': {'end_waypoint': destination.name}, + 'mission_name': self.api.dock_and_charge_mission} + self.request_dock(dock_json, self.mission) + + if destination.inside_lift is not None: + positions_for_lift = self.lift_positions.get(destination.inside_lift.name) + if positions_for_lift is not None: + position_info = positions_for_lift.get(destination.map) + if position_info is not None: + position_name = position_info['name'] + mir_pos = self.api.known_positions.get(position_name) + if mir_pos is not None: + p_x = mir_pos['pos_x'] + p_y = mir_pos['pos_y'] + r = self.last_known_status.response['position'] + dx = p_x - r['x'] + dy = p_y - r['y'] + dist = math.sqrt(dx*dx + dy*dy) + tolerance = position_info.get('tolerance', 0.3) + if dist < tolerance: + # We are already close enough to the point, so we + # will just have the robot stop and tell RMF to go + # ahead. + self.node.get_logger().info(f'[{self.name}] Calling stop in the lift.') + self.request_stop(self.mission) + self.mission = None + execution.finished() + return + # We are inside the lift but not close enough to the + # desired center point, so we will ask the robot to + # simply move there + self.mission = MissionHandle(execution) + self.request_go_to_known_position(position_name, self.mission) + return + + # If this is a move to task, send rmf_move mission + self.mission = MissionHandle(execution) + self.request_navigate(destination, self.mission) + + @parallel + def request_navigate( + self, + destination, + mission_handle: MissionHandle + ): + mission_queue_id = None + if destination.name: + self.node.get_logger().info(f'[{self.name}] is going to MiR position {destination.name}') + mission_queue_id = self.api.go_to_known_position(destination.name) + if mission_queue_id is None: + self.node.get_logger().info(f'[{self.name}] is going to MiR coordinates [{destination.position}]') + mission_queue_id = self.api.navigate(destination.position) + mission_handle.set_mission_queue_id(mission_queue_id) + + @parallel + def request_go_to_known_position( + self, + position_name: str, + mission_handle: MissionHandle + ): + mission_queue_id = self.api.go_to_known_position(position_name) + mission_handle.set_mission_queue_id(mission_queue_id) + + + @parallel + def request_dock( + self, + docking_points, + mission_handle: MissionHandle + ): + if not ('description' in docking_points): + return + self.node.get_logger().info(f'Requested dock mission for [{self.name}]: {docking_points}') + end_waypoint = docking_points['description']['end_waypoint'] + if 'start_waypoint' in docking_points['description']: + start_waypoint = docking_points['description']['start_waypoint'] + else: + start_waypoint = None + mission_name = docking_points['mission_name'] + + mission_queue_id = self.api.dock(mission_name, start_waypoint, end_waypoint) + mission_handle.set_mission_queue_id(mission_queue_id) + + def stop(self, activity): + mission = self.mission + if mission is not None: + if mission.docking: + self.node.get_logger().info(f'Robot is performing docking mission, ignoring stop issued by RMF') + return + if mission.execution is not None and activity.is_same(mission.execution.identifier): + self.node.get_logger().info(f'[{self.name}] Stop requested from RMF!') + self.request_stop(mission) + self.mission = None + # Before replan, let's make sure the robot footprint is set correctly + self.api.update_footprint() + + @parallel + def request_stop(self, mission: MissionHandle): + if mission is not None: + with mission.mutex: + if mission.mission_queue_id is not None and not mission.do_not_cancel: + self.api.mission_queue_id_delete(mission.mission_queue_id) + + def localize(self, estimate, execution): + self.mission = MissionHandle(execution, localize=True) + self.mission.do_not_cancel = True + self.request_localize(estimate, self.mission) + + @parallel + def request_localize(self, estimate, mission: MissionHandle): + count = 0 + mission_queue_id = None + while count < self.retries and not mission_queue_id: + count += 1 + try: + mission_queue_id = self.api.localize( + estimate.map, estimate.position, estimate.graph_index + ) + if mission_queue_id is not None: + self.node.get_logger().info(f'Localize mission is successfully queued!') + break + except Exception as err: + self.node.get_logger().info( + f'Failed to localize on map {estimate.map}: {err}. Retrying...' + ) + time.sleep(1) + + if not mission_queue_id: + self.node.get_logger().info( + f'Failed to localize on map {estimate.map}. Maximum localize retries exceeded!' + ) + mission.execution.finished() + mission_queue_id = None + return + + mission.set_mission_queue_id(mission_queue_id) + + def perform_action(self, category, description, execution): + task_id = self.update_handle.more().current_task_id() + # ----------------------------------------------------- + # INSERT PERFORM ACTION LOGIC HERE + # ----------------------------------------------------- + raise NotImplementedError + + def dist(self, A, B): + assert(len(A) > 1) + assert(len(B) > 1) + return math.sqrt((A[0] - B[0])**2 + (A[1] - B[1])**2) From 118be994832f5abd964b8e2369d76fef6aeb2e11 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 10 Jan 2024 10:45:01 +0800 Subject: [PATCH 02/50] Create standard RMF missions from fleet adapter Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/fleet_adapter_mir.py | 17 +- .../fleet_adapter_mir/mir_api.py | 148 +++++++++++++++--- .../fleet_adapter_mir/robot_adapter_mir.py | 5 +- 3 files changed, 145 insertions(+), 25 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py index f772154..92a96b4 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py @@ -1,6 +1,7 @@ import asyncio import sys import yaml +import json import nudged import argparse import time @@ -117,7 +118,7 @@ def start(self): rclpy.shutdown() -def create_fleet(fleet_config, config_yaml, cmd_node) -> FleetAdapterMiR: +def create_fleet(fleet_config, config_yaml, cmd_node, rmf_missions) -> FleetAdapterMiR: """Create RMF Adapter and fleet handle""" for level, coords in config_yaml['conversions']['reference_coordinates'].items(): tf = compute_transforms(level, coords, cmd_node) @@ -152,6 +153,7 @@ def create_fleet(fleet_config, config_yaml, cmd_node) -> FleetAdapterMiR: rmf_config, mir_config, conversions, + rmf_missions, fleet_handle, fleet_config, cmd_node, @@ -173,7 +175,7 @@ def main(argv=sys.argv): args_without_ros = rclpy.utilities.remove_ros_args(argv) parser = argparse.ArgumentParser( - prog="fleet_adapter_mir", + prog="cgh_fleet_adapter_mir", description="Configure and spin up fleet adapters for MiR 100 robots " "that interface between the " "MiR REST API, ROS2, and rmf_core!") @@ -181,6 +183,8 @@ def main(argv=sys.argv): help="Input config.yaml file to process") parser.add_argument("-n", "--nav_graph", type=str, required=True, help="Path to the nav_graph for this fleet adapter") + parser.add_argument("-a", "--actions", type=str, required=False, default='', + help="Path to the RMF mission actions to be created") parser.add_argument("-m", "--mock", action='store_true', help="Init a mock adapter instead " "(does not require a schedule node, " @@ -193,6 +197,7 @@ def main(argv=sys.argv): config_path = args.config_file nav_graph_path = args.nav_graph + actions_path = args.actions fleet_config = rmf_easy.FleetConfiguration.from_config_files( config_path, nav_graph_path @@ -202,6 +207,12 @@ def main(argv=sys.argv): with open(config_path, 'r') as f: config_yaml = yaml.safe_load(f) + if actions_path == '': + rmf_missions = None + else: + with open(actions_path, 'r') as g: + rmf_missions = json.load(g) + dry_run = args.dry_run # For testing if dry_run: @@ -222,7 +233,7 @@ def main(argv=sys.argv): cmd_node = rclpy.node.Node(config_yaml['node_names']['robot_command_handle']) # Create the fleet, including the robots that are in the config file - fleet = create_fleet(fleet_config, config_yaml, cmd_node) + fleet = create_fleet(fleet_config, config_yaml, cmd_node, rmf_missions) # GO! fleet.start() diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py index 7d330cc..da0e22f 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py @@ -54,13 +54,14 @@ def __init__(self, response: dict, map_conversions: MapConversions, map_name: st class MirAPI: - def __init__(self, prefix, headers, conversions, timeout=10.0, debug=False): + def __init__(self, prefix, headers, conversions, rmf_missions, timeout=10.0, debug=False): #HTTP connection self.prefix = prefix self.debug = False self.headers = headers self.timeout = timeout self.connected = False + self.rmf_missions = rmf_missions self.map_conversions = MapConversions(conversions['maps']) self.known_missions = {} self.known_positions = {} @@ -68,6 +69,7 @@ def __init__(self, prefix, headers, conversions, timeout=10.0, debug=False): self.mission_keys: dict = conversions['missions'] self.mission_actions: dict = {} self.mission_action_types: dict = {} + self.footprint_keys: dict = conversions['footprints'] self.attempt_connection() def attempt_connection(self): @@ -84,6 +86,7 @@ def attempt_connection(self): self.load_missions() self.load_maps() + self.create_rmf_missions() move_key = 'move' assert move_key in self.mission_keys, ( @@ -158,6 +161,9 @@ def make_optional_mission(key): self.created_by_id = self.me_get()['guid'] + # Set footprints + self.footprint = self.footprints_guid_get(self.footprint_keys['robot']) + self.update_footprint() def update_known_positions(self): self.known_positions = {} @@ -176,18 +182,33 @@ def update_known_positions(self): self.positions_guid_get(pos['guid']) ) - def create_rmf_missions(self, rmf_missions: dict): - rmf_mission_group_id = 'some_id' #TODO create a group id - for mission_name, mission_json in rmf_missions.items(): + def create_rmf_missions(self): + if self.rmf_missions is None: + return + + # Retrieve the RMF group id if it already exists + mission_groups = self.mission_groups_get() + rmf_mission_group_id = None + mission_group_name = 'RMF' + for group in mission_groups: + if group['name'] == mission_group_name: + rmf_mission_group_id = group['guid'] + break + # If the RMF mission group doesn't exist, create one + if rmf_mission_group_id is None: + mission_group = self.mission_groups_post(mission_group_name) + rmf_mission_group_id = mission_group['guid'] + + # Create RMF missions if they don't exist on the robot + for mission_name, mission_json in self.rmf_missions.items(): if mission_name not in self.known_missions: # Create the relevant mission on MiR - mission_id = self.missions_post(mission_name, rmf_mission_group_id) + mission = self.missions_post(mission_name, rmf_mission_group_id) + self.known_missions[mission['name']] = mission # Fill in mission actions - self.create_rmf_mission_actions(mission_name, mission_id, mission_json) + self.create_rmf_mission_actions(mission_name, mission['guid'], mission_json) def create_rmf_mission_actions(self, mission_name: str, mission_id: str, mission_json: list[dict]): - mission_actions = [] - for action in mission_json: # Find schema for action type action_type = action.get('action_type') @@ -209,15 +230,18 @@ def create_rmf_mission_actions(self, mission_name: str, mission_id: str, mission # If the input type is a position, we would need a valid GUID for the value field # Let's find a placeholder position GUID from the list of known positions if param_body['id'] == 'position' or param_body['id'] == 'entry_position': - default_pos = list(self.known_positions.items())[0]['guid'] + default_pos = self.positions_get()[0]['guid'] param_body['value'] = default_pos + # Similarly, if the input type is a marker type, we'll need to find a placeholder + # marker type GUID from the list of known marker types + if param_body['id'] == 'marker_type': + default_marker = self.docking_offsets_get()[0]['guid'] + param_body['value'] = default_marker action_body_param.append(param_body) # Next, add the default parameters that come with this action for param in action_type_schema['parameters']: - # TODO: skip if this parameter has already been included in the json provided in rmf missions - # e.g. "position" in rmf_move_to_position if param['id'] in id_tracker: continue id_tracker.append(param['id']) @@ -236,10 +260,8 @@ def create_rmf_mission_actions(self, mission_name: str, mission_id: str, mission action_body['action_type'] = action_type action_body['priority'] = action.get('priority') action_body['parameters'] = action_body_param - - mission_actions.append(action_body) - - action_guid = self.missions_mission_id_actions_post(mission_id, mission_actions) + action_body['mission_id'] = mission_id + self.missions_mission_id_actions_post(mission_id, action_body) def navigate(self, position): pos_x = round(position[0], 3) @@ -497,6 +519,8 @@ def load_maps(self): self.known_maps[map['name']] = map['guid'] def me_get(self): + if not self.connected: + return try: response = requests.get(self.prefix + 'users/me', headers=self.headers, timeout=self.timeout) if self.debug: @@ -507,6 +531,17 @@ def me_get(self): except Exception as err: print(f'Other error: {err}') + def mission_groups_get(self): + if not self.connected: + return + try: + response = requests.get(self.prefix + f'mission_groups', headers = self.headers, timeout=self.timeout) + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other here error: {err}") + def actions_get(self): if not self.connected: return @@ -602,7 +637,7 @@ def mission_queue_post(self, mission_id, full_mission_description=None): except Exception as err: print(f"Other here error: {err}") - def missions_mission_id_actions_post(self,mission_id,body): + def missions_mission_id_actions_post(self, mission_id, body): if not self.connected: return try: @@ -630,11 +665,35 @@ def missions_mission_id_actions_get(self, mission_id): except Exception as err: print(f"Other error: {err}") - def missions_post(self, mission): + def mission_groups_post(self, group_name): if not self.connected: return + data = { + 'name': group_name, + 'priority': 0, + 'feature': 'default', + 'icon': '' + } + try: + response = requests.post(self.prefix + 'mission_groups' , headers = self.headers, data=json.dumps(data), timeout = self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def missions_post(self, mission, mission_group_id): + if not self.connected: + return + data = { + 'name': mission, + 'hidden': False, + 'group_id': mission_group_id + } try: - response = requests.post(self.prefix + 'missions' , headers = self.headers, data=mission, timeout = self.timeout) + response = requests.post(self.prefix + 'missions' , headers = self.headers, data=json.dumps(data), timeout = self.timeout) if self.debug: print(f"Response: {response.json()}") return response.json() @@ -748,6 +807,52 @@ def maps_get(self): print(f"Other error: {err}") return [] + def docking_offsets_get(self): + if not self.connected: + return + try: + response = requests.get(self.prefix + 'docking_offsets', headers=self.headers, timeout=self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def docking_offsets_guid_get(self, guid: str): + if not self.connected: + return + try: + response = requests.get(self.prefix + f'docking_offsets/{guid}', headers=self.headers, timeout=self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def footprints_get(self): + if not self.connected: + return + try: + response = requests.get(self.prefix + 'footprints', headers=self.headers, timeout=self.timeout) + if self.debug: + print(f"Response: {response.json()}") + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + + def footprints_guid_get(self, footprint_name: str): + footprints = self.footprints_get() + for ft in footprints: + if ft['name'] == footprint_name: + return ft['guid'] + return None + def mission_completed(self, mission_queue_id): mission_status = self.mission_queue_id_get(mission_queue_id) if not mission_status: @@ -772,4 +877,9 @@ def get_mission_params_with_value(self, mission_name: str, action_type: str, par return mission_params def update_footprint(self): - return self.queue_mission_by_name(self.footprint_mission) + ft_guid = self.footprint + ft_params = self.get_mission_params_with_value(self.footprint_mission, + 'set_footprint', + 'footprint', + ft_guid) + return self.queue_mission_by_name(self.footprint_mission, ft_params) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index ea5ad7b..4e8462b 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -59,6 +59,7 @@ def __init__( rmf_config: rmf_easy.RobotConfiguration, mir_config: dict, conversions: dict, + rmf_missions: dict, fleet_handle, fleet_config, node: Node, @@ -78,7 +79,7 @@ def __init__( 'Content-Type': mir_config['user'], 'Authorization': mir_config['password'], } - self.api: MirAPI = MirAPI(prefix, headers, conversions) + self.api: MirAPI = MirAPI(prefix, headers, conversions, rmf_missions) status = self.api.status_get() while self.api.status_get() is None: @@ -413,8 +414,6 @@ def stop(self, activity): self.node.get_logger().info(f'[{self.name}] Stop requested from RMF!') self.request_stop(mission) self.mission = None - # Before replan, let's make sure the robot footprint is set correctly - self.api.update_footprint() @parallel def request_stop(self, mission: MissionHandle): From aceb40195873741770e180328366e03200d29a98 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 10 Jan 2024 16:19:25 +0800 Subject: [PATCH 03/50] Set up plugin system for custom MiR perform actions Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 16 + .../fleet_adapter_mir/fleet_adapter_mir.py | 2 + .../fleet_adapter_mir/mir_action.py | 51 ++ .../fleet_adapter_mir/mir_api.py | 26 +- .../fleet_adapter_mir/rmf_cart_delivery.py | 428 +++++++++++++++ .../fleet_adapter_mir/rmf_cart_missions.json | 500 +++++++++++------- .../fleet_adapter_mir/robot_adapter_mir.py | 44 +- 7 files changed, 864 insertions(+), 203 deletions(-) create mode 100644 fleet_adapter_mir/fleet_adapter_mir/mir_action.py create mode 100644 fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index 6db6795..56f26e4 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -96,3 +96,19 @@ rmf_fleet: robot_state_update_frequency: 1 debug: False # whether to enable debug printouts from EasyFullControl + +plugins: + rmf_cart_delivery: + actions: ["delivery_pickup", "delivery_dropoff"] + missions: + dock_to_cart: "rmf_dock_to_cart" + pickup: "rmf_pickup_cart_top" + dropoff: "rmf_dropoff_cart_top" + exit_lot: "rmf_exit_lot" + update_footprint: "rmf_update_footprint" + footprints: + robot: "MiR100-200" + cart: "HostShelfCart800" + mission_json: "/path/to/mission/json" + search_timeout: 60 + pickup_dist_threshold: 3.0 # metres diff --git a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py index 92a96b4..ebf082f 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py @@ -142,6 +142,7 @@ def create_fleet(fleet_config, config_yaml, cmd_node, rmf_missions) -> FleetAdap conversions = config_yaml['conversions'] update_frequency = config_yaml['rmf_fleet']['robot_state_update_frequency'] debug = config_yaml['rmf_fleet']['debug'] + plugin_config = config_yaml.get('plugins') event_loop = asyncio.new_event_loop() @@ -156,6 +157,7 @@ def create_fleet(fleet_config, config_yaml, cmd_node, rmf_missions) -> FleetAdap rmf_missions, fleet_handle, fleet_config, + plugin_config, cmd_node, event_loop, debug, diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_action.py b/fleet_adapter_mir/fleet_adapter_mir/mir_action.py new file mode 100644 index 0000000..e6ec961 --- /dev/null +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_action.py @@ -0,0 +1,51 @@ + +import json +from .mir_api import MirAPI, MirStatus, MiRStateCode + + +class MirAction: + def __init__( + self, + node, + name, + mir_api: MirAPI, + update_handle, + actions: list[str], + missions_json: str | None, + action_config: dict | None, + ): + self.node = node + self.name = name + self.api = mir_api + self.update_handle = update_handle + self.actions = actions + self.action_config = action_config + + if missions_json: + with open(missions_json, 'r') as g: + action_missions = json.load(g) + # Create these missions on the robot + self.api.create_missions(action_missions) + # Update mission actions stored in MirAPI + for mission, mission_data in self.api.known_missions.items(): + self.api.mission_actions[mission] = self.api.missions_mission_id_actions_get(mission_data['guid']) + + def update_action(self): + # To be populated in the plugins + pass + + def perform_action(self, category, description, execution): + # To be populated in the plugins + pass + + def cancel_current_task(self, cancel_success, cancel_fail, label): + current_task_id = self.update_handle.more().current_task_id() + self.node.get_logger().info(f'Cancel task requested for [{current_task_id}]') + def _on_cancel(result: bool): + if result: + self.node.get_logger().info(f'Found task [{current_task_id}], cancelling...') + cancel_success() + else: + self.node.get_logger().info(f'Failed to cancel task [{current_task_id}]') + cancel_fail() + self.update_handle.more().cancel_task(current_task_id, [label], lambda result: _on_cancel(result)) diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py index da0e22f..5337662 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py @@ -69,7 +69,6 @@ def __init__(self, prefix, headers, conversions, rmf_missions, timeout=10.0, deb self.mission_keys: dict = conversions['missions'] self.mission_actions: dict = {} self.mission_action_types: dict = {} - self.footprint_keys: dict = conversions['footprints'] self.attempt_connection() def attempt_connection(self): @@ -86,7 +85,7 @@ def attempt_connection(self): self.load_missions() self.load_maps() - self.create_rmf_missions() + self.create_missions(self.rmf_missions) move_key = 'move' assert move_key in self.mission_keys, ( @@ -122,11 +121,6 @@ def make_optional_mission(key): return target_mission self.localize_mission: str | None = make_optional_mission('localize') - self.dock_to_cart_mission: str | None = make_optional_mission('dock_to_cart') - self.pickup_mission: str | None = make_optional_mission('pickup') - self.dropoff_mission: str | None = make_optional_mission('dropoff') - self.exit_mission: str | None = make_optional_mission('exit_lot') - self.footprint_mission: str | None = make_optional_mission('check_footprint') self.go_to: str | None = make_optional_mission('go_to') if self.localize_mission is not None: @@ -161,10 +155,6 @@ def make_optional_mission(key): self.created_by_id = self.me_get()['guid'] - # Set footprints - self.footprint = self.footprints_guid_get(self.footprint_keys['robot']) - self.update_footprint() - def update_known_positions(self): self.known_positions = {} for pos in self.positions_get(): @@ -182,8 +172,8 @@ def update_known_positions(self): self.positions_guid_get(pos['guid']) ) - def create_rmf_missions(self): - if self.rmf_missions is None: + def create_missions(self, rmf_missions): + if rmf_missions is None: return # Retrieve the RMF group id if it already exists @@ -200,7 +190,7 @@ def create_rmf_missions(self): rmf_mission_group_id = mission_group['guid'] # Create RMF missions if they don't exist on the robot - for mission_name, mission_json in self.rmf_missions.items(): + for mission_name, mission_json in rmf_missions.items(): if mission_name not in self.known_missions: # Create the relevant mission on MiR mission = self.missions_post(mission_name, rmf_mission_group_id) @@ -875,11 +865,3 @@ def get_mission_params_with_value(self, mission_name: str, action_type: str, par mission_params = [p] return mission_params return mission_params - - def update_footprint(self): - ft_guid = self.footprint - ft_params = self.get_mission_params_with_value(self.footprint_mission, - 'set_footprint', - 'footprint', - ft_guid) - return self.queue_mission_by_name(self.footprint_mission, ft_params) diff --git a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py new file mode 100644 index 0000000..520b12b --- /dev/null +++ b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py @@ -0,0 +1,428 @@ +import math +from icecream import ic +import enum +from typing import Any +from dataclasses import dataclass +import requests +from urllib.error import HTTPError +from .mir_action import MirAction +from .mir_api import MirAPI, MirStatus, MiRStateCode + + +class PickupState(enum.IntEnum): + PICKUP_ALLOCATED = 0 + MOVE_REQUESTED = 1 + AT_PICKUP = 2 + DOCK_REQUESTED = 3 + DOCK_COMPLETED = 4 + PICKUP_REQUESTED = 5 + PICKUP_SUCCESS = 6 + WARNING_ALERT_PUBLISHED = 7 + TASK_CANCELLED = 8 + + +@dataclass +class Pickup: + state: PickupState + pickup_lots: list[str] # contains either a single pickup lot or list of pickup lots + execution: Any + mission_start_time: float + mission_queue_id: str + latching: bool + + +@dataclass +class Dropoff: + execution: Any + mission_queue_id: str + + +class CartDelivery(MirAction): + def __init__( + self, + retrieve_mir_coordinates, # callback + ): + self.known_positions = self.api.known_positions + + # Delivery related params + self.pickup: Pickup = None + self.dropoff: Dropoff = None + self.search_timeout = self.action_config.get('search_timeout', 60) # seconds + + # Useful robot adapter callbacks + self.retrieve_mir_coordinates = retrieve_mir_coordinates + + # Store the mission names to be used later + self.dock_to_cart_mission = self.action_config['missions']['dock_to_cart'] + self.pickup_mission = self.action_config['missions']['pickup'] + self.dropoff_mission = self.action_config['missions']['dropoff'] + self.exit_mission = self.action_config['missions']['exit_lot'] + self.footprint_mission = self.action_config['missions']['update_footprint'] + + # Initialize footprints + self.robot_footprint_guid = self.api.footprints_guid_get(self.action_config['footprints']['robot']) + self.cart_footprint_guid = self.api.footprints_guid_get(self.action_config['footprints']['cart']) + self.update_footprint(self.robot_footprint_guid) + + def update_action(self): + if self.pickup is not None: + if self.check_pickup(self.pickup): + self.pickup = None + if self.dropoff is not None: + if self.check_dropoff(self.dropoff): + self.dropoff = None + + def perform_action(self, category, description, execution): + if category == 'delivery_pickup': + pickup_lot = description.get('pickup_lot') + self.cart_pickup(execution, pickup_lot) + elif category == 'delivery_dropoff': + self.node.get_logger().info(f'Received dropoff request!') + self.dropoff = Dropoff(execution=execution, mission_queue_id=None) + + def cancel_task(self, label: str = None): + def _cancel_success(): + if self.pickup: + self.pickup.state = PickupState.TASK_CANCELLED + def _cancel_fail(): + pass + self.cancel_current_task(_cancel_success, _cancel_fail, label) + + def cart_pickup(self, execution, pickup_lot: str): + if self.pickup is not None: + # If there is an existing pickup, we'll replace it + if self.pickup.execution is not None and self.pickup.execution.okay(): + self.pickup.execution.finished() + self.pickup = None + + # Check if robot's latch is open + if self.is_latch_open(): + # Latch is open, unable to perform pickup + self.node.get_logger().info(f'Robot [{self.name}] latch is open, unable to perform pickup, cancelling task...') + self.cancel_task() + return + + self.node.get_logger().info(f'New pickup requested for robot [{self.name}]') + + # TODO(XY): Allow multiple pickups? + pickup_lots = [pickup_lot] + + self.pickup = Pickup( + state=PickupState.PICKUP_ALLOCATED, + pickup_lots=pickup_lots, + execution=execution, + mission_start_time=None, + mission_queue_id=None, + latching=False + ) + return + + def check_pickup(self, pickup: Pickup): + # If this is a PerformAction pickup, check that the action is underway and valid + if pickup.execution is not None and not pickup.execution.okay(): + self.node.get_logger().info(f'[{pickup.type}] action is killed/canceled.') + pickup.state = PickupState.TASK_CANCELLED + + # Start state machine check + self.node.get_logger().debug(f'PickupState: {pickup.state.name}') + match pickup.state: + case PickupState.PICKUP_ALLOCATED: + # Move to the first pickup place on the list + pickup_lot = pickup.pickup_lots[0] + self.node.get_logger().info(f'Requested to pickup cart at waypoint {pickup.state.name}') + if self.known_positions.get(pickup_lot) is None: + self.node.get_logger().info(f'Pickup Lot does not exist in MiR map, please resubmit.') + self.cancel_task() + pickup.state = PickupState.TASK_CANCELLED + return + + # Find the MiR coordinates of this pickup place + mir_pose = self.retrieve_mir_coordinates(pickup_lot) + pickup.mission_queue_id = self.api.navigate(mir_pose) + pickup.state = PickupState.MOVE_REQUESTED + + case PickupState.MOVE_REQUESTED: + # Make sure that there is an rmf_move mission issued + if pickup.mission_queue_id is None: + self.node.get_logger().info(f'Robot [{self.name}] is indicated to be at the MOVE_REQUESTED state but ' + f'no mission queue ID stored for this pickup mission! Returning to PICKUP_ALLOCATED state.') + pickup.state = PickupState.PICKUP_ALLOCATED + return + + # Robot has reached the pickup lot + pickup_lot = pickup.pickup_lots[0] + if self.api.mission_completed(pickup.mission_queue_id): + self.node.get_logger().info(f'Robot [{self.name}] has reached the pickup waypoint {pickup_lot}') + pickup.mission_queue_id = None + pickup.state = PickupState.AT_PICKUP + return + + # If the robot is relatively close to the pickup lot, we also consider it to be at pickup + # and allow the dock_to_cart mission to position the robot in front of the cart + pickup_pose = self.retrieve_mir_coordinates(pickup) + current_pose = self.api.status_get().state.position + if self.dist(pickup_pose, current_pose) < self.action_config['pickup_dist_threshold']: + # Delete the ongoing mission + self.api.mission_queue_id_delete(pickup.mission_queue_id) + self.node.get_logger().info(f'Robot [{self.name}] is sufficiently near to the pickup waypoint {pickup_lot} for docking') + pickup.mission_queue_id = None + pickup.state = PickupState.AT_PICKUP + + case PickupState.AT_PICKUP: + # Send rmf_dock_to_cart mission + assert self.dock_to_cart_mission is not None + current_wp_name = pickup.pickup_lots[0] + cart_marker_guid = self.api.known_positions[current_wp_name]['guid'] + mission_params = self.api.get_mission_params_with_value(self.dock_to_cart_mission, + 'docking', + 'cart_marker', + cart_marker_guid) + self.update_footprint(self.robot_footprint_guid) + mission_queue_id = self.api.queue_mission_by_name(self.dock_to_cart_mission, mission_params) + if not mission_queue_id: + error_str = f'Mission {self.dock_to_cart_mission} not supported, ignoring' + self.node.get_logger().error(error_str) + return + pickup.mission_queue_id = mission_queue_id + pickup.state = PickupState.DOCK_REQUESTED + self.node.get_logger().info(f'Dock to cart mission requested with mission queue id {mission_queue_id}') + + case PickupState.DOCK_REQUESTED: + # Make sure that there is an rmf_dock_to_cart mission issued + if pickup.mission_queue_id is None: + self.node.get_logger().info(f'Robot [{self.name}] is indicated to be at the DOCK_REQUESTED state but ' + f'no mission queue ID stored for this docking mission! Returning to AT_PICKUP state.') + pickup.state = PickupState.AT_PICKUP + return + # Mission completed, move onto the next state + if self.api.mission_completed(pickup.mission_queue_id): + self.node.get_logger().info(f'Robot [{self.name}] dock to cart mission {pickup.mission_queue_id} completed or timed out.') + pickup.mission_queue_id = None + pickup.mission_start_time = None + pickup.state = PickupState.DOCK_COMPLETED + return + # Mission not yet completed, we check the timeout status to decide if we need to publish any alert + if pickup.mission_start_time is None: + pickup.mission_start_time = self.node.get_clock().now().nanoseconds / 1e9 + now = self.node.get_clock().now().nanoseconds / 1e9 + seconds_passed = now - pickup.mission_start_time + # Publish update every 10 seconds just to monitor + if round(seconds_passed)%10 == 0: + self.node.get_logger().info(f'{round(seconds_passed)} seconds have passed since pickup mission requested.') + # Mission timeout, cart not found + if seconds_passed > self.search_timeout: + # Delete mission from mir first + self.api.mission_queue_id_delete(pickup.mission_queue_id) + # Regardless of whether the robot completed docking properly, we move to the next state to check + self.node.get_logger().info(f'Robot [{self.name}] dock to cart mission {pickup.mission_queue_id} timed out! ' + f'Configured search timeout is {self.search_timeout} seconds.') + pickup.state = PickupState.DOCK_COMPLETED + return + + case PickupState.DOCK_COMPLETED: + if pickup.mission_start_time is not None: + pickup.mission_start_time = None + # Check if robot docked under the correct cart + cart_check = self.is_correct_cart() + if cart_check: + # If cart is correct, send pickup mission + assert self.pickup_mission is not None + mission_queue_id = self.api.queue_mission_by_name(self.pickup_mission) + if not mission_queue_id: + error_str = f'Mission {self.pickup_mission} not supported, ignoring' + self.node.get_logger().error(error_str) + return + pickup.mission_queue_id = mission_queue_id + pickup.mission_start_time = self.node.get_clock().now().nanoseconds / 1e9 + pickup.latching = True + self.node.get_logger().info(f'Robot [{self.name}] found the correct cart, pickup mission requested with mission queue id {mission_queue_id}') + pickup.state = PickupState.PICKUP_REQUESTED + elif cart_check is None: + # If cart is missing, cancel this task + self.node.get_logger().info(f'Robot [{self.name}] was unable to dock under any carts, please check that cart is present. Cancelling task.') + pickup.state = PickupState.TASK_CANCELLED + else: + # If cart is wrong, cancel this task also but after we exit from the lot + self.node.get_logger().info(f'Robot [{self.name}] found the wrong cart, exiting lot and cancelling task.') + self.exit_lot() + pickup.state = PickupState.TASK_CANCELLED + + case PickupState.PICKUP_REQUESTED: + if self.is_latch_open(): + # Latch successfully opened, indicate pickup as success + pickup.state = PickupState.PICKUP_SUCCESS + pickup.latching = False + + case PickupState.PICKUP_SUCCESS: + # Correct ID, we can end the delivery now + self.node.get_logger().info(f'Robot [{self.name}] successfully received cart, exiting lot with cart and ending mission') + self.exit_lot() + if pickup.execution is not None: + pickup.execution.finished() + return True + + case PickupState.TASK_CANCELLED: + self.node.get_logger().info(f'Robot [{self.name}] is in pickup cancelled state.') + # If some MiR mission is in progress, we abort it unless it is latching + if pickup.mission_queue_id is not None and not self.api.mission_completed(pickup.mission_queue_id): + if pickup.latching: + self.node.get_logger().info(f'Robot [{self.name}] is performing latching, cancelling task after this action is complete.') + return False + self.api.mission_queue_id_delete(pickup.mission_queue_id) + # Clear any errors + self.api.clear_error() + self.api.status_put(state_id=MiRStateCode.READY) + self.release_cart() + # Mark pickup session as completed + return True + + return False + + def check_dropoff(self, dropoff: Dropoff): + # Check if action is underway or cancelled + if dropoff.execution is not None and not dropoff.execution.okay(): + self.node.get_logger().info(f'Dropoff action is killed/canceled') + + # If cart release is in progress, we let it finish first + if dropoff.mission_queue_id is not None and not self.api.mission_completed(dropoff.mission_queue_id): + return False + + # Task is cancelled and cart is done dropping off/mission is not yet queued anyway, + # we can mark it as completed at this point + self.api.clear_error() + self.api.status_put(state_id=MiRStateCode.READY) + # Mark dropoff session as completed + return True + + # No mission queued yet + if dropoff.mission_queue_id is None: + assert self.dropoff_mission is not None + mission_queue_id = self.api.queue_mission_by_name(self.dropoff_mission) + if not mission_queue_id: + error_str = f'Mission {self.dropoff_mission} not supported, ignoring' + self.node.get_logger().error(error_str) + return True + self.node.get_logger().info(f'Mission {self.dropoff_mission} added to queue.') + dropoff.mission_queue_id = mission_queue_id + self.node.get_logger().info(f'Dropoff mission requested with mission queue id {mission_queue_id}') + + # Mission queued, check completion + else: + if self.api.mission_completed(dropoff.mission_queue_id): + self.node.get_logger().info(f'Dropoff mission completed!') + if dropoff.execution is not None: + dropoff.execution.finished() + return True + else: + self.node.get_logger().info(f'Dropoff mission in progress...') + return + + def is_latch_open(self): + # Return True if latch is open, else False + # ------------------------ + # IMPLEMENT YOUR CODE HERE + # ------------------------ + return False + + def is_under_cart(self): + # Return True if robot is under any carts, else False + # ------------------------ + # IMPLEMENT YOUR CODE HERE + # ------------------------ + return False + + def is_correct_cart(self): + # Return True if cart is correct, False if cart is wrong, None if no cart + # ------------------------ + # IMPLEMENT YOUR CODE HERE + # ------------------------ + return None + + def exit_lot(self): + if not self.api.connected: + return None + # Set footprint to robot footprint before exiting lot + self.update_footprint(self.robot_footprint_guid) + return self.api.queue_mission_by_name(self.exit_mission) + + def release_cart(self): + # If robot latch is open, close it + if self.is_latch_open(): + self.node.get_logger().info(f'Robot [{self.name}] detected to have latch open, dropping off cart...') + self.dropoff = Dropoff( + execution=None, + mission_queue_id=None) + # Dropoff mission will take care of the lot exit, so we can return here + return + # If robot is under a cart, exit lot + if self.is_under_cart(): + self.node.get_logger().info(f'Cart detected above robot [{self.name}] during a release call, exiting lot...') + self.exit_lot() + + def update_footprint(self, ft_guid: str): + ft_params = self.api.get_mission_params_with_value(self.footprint_mission, + 'set_footprint', + 'footprint', + ft_guid) + return self.api.queue_mission_by_name(self.footprint_mission, ft_params) + + def dist(self, A, B): + assert(len(A) > 1) + assert(len(B) > 1) + return math.sqrt((A[0] - B[0])**2 + (A[1] - B[1])**2) + + + # ------------------------------------------------------------------------------------------------------------------ + # MIR API FOR CART RELATED MISSIONS + # ------------------------------------------------------------------------------------------------------------------ + + def register_get(self, register: int): + if not self.api.connected: + return None + try: + response = requests.get(self.api.prefix + f'registers/{register}', headers = self.api.headers, timeout = self.api.timeout) + if self.api.debug: + print(f"Response: {response.headers}") + # Response value is string, return integer of value + return int(response.json().get('value', 0)) + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + return None + except Exception as err: + print(f"Other error: {err}") + return None + + def io_module_guid_status_get(self, io_guid: str): + if not self.api.connected: + return None + if io_guid is None: + return None + try: + response = requests.get(self.api.prefix + f'io_modules/{io_guid}/status', headers = self.api.headers, timeout = self.api.timeout) + if self.api.debug: + print(f"Response: {response.headers}") + if 'input_state' not in response.json(): + return None + return response.json()['input_state'] + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + return None + except Exception as err: + print(f"Other error: {err}") + return None + + def io_modules_get(self): + if not self.api.connected: + return None + try: + response = requests.get(self.api.prefix + f'io_modules', headers = self.api.headers, timeout = self.api.timeout) + if self.api.debug: + print(f"Response: {response.headers}") + # Response value is string, return integer of value + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + return None + except Exception as err: + print(f"Other error: {err}") + return None diff --git a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_missions.json b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_missions.json index b7d49ba..86ce67c 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_missions.json +++ b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_missions.json @@ -4,28 +4,17 @@ "priority": 1, "parameters": [ { - "value": "mirconst-guid-0000-0001-footprint000", - "input_name": null, - "id": "footprint" - } - ], - "action_type": "set_footprint" - }, - { - "priority": 2, - "parameters": [ - { - "value": -1.0, + "value": -1, "input_name": null, "id": "x" }, { - "value": 0.0, + "value": 0, "input_name": null, "id": "y" }, { - "value": 0.0, + "value": 0, "input_name": null, "id": "orientation" }, @@ -46,90 +35,11 @@ } ], "action_type": "relative_move" - }, - { - "priority": 3, - "parameters": [ - { - "value": "plc_register", - "input_name": null, - "id": "compare" - }, - { - "value": "e2fc134a-56bb-11ee-a409-00012998f454", - "input_name": null, - "id": "module" - }, - { - "value": "0", - "input_name": null, - "id": "io_port" - }, - { - "value": "8", - "input_name": null, - "id": "register" - }, - { - "value": "==", - "input_name": null, - "id": "operator" - }, - { - "value": 1.0, - "input_name": null, - "id": "value" - }, - { - "value": "", - "input_name": null, - "id": "true" - }, - { - "value": "", - "input_name": null, - "id": "false" - } - ], - "action_type": "if" - }, - { - "priority": 4, - "parameters": [ - { - "value": "f65abbec-ea4e-11ed-af33-00012983ef2c", - "input_name": null, - "id": "footprint" - } - ], - "action_type": "set_footprint" - }, - { - "priority": 5, - "parameters": [ - { - "value": "mirconst-guid-0000-0001-footprint000", - "input_name": null, - "id": "footprint" - } - ], - "action_type": "set_footprint" } ], "rmf_dock_to_cart": [ { "priority": 1, - "parameters": [ - { - "value": "mirconst-guid-0000-0001-footprint000", - "input_name": null, - "id": "footprint" - } - ], - "action_type": "set_footprint" - }, - { - "priority": 2, "parameters": [ { "value": null, @@ -155,7 +65,7 @@ "action_type": "docking" } ], - "rmf_dropoff_cart": [ + "rmf_dropoff_cart_side": [ { "priority": 2, "parameters": [ @@ -215,32 +125,27 @@ "action_type": "load_mission" } ], - "rmf_follow_line": [ + "rmf_dropoff_cart_top": [ { "priority": 1, - "parameters": [], - "action_type": "adjust_localization" - }, - { - "priority": 2, "parameters": [ { - "value": "plc_register", + "value": "io_module", "input_name": null, "id": "compare" }, { - "value": "e2fc134a-56bb-11ee-a409-00012998f454", + "value": "mirconst-guid-0000-0001-internalIO00", "input_name": null, "id": "module" }, { - "value": "0", + "value": 3, "input_name": null, "id": "io_port" }, { - "value": "8", + "value": "1", "input_name": null, "id": "register" }, @@ -250,7 +155,7 @@ "id": "operator" }, { - "value": 1.0, + "value": 0, "input_name": null, "id": "value" }, @@ -267,22 +172,58 @@ ], "action_type": "if" }, + { + "priority": 2, + "parameters": [ + { + "value": "No shelf is lifted according to IO input 3", + "input_name": null, + "id": "message" + } + ], + "action_type": "throw_error" + }, { "priority": 3, "parameters": [ { - "value": "f65abbec-ea4e-11ed-af33-00012983ef2c", + "value": null, "input_name": null, - "id": "footprint" + "id": "sound" + }, + { + "value": null, + "input_name": null, + "id": "volume" + }, + { + "value": "muted", + "input_name": null, + "id": "front" + }, + { + "value": "muted", + "input_name": null, + "id": "rear" + }, + { + "value": "muted", + "input_name": null, + "id": "sides" + }, + { + "value": "", + "input_name": null, + "id": "content" } ], - "action_type": "set_footprint" + "action_type": "reduce_protective_fields" }, { - "priority": 4, + "priority": 8, "parameters": [ { - "value": "mirconst-guid-0000-0001-footprint000", + "value": "mirconst-guid-0000-0001-footprint007", "input_name": null, "id": "footprint" } @@ -290,122 +231,148 @@ "action_type": "set_footprint" }, { - "priority": 5, + "priority": 9, "parameters": [ { - "value": "to be replaced with a value guid in the fleet adapter", - "input_name": "start_waypoint", - "id": "position" + "value": -1, + "input_name": null, + "id": "x" }, { - "value": 10, + "value": 0, "input_name": null, - "id": "retries" + "id": "y" }, { - "value": 0.2, + "value": 0, "input_name": null, - "id": "distance_threshold" - } - ], - "action_type": "move" - }, - { - "priority": 6, - "parameters": [ + "id": "orientation" + }, { - "value": "plc_register", + "value": 0.25, "input_name": null, - "id": "compare" + "id": "max_linear_speed" }, { - "value": "e2fc134a-56bb-11ee-a409-00012998f454", + "value": 0.25, "input_name": null, - "id": "module" + "id": "max_angular_speed" }, { - "value": "0", + "value": true, "input_name": null, - "id": "io_port" - }, + "id": "collision_detection" + } + ], + "action_type": "relative_move" + }, + { + "priority": 10, + "parameters": [ { - "value": "8", + "value": "577eaace-be75-11ed-a404-0001297c2278", "input_name": null, - "id": "register" - }, + "id": "footprint" + } + ], + "action_type": "set_footprint" + }, + { + "priority": 4, + "parameters": [ { - "value": "==", + "value": "mirconst-guid-0000-0001-internalIO00", "input_name": null, - "id": "operator" + "id": "module" }, { - "value": 1.0, + "value": 3, "input_name": null, - "id": "value" + "id": "port" }, { - "value": "", + "value": "off", "input_name": null, - "id": "true" + "id": "operation" }, { - "value": "", + "value": null, "input_name": null, - "id": "false" + "id": "timeout" } ], - "action_type": "if" + "action_type": "set_io" }, { - "priority": 7, + "priority": 5, "parameters": [ { - "value": "454e65f9-06b9-11ee-acbc-00012983ef2c", + "value": "00:00:01.000000", "input_name": null, - "id": "footprint" + "id": "time" } ], - "action_type": "set_footprint" + "action_type": "wait" }, { - "priority": 8, + "priority": 6, "parameters": [ { - "value": "mirconst-guid-0000-0001-footprint000", + "value": "mirconst-guid-0000-0001-internalIO00", "input_name": null, - "id": "footprint" + "id": "module" + }, + { + "value": 2, + "input_name": null, + "id": "port" + }, + { + "value": "on", + "input_name": null, + "id": "operation" + }, + { + "value": "00:00:10.000000", + "input_name": null, + "id": "timeout" + }, + { + "value": "", + "input_name": null, + "id": "content" } ], - "action_type": "set_footprint" + "action_type": "set_reset_io" }, { - "priority": 9, + "priority": 7, "parameters": [ { - "value": "to be replaced with a value guid in the fleet adapter", - "input_name": "end_waypoint", - "id": "position" + "value": "mirconst-guid-0000-0001-internalIO00", + "input_name": null, + "id": "module" }, { - "value": 10, + "value": 2, "input_name": null, - "id": "retries" + "id": "port" + }, + { + "value": "on", + "input_name": null, + "id": "value" }, { - "value": 0.2, + "value": "00:00:10.000000", "input_name": null, - "id": "distance_threshold" + "id": "timeout" } ], - "action_type": "move" - }, - { - "priority": 10, - "parameters": [], - "action_type": "adjust_localization" + "action_type": "wait_for_io" } ], - "rmf_pickup_cart": [ + "rmf_pickup_cart_side": [ { "priority": 1, "parameters": [ @@ -486,27 +453,110 @@ "action_type": "set_footprint" } ], - "rmf_update_footprint": [ + "rmf_pickup_cart_top": [ + { + "priority": 7, + "parameters": [ + { + "value": "mirconst-guid-0000-0001-footprint007", + "input_name": null, + "id": "footprint" + } + ], + "action_type": "set_footprint" + }, + { + "priority": 8, + "parameters": [ + { + "value": null, + "input_name": null, + "id": "sound" + }, + { + "value": null, + "input_name": null, + "id": "volume" + }, + { + "value": "muted", + "input_name": null, + "id": "front" + }, + { + "value": "muted", + "input_name": null, + "id": "rear" + }, + { + "value": "muted", + "input_name": null, + "id": "sides" + }, + { + "value": "", + "input_name": null, + "id": "content" + } + ], + "action_type": "reduce_protective_fields" + }, + { + "priority": 9, + "parameters": [ + { + "value": 0, + "input_name": null, + "id": "x" + }, + { + "value": 0, + "input_name": null, + "id": "y" + }, + { + "value": 0, + "input_name": null, + "id": "orientation" + }, + { + "value": 0.25, + "input_name": null, + "id": "max_linear_speed" + }, + { + "value": 0.25, + "input_name": null, + "id": "max_angular_speed" + }, + { + "value": true, + "input_name": null, + "id": "collision_detection" + } + ], + "action_type": "relative_move" + }, { "priority": 1, "parameters": [ { - "value": "plc_register", + "value": "io_module", "input_name": null, "id": "compare" }, { - "value": "e2fc134a-56bb-11ee-a409-00012998f454", + "value": "mirconst-guid-0000-0001-internalIO00", "input_name": null, "id": "module" }, { - "value": "0", + "value": 2, "input_name": null, "id": "io_port" }, { - "value": "8", + "value": "1", "input_name": null, "id": "register" }, @@ -516,7 +566,7 @@ "id": "operator" }, { - "value": 1.0, + "value": 0, "input_name": null, "id": "value" }, @@ -537,19 +587,115 @@ "priority": 2, "parameters": [ { - "value": "f65abbec-ea4e-11ed-af33-00012983ef2c", + "value": "A shelf is already lifted according to IO input 2", "input_name": null, - "id": "footprint" + "id": "message" } ], - "action_type": "set_footprint" + "action_type": "throw_error" }, { "priority": 3, "parameters": [ { - "value": "mirconst-guid-0000-0001-footprint000", + "value": "mirconst-guid-0000-0001-internalIO00", + "input_name": null, + "id": "module" + }, + { + "value": 2, + "input_name": null, + "id": "port" + }, + { + "value": "off", + "input_name": null, + "id": "operation" + }, + { + "value": null, + "input_name": null, + "id": "timeout" + } + ], + "action_type": "set_io" + }, + { + "priority": 4, + "parameters": [ + { + "value": "00:00:01.000000", + "input_name": null, + "id": "time" + } + ], + "action_type": "wait" + }, + { + "priority": 5, + "parameters": [ + { + "value": "mirconst-guid-0000-0001-internalIO00", "input_name": null, + "id": "module" + }, + { + "value": 3, + "input_name": null, + "id": "port" + }, + { + "value": "on", + "input_name": null, + "id": "operation" + }, + { + "value": "00:00:10.000000", + "input_name": null, + "id": "timeout" + }, + { + "value": "", + "input_name": null, + "id": "content" + } + ], + "action_type": "set_reset_io" + }, + { + "priority": 6, + "parameters": [ + { + "value": "mirconst-guid-0000-0001-internalIO00", + "input_name": null, + "id": "module" + }, + { + "value": 3, + "input_name": null, + "id": "port" + }, + { + "value": "on", + "input_name": null, + "id": "value" + }, + { + "value": "00:00:10.000000", + "input_name": null, + "id": "timeout" + } + ], + "action_type": "wait_for_io" + } + ], + "rmf_update_footprint": [ + { + "priority": 1, + "parameters": [ + { + "value": "mirconst-guid-0000-0001-footprint000", + "input_name": "footprint", "id": "footprint" } ], diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 4e8462b..80ee1e2 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -1,8 +1,13 @@ import math +import numpy as np +from icecream import ic +import enum +import copy import time from typing import Any from dataclasses import dataclass import json +import datetime from rmf_adapter.robot_update_handle import Tier import rmf_adapter.easy_full_control as rmf_easy import rclpy @@ -11,6 +16,9 @@ from .mir_api import MirAPI, MirStatus, MiRStateCode from threading import Lock +# Import plugins +from .rmf_cart_delivery import CartDelivery + # Parallel processing solution derived from # https://stackoverflow.com/a/59385935 @@ -58,10 +66,12 @@ def __init__( name: str, rmf_config: rmf_easy.RobotConfiguration, mir_config: dict, + # pickup_config: dict, conversions: dict, rmf_missions: dict, fleet_handle, fleet_config, + plugin_config: dict | None, node: Node, event_loop, debug=False @@ -109,6 +119,19 @@ def __init__( self.nav_issue_ticket = None # We should only have one issue ticket at a time to manage unsuccessful missions self.requested_replan = False self.replan_counts = 0 + self.plugins = {} + # Available plugins: + if 'rmf_cart_delivery' in plugin_config: + self.plugins['rmf_cart_delivery'] = CartDelivery( + node=node, + name=name, + mir_api=self.api, + update_handle=self.update_handle, + actions=plugin_config['rmf_cart_delivery']['actions'], + missions_json=plugin_config['rmf_cart_delivery']['missions_json'], + action_config=plugin_config['rmf_cart_delivery'], + retrieve_mir_coordinates=self.retrieve_mir_coordinates) + # To be added on with other plugins self.fleet_handle = fleet_handle self.update_handle = fleet_handle.add_robot( @@ -166,6 +189,10 @@ def request_update(self): if mission is not None and mission.done: self.update_rmf_finished(mission) + # PerformAction related checks + for plugin in self.plugins.values(): + plugin.update_action() + # Clear error on updates if status is not None and 'errors' in status.response and len(status.response['errors']) > 0: self.api.clear_error() @@ -457,12 +484,21 @@ def request_localize(self, estimate, mission: MissionHandle): mission.set_mission_queue_id(mission_queue_id) def perform_action(self, category, description, execution): - task_id = self.update_handle.more().current_task_id() - # ----------------------------------------------------- - # INSERT PERFORM ACTION LOGIC HERE - # ----------------------------------------------------- + for plugin in self.plugins.values(): + if category in plugin.actions: + action = self.actions[category] + action.perform_action(category, description, execution) + return raise NotImplementedError + def retrieve_mir_coordinates(self, waypoint_name: str): + transform = self.fleet_config.transformations_to_robot_coordinates + transform_current_map = transform.get(self.current_map) + rmf_pose = self.fleet_config.graph.find_waypoint(waypoint_name).location + new_rmf_pose = np.array([rmf_pose[0], rmf_pose[1], 0.0]) + mir_pose = transform_current_map.apply(new_rmf_pose) + return mir_pose + def dist(self, A, B): assert(len(A) > 1) assert(len(B) > 1) From 6e2dd989cbdf775bff9a03b1ef25c772fff61d96 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 11 Jan 2024 12:22:56 +0800 Subject: [PATCH 04/50] Cart delivery plugin fixes Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 11 +++----- .../fleet_adapter_mir/__init__.py | 2 -- .../fleet_adapter_mir/rmf_cart_delivery.py | 11 +++++++- .../fleet_adapter_mir/robot_adapter_mir.py | 27 +++++++------------ .../rmf_cart_missions.json | 0 .../rmf_missions.json | 0 6 files changed, 23 insertions(+), 28 deletions(-) rename {fleet_adapter_mir/fleet_adapter_mir => missions}/rmf_cart_missions.json (100%) rename {fleet_adapter_mir/fleet_adapter_mir => missions}/rmf_missions.json (100%) diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index 56f26e4..1af76aa 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -36,12 +36,7 @@ conversions: move: "rmf_move" # This mission must be predefined dock_and_charge: "rmf_dock_and_charge" # This mission must be predefined localize: "rmf_localize" - # dock_to_cart: "rmf_dock_to_cart" - # pickup: "rmf_pickup_cart" - # dropoff: "rmf_dropoff_cart" - # exit_lot: "rmf_exit_lot" go_to: "rmf_move_to_position" - # check_footprint: "rmf_update_footprint" lift_positions: lift_1: # Name of lift matching the navigation graph @@ -83,7 +78,7 @@ rmf_fleet: delivery: True clean: False finishing_request: "charge" # [park, charge, nothing] - actions: [] + actions: ["delivery_pickup", "delivery_dropoff", "dropoff_if_carrying_cart"] robots: mir_1: charger: "Charger_1" @@ -99,7 +94,7 @@ rmf_fleet: plugins: rmf_cart_delivery: - actions: ["delivery_pickup", "delivery_dropoff"] + actions: ["delivery_pickup", "delivery_dropoff", "dropoff_if_carrying_cart"] missions: dock_to_cart: "rmf_dock_to_cart" pickup: "rmf_pickup_cart_top" @@ -109,6 +104,6 @@ plugins: footprints: robot: "MiR100-200" cart: "HostShelfCart800" - mission_json: "/path/to/mission/json" + missions_json: "/home/some_user/mir_ws/src/fleet_adapter_mir/missions/rmf_cart_missions.json" search_timeout: 60 pickup_dist_threshold: 3.0 # metres diff --git a/fleet_adapter_mir/fleet_adapter_mir/__init__.py b/fleet_adapter_mir/fleet_adapter_mir/__init__.py index de62d2a..e69de29 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/__init__.py +++ b/fleet_adapter_mir/fleet_adapter_mir/__init__.py @@ -1,2 +0,0 @@ -from .fleet_adapter_mir import * -from .MiRClientAPI import * diff --git a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py index 520b12b..4cbf4df 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py +++ b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py @@ -40,8 +40,17 @@ class Dropoff: class CartDelivery(MirAction): def __init__( self, + node, + name, + mir_api, + update_handle, + actions, + missions_json, + action_config, retrieve_mir_coordinates, # callback ): + MirAction.__init__(self, node, name, mir_api, update_handle, actions, + missions_json, action_config) self.known_positions = self.api.known_positions # Delivery related params @@ -159,7 +168,7 @@ def check_pickup(self, pickup: Pickup): # If the robot is relatively close to the pickup lot, we also consider it to be at pickup # and allow the dock_to_cart mission to position the robot in front of the cart - pickup_pose = self.retrieve_mir_coordinates(pickup) + pickup_pose = self.retrieve_mir_coordinates(pickup_lot) current_pose = self.api.status_get().state.position if self.dist(pickup_pose, current_pose) < self.action_config['pickup_dist_threshold']: # Delete the ongoing mission diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 80ee1e2..db7948d 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -119,6 +119,15 @@ def __init__( self.nav_issue_ticket = None # We should only have one issue ticket at a time to manage unsuccessful missions self.requested_replan = False self.replan_counts = 0 + + self.fleet_handle = fleet_handle + self.update_handle = fleet_handle.add_robot( + self.name, + status.state, + rmf_config, + self._make_callbacks(), + ) + self.plugins = {} # Available plugins: if 'rmf_cart_delivery' in plugin_config: @@ -133,14 +142,6 @@ def __init__( retrieve_mir_coordinates=self.retrieve_mir_coordinates) # To be added on with other plugins - self.fleet_handle = fleet_handle - self.update_handle = fleet_handle.add_robot( - self.name, - status.state, - rmf_config, - self._make_callbacks(), - ) - @property def activity(self): # Move the mission reference into a separate variable just in case @@ -300,13 +301,6 @@ def _make_callbacks(self): return callbacks def navigate(self, destination, execution): - # If this is a cancellation behavior command, we check if we're meant to ignore it. - if self.ignore_action: - self.node.get_logger().info(f'Ignoring navigation command, ignore action flag is ' - f'raised after checking latch.') - execution.finished() - return - # If the nav command coming in is to bring the robot to a charger, but the robot is # already charging at the same charger, we ignore this nav command status = self.last_known_status @@ -486,8 +480,7 @@ def request_localize(self, estimate, mission: MissionHandle): def perform_action(self, category, description, execution): for plugin in self.plugins.values(): if category in plugin.actions: - action = self.actions[category] - action.perform_action(category, description, execution) + plugin.perform_action(category, description, execution) return raise NotImplementedError diff --git a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_missions.json b/missions/rmf_cart_missions.json similarity index 100% rename from fleet_adapter_mir/fleet_adapter_mir/rmf_cart_missions.json rename to missions/rmf_cart_missions.json diff --git a/fleet_adapter_mir/fleet_adapter_mir/rmf_missions.json b/missions/rmf_missions.json similarity index 100% rename from fleet_adapter_mir/fleet_adapter_mir/rmf_missions.json rename to missions/rmf_missions.json From 47db7f19a4aa3968eb04fadc7b1378571d749068 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 12 Jan 2024 11:03:20 +0800 Subject: [PATCH 05/50] Add cart pickup task script Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/rmf_cart_delivery.py | 16 +- fleet_adapter_mir_tasks/LICENSE | 202 ++++++++++++++++++ .../fleet_adapter_mir_tasks/__init__.py | 0 .../dispatch_pickup.py | 201 +++++++++++++++++ fleet_adapter_mir_tasks/package.xml | 18 ++ .../resource/fleet_adapter_mir_tasks | 0 fleet_adapter_mir_tasks/setup.cfg | 4 + fleet_adapter_mir_tasks/setup.py | 25 +++ 8 files changed, 461 insertions(+), 5 deletions(-) create mode 100644 fleet_adapter_mir_tasks/LICENSE create mode 100644 fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/__init__.py create mode 100644 fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py create mode 100644 fleet_adapter_mir_tasks/package.xml create mode 100644 fleet_adapter_mir_tasks/resource/fleet_adapter_mir_tasks create mode 100644 fleet_adapter_mir_tasks/setup.cfg create mode 100644 fleet_adapter_mir_tasks/setup.py diff --git a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py index 4cbf4df..078d1a6 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py +++ b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py @@ -25,6 +25,7 @@ class PickupState(enum.IntEnum): class Pickup: state: PickupState pickup_lots: list[str] # contains either a single pickup lot or list of pickup lots + cart_id: str execution: Any mission_start_time: float mission_queue_id: str @@ -84,7 +85,8 @@ def update_action(self): def perform_action(self, category, description, execution): if category == 'delivery_pickup': pickup_lot = description.get('pickup_lot') - self.cart_pickup(execution, pickup_lot) + cart_id = description.get('cart_id') + self.cart_pickup(execution, pickup_lot, cart_id) elif category == 'delivery_dropoff': self.node.get_logger().info(f'Received dropoff request!') self.dropoff = Dropoff(execution=execution, mission_queue_id=None) @@ -97,7 +99,7 @@ def _cancel_fail(): pass self.cancel_current_task(_cancel_success, _cancel_fail, label) - def cart_pickup(self, execution, pickup_lot: str): + def cart_pickup(self, execution, pickup_lot: str, cart_id: str): if self.pickup is not None: # If there is an existing pickup, we'll replace it if self.pickup.execution is not None and self.pickup.execution.okay(): @@ -119,6 +121,7 @@ def cart_pickup(self, execution, pickup_lot: str): self.pickup = Pickup( state=PickupState.PICKUP_ALLOCATED, pickup_lots=pickup_lots, + cart_id=cart_id, execution=execution, mission_start_time=None, mission_queue_id=None, @@ -232,7 +235,7 @@ def check_pickup(self, pickup: Pickup): if pickup.mission_start_time is not None: pickup.mission_start_time = None # Check if robot docked under the correct cart - cart_check = self.is_correct_cart() + cart_check = self.is_correct_cart(pickup.cart_id) if cart_check: # If cart is correct, send pickup mission assert self.pickup_mission is not None @@ -327,6 +330,7 @@ def check_dropoff(self, dropoff: Dropoff): return def is_latch_open(self): + # Checks if the robot's latch is open and carrying a cart # Return True if latch is open, else False # ------------------------ # IMPLEMENT YOUR CODE HERE @@ -334,14 +338,16 @@ def is_latch_open(self): return False def is_under_cart(self): + # Checks if the robot is docked under a cart # Return True if robot is under any carts, else False # ------------------------ # IMPLEMENT YOUR CODE HERE # ------------------------ return False - def is_correct_cart(self): - # Return True if cart is correct, False if cart is wrong, None if no cart + def is_correct_cart(self, cart_id: str): + # Checks if the detected cart identifier matches the target cart_id + # Return True if cart is correct, False if cart is wrong, None if no cart detected # ------------------------ # IMPLEMENT YOUR CODE HERE # ------------------------ diff --git a/fleet_adapter_mir_tasks/LICENSE b/fleet_adapter_mir_tasks/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/fleet_adapter_mir_tasks/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/__init__.py b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py new file mode 100644 index 0000000..8f561ea --- /dev/null +++ b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python3 + +# Copyright 2022 Open Source Robotics Foundation, Inc. +# +# 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. + +import sys +import uuid +import argparse +import json +import asyncio + +import rclpy +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.qos import qos_profile_system_default +from rclpy.qos import QoSProfile +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSReliabilityPolicy as Reliability + +from rmf_task_msgs.msg import ApiRequest, ApiResponse + + +############################################################################### + +class TaskRequester(Node): + + def __init__(self, argv=sys.argv): + super().__init__('task_requester') + parser = argparse.ArgumentParser() + parser.add_argument('-F', '--fleet', required=False, default='', + type=str, help='Fleet name') + parser.add_argument('-R', '--robot', required=False, default='', + type=str, help='Robot name') + parser.add_argument('-c', '--cart_id', required=True, + type=str, help='Cart identifier') + parser.add_argument('-p', '--pickup_lot', required=True, + type=str, help='Pick up place') + parser.add_argument('-d', '--dropoff_lot', required=True, + type=str, help='Dropoff lot waypoint name') + parser.add_argument('-e', '--emergency_lots', required=False, default='', + type=str, nargs='+', help='Emergency waypoints') + parser.add_argument('-st', '--start_time', + help='Start time from now in secs, default: 0', + type=int, default=0) + parser.add_argument('-pt', '--priority', + help='Priority value for this request', + type=int, default=0) + parser.add_argument("--use_sim_time", action="store_true", + help='Use sim time, default: false') + + self.args = parser.parse_args(argv[1:]) + self.response = asyncio.Future() + + transient_qos = QoSProfile( + history=History.KEEP_LAST, + depth=1, + reliability=Reliability.RELIABLE, + durability=Durability.TRANSIENT_LOCAL) + + self.pub = self.create_publisher( + ApiRequest, 'task_api_requests', transient_qos) + + # enable ros sim time + if self.args.use_sim_time: + self.get_logger().info("Using Sim Time") + param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + self.set_parameters([param]) + + # Construct task + msg = ApiRequest() + msg.request_id = f"delivery_pickup_" + str(uuid.uuid4()) + payload = {} + if self.args.fleet and self.args.robot: + payload["type"] = "robot_task_request" + payload["robot"] = self.args.robot + payload["fleet"] = self.args.fleet + else: + payload["type"] = "dispatch_task_request" + request = {} + + # Set task request start time + now = self.get_clock().now().to_msg() + now.sec = now.sec + self.args.start_time + start_time = now.sec * 1000 + round(now.nanosec/10**6) + request["unix_millis_earliest_start_time"] = start_time + # todo(YV): Fill priority after schema is added + + # Define task request category + request["category"] = "compose" + + # Define task request description with phases + description = {} # task_description_Compose.json + description["category"] = "delivery_pickup" + description["phases"] = [] + activities = [] + + pickup_lot = self.args.pickup_lot + + # on_cancel = [] + # emergency_lots = [] + # if self.args.emergency_lots: + # for lot in self.args.emergency_lots: + # emergency_lots.append({"waypoint": lot}) + # cancellation_desc = { + # "one_of": emergency_lots, + # "constraints": [{ + # "category": "prefer_same_map", + # "description": "some description" + # }] + # } + # cancellation_activities = [] + # cancellation_activities.append({"category": "perform_action", + # "description": { + # "unix_millis_action_duration_estimate": 60000, + # "category": "dropoff_if_carrying_cart", + # "description": {} + # }}) + # cancellation_activities.append({"category": "go_to_place", + # "description": cancellation_desc}) + # cancellation_activities.append({"category": "perform_action", + # "description": { + # "unix_millis_action_duration_estimate": 60000, + # "category": "delivery_dropoff", + # "description": {} + # }}) + # on_cancel.append( + # {"category": "sequence", + # "description": cancellation_activities}) + + # Add activities + activities.append({"category": "go_to_place", + "description": pickup_lot}) + activities.append({"category": "perform_action", + "description": { + "unix_millis_action_duration_estimate": 60000, + "category": "delivery_pickup", + "description": { + "cart_id": self.args.cart_id, + "pickup_lot": pickup_lot + }}}) + activities.append({"category": "go_to_place", + "description": self.args.dropoff_lot}) + activities.append({"category": "perform_action", + "description": { + "unix_millis_action_duration_estimate": 60000, + "category": "delivery_dropoff", + "description": {} + }}) + # Add activities to phases + description["phases"].append( + {"activity": {"category": "sequence", + "description": {"activities": activities}}, + "on_cancel": on_cancel}) + request["description"] = description + payload["request"] = request + msg.json_msg = json.dumps(payload) + + def receive_response(response_msg: ApiResponse): + if response_msg.request_id == msg.request_id: + self.response.set_result(json.loads(response_msg.json_msg)) + + transient_qos.depth = 10 + self.sub = self.create_subscription( + ApiResponse, 'task_api_responses', receive_response, transient_qos + ) + + print(f"Json msg payload: \n{json.dumps(payload, indent=2)}") + self.pub.publish(msg) + + +############################################################################### + + +def main(argv=sys.argv): + rclpy.init(args=sys.argv) + args_without_ros = rclpy.utilities.remove_ros_args(sys.argv) + + task_requester = TaskRequester(args_without_ros) + rclpy.spin_until_future_complete( + task_requester, task_requester.response, timeout_sec=5.0) + if task_requester.response.done(): + print(f'Got response:\n{task_requester.response.result()}') + else: + print('Did not get a response') + rclpy.shutdown() + + +if __name__ == '__main__': + main(sys.argv) diff --git a/fleet_adapter_mir_tasks/package.xml b/fleet_adapter_mir_tasks/package.xml new file mode 100644 index 0000000..b492141 --- /dev/null +++ b/fleet_adapter_mir_tasks/package.xml @@ -0,0 +1,18 @@ + + + + fleet_adapter_mir_tasks + 0.1.0 + RMF fleet adapter custom tasks for MiR robots + Xiyu + Apache License 2.0 + + rmf_fleet_msgs + rmf_task_msgs + rmf_dispenser_msgs + rmf_lift_msgs + + + ament_python + + diff --git a/fleet_adapter_mir_tasks/resource/fleet_adapter_mir_tasks b/fleet_adapter_mir_tasks/resource/fleet_adapter_mir_tasks new file mode 100644 index 0000000..e69de29 diff --git a/fleet_adapter_mir_tasks/setup.cfg b/fleet_adapter_mir_tasks/setup.cfg new file mode 100644 index 0000000..3462068 --- /dev/null +++ b/fleet_adapter_mir_tasks/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/fleet_adapter_mir_tasks +[install] +install_scripts=$base/lib/fleet_adapter_mir_tasks diff --git a/fleet_adapter_mir_tasks/setup.py b/fleet_adapter_mir_tasks/setup.py new file mode 100644 index 0000000..302fba9 --- /dev/null +++ b/fleet_adapter_mir_tasks/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'fleet_adapter_mir_tasks' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Xiyu', + maintainer_email='xiyu@openrobotics.org', + description='RMF fleet adapter custom tasks for MiR robots', + license='Apache License 2.0', + entry_points={ + 'console_scripts': [ + 'dispatch_pickup = fleet_adapter_mir_tasks.dispatch_pickup:main' + ], + }, +) From 9d928961367520977f2c94a81f657a752d2a42cb Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 12 Jan 2024 18:21:55 +0800 Subject: [PATCH 06/50] Footprint and marker type things and update readme Signed-off-by: Xiyu Oh --- README.md | 18 +- configs/mir_config.yaml | 18 +- fleet_adapter_mir/README.md | 66 +- .../fleet_adapter_mir/mir_api.py | 4 + .../fleet_adapter_mir/rmf_cart_delivery.py | 14 +- .../fleet_adapter_mir/robot_adapter_mir.py | 9 +- .../dispatch_pickup.py | 74 +- missions/rmf_cart_missions.json | 630 +----------------- 8 files changed, 133 insertions(+), 700 deletions(-) diff --git a/README.md b/README.md index f94095b..2155ba0 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # fleet_adapter_mir -MiR100 and MirFM Fleet Adapter using the https://github.com/open-rmf/rmf_ros2/tree/main/rmf_fleet_adapter_python +MiR100, 200 and 250 using the https://github.com/open-rmf/rmf_ros2/tree/main/rmf_fleet_adapter_python @@ -30,24 +30,13 @@ colcon build ## Description -These packages implement a MiR robot/MiR Fleet command handle that is managed by a fleet adapter in Python. (Along with some helpers.) It can be used to command and manage a fleet of MiR robots using RMF! +These packages implement a MiR command handle that is managed by a fleet adapter in Python. (Along with some helpers.) It can be used to command and manage a fleet of MiR robots using RMF! It uses the `rmf_fleet_adapter_python` bindings, which allows for communication with `open-rmf` libraries and ROS2 nodes. In effect, it interfaces the MiR REST API with `open-rmf`, all without needing to directly use ROS 2 messages to communicate with `open-rmf`! - -### MiR vs. MiR Fleet - -Since the MiR robots and MiR Fleet work with different sets of endpoints that serve different functions, both `fleet_adapter_mir` and `fleet_adapter_mirfm` packages are availble to demonstrate RMF integration between MiR100 and MiR Fleet respectively. In addition, the `mir_fleet_client` package provides additional API needed for the MiR Fleet + RMF integration. - -For users who wish to take advantage of MiR Fleet's centralized control system/interface or do not have access to individual MiR robots, the `fleet_adapter_mirfm` package enables RMF integration with MiR Fleet by obtaining individual MiR robot positions and mission GUIDs via the MiR Fleet API and dispatching commands directly to the robots themselves. The current MiR Fleet API does not provide all the necessary endpoints for RMF to perform its task allocation and traffic deconfliction to the full extent, hence the implementation contains direct communication between RMF and MiR robots as well. - -As such, it is recommended to implement the fleet adapter directly with individual MiR robots using `fleet_adapter_mir`. - - - ## Additional Notes ### Different Units for Angles @@ -83,7 +72,8 @@ We have to deal with the `rmf_traffic` navgraph and the MiR map, with their own Luckily, we can leverage the `nudged` Python library to compute coordinate transforms between the two sets of coordinates. You just have to make sure to provide equivalent reference points within the two maps so transforms can be computed. -The transform objects are stored in the robot command handle's `self.transforms` member. +The transform objects are stored in the EasyFullControl fleet adapter handle. + diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index 1af76aa..9089a1c 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -96,14 +96,16 @@ plugins: rmf_cart_delivery: actions: ["delivery_pickup", "delivery_dropoff", "dropoff_if_carrying_cart"] missions: - dock_to_cart: "rmf_dock_to_cart" - pickup: "rmf_pickup_cart_top" - dropoff: "rmf_dropoff_cart_top" - exit_lot: "rmf_exit_lot" - update_footprint: "rmf_update_footprint" + dock_to_cart: "rmf_dock_to_cart" # Name of MiR mission for robot to dock under a cart - to be created by fleet adapter upon launch + pickup: "rmf_pickup_cart" # Name of MiR mission for robot to latch onto a cart - to be created and filled in by user + dropoff: "rmf_dropoff_cart" # Name of MiR mission for robot to unlatch and exit from under a cart - to be created and filled in by user + exit_lot: "rmf_exit_lot" # Name of MiR mission for robot to exit from under a cart - to be created by fleet adapter upon launch + update_footprint: "rmf_update_footprint" # Name of MiR mission for robot to update the footprint of a robot - to be created by fleet adapter upon launch footprints: - robot: "MiR100-200" - cart: "HostShelfCart800" - missions_json: "/home/some_user/mir_ws/src/fleet_adapter_mir/missions/rmf_cart_missions.json" + robot: "MiR100-200" # Name of robot footprint stored on MiR + cart: "ShelfCart" # Name of cart footprint stored on MiR + marker_types: + cart: "AsymmetricShelf" # Name of cart marker type + missions_json: "/home/some_user/mir_ws/src/fleet_adapter_mir/missions/rmf_cart_missions.json" # Filepath to RMF cart delivery missions search_timeout: 60 pickup_dist_threshold: 3.0 # metres diff --git a/fleet_adapter_mir/README.md b/fleet_adapter_mir/README.md index 3aa54b8..a70dc16 100644 --- a/fleet_adapter_mir/README.md +++ b/fleet_adapter_mir/README.md @@ -41,8 +41,72 @@ Alternatively, if you want to run everything with full capabilities, (though not ros2 run fleet_adapter_mir fleet_adapter_mir -c mir_config.yaml -n nav_graph.yaml ``` +If you have not configured the necessary RMF missions on the MiR, you may parse the relevant JSON filepaths when launching the fleet adapter node. On startup, the fleet adapter will create these missions via MiR REST API + +```bash +ros2 run fleet_adapter_mir fleet_adapter_mir -c mir_config.yaml -n nav_graph.yaml -a ../missions/rmf_missions.json +``` + ### Configuration -An example configuration file, `mir_config.yaml` has been provided. It has been generously commented, and in the cases where it has not, the parameter names are self-explanatory enough. \ No newline at end of file +An example configuration file, `mir_config.yaml` has been provided. It has been generously commented, and in the cases where it has not, the parameter names are self-explanatory enough. + + + +### Setting up building maps + +**Chargers** + +To use the `rmf_dock_and_charge` mission for charging, you may provide a description similar to the following when adding a `dock_name` to the charging point in your building map yaml/navigation graph: +```json +{"description": {"end_waypoint": "charger_name"}, "mission_name": "rmf_dock_and_charge"} +``` +Where you replace `end_waypoint` with the name of your MiR charger. + + +**MiR positions** + +For more accurate robot maneuver, you may wish to send the MiR to a Robot Position instead of using coordinates. For such waypoints, you can provide a `dock_name` with the `rmf_move_to_position` mission specified: +```json +{"description": {"end_waypoint": "waypoint_name"}, "mission_name": "rmf_move_to_position"} +``` + + + +### Plugins + +The MiR is capable of performing various types of custom missions and tasks. You can now easily set up plugins offered in this repo instead of writing your own perform action for common use cases. + +For cart deliveries from point A to B: + +**rmf_cart_delivery** + +The `rmf_cart_delivery` plugin allows users to submit pickup and dropoff tasks to MiR integrated with RMF. The workflow of the task is as follows: +1. RMF will send the robot to the pickup lot +2. The robot will attempt to dock under a cart in the pickup lot +3. If the robot successfully docks under the correct cart, it will proceed to deliver it to a dropoff point. If the cart is missing or is not the desired cart, RMF will cancel the task. + +Some relevant MiR missions (docking, exit, update footprint) will be automatically created on the MiR on startup. These missions are used to facilitate the pickup and dropoff activities and can be found in the plugin config under `missions`. They are: +- `rmf_dock_to_cart`: Docks robot under the cart +- `rmf_exit_lot`: Calls the robot to exit from under the cart +- `rmf_update_footprint`: Updates the robot footprint +They are defined and stored in the `rmf_cart_missions.json` file and do not require any further configuration. + +However, since there are various types of latching methods available for different MiR models, users will need to set up their custom pickup and dropoff missions on the MiR: +1. Create 2 missions on the MiR: + - `rmf_pickup_cart`: Triggers the robot's latching module to open + - `rmf_dropoff_cart`: Triggers the robot's latching module to close and release the cart, then exit from under the cart (relative move -1 metre in the X-direction) +2. Fill in the MiR mission names in the plugin config under `missions`. The default mission names are `rmf_pickup_cart` and `rmf_dropoff_cart`. +3. Fill in the footprints and marker types to be used for your specific robot and cart in the plugin config. +4. In `rmf_cart_delivery.py`, fill in the logic to check whether the robot's latching module is open in blanks marked `# IMPLEMENT YOUR CODE HERE`. Some API calls to check the MiR's PLC registers and IO modules are provided in case you may want to use them. + +To submit a cart delivery task, you may use the `dispatch_pickup` task script: +```bash +ros2 run fleet_adapter_mir_tasks dispatch_pickup -g go_to_waypoint -p pickup_lot -d dropoff_lot -c some_cart_id +``` +- `-g`: Takes in an existing waypoint name for the robot to travel to before performing the pickup +- `-p`: Name of the pickup lot. This name should be identical to the shelf position configured on the MiR. +- `-d`: Name of the dropoff lot. This name should be identical to the robot or shelf position configured on the MiR. +- `-c`: Optional cart identifier for the fleet adapter to assess whether the cart is correct for pickup. diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py index 5337662..5201b22 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py @@ -227,6 +227,10 @@ def create_rmf_mission_actions(self, mission_name: str, mission_id: str, mission if param_body['id'] == 'marker_type': default_marker = self.docking_offsets_get()[0]['guid'] param_body['value'] = default_marker + # If the input type is a footprint, we'll also use a placeholder footprint GUID + if param_body['id'] == 'footprint': + default_footprint = self.footprints_get()[0]['guid'] + param_body['value'] = default_footprint action_body_param.append(param_body) diff --git a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py index 078d1a6..47f0c60 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py +++ b/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py @@ -69,9 +69,10 @@ def __init__( self.exit_mission = self.action_config['missions']['exit_lot'] self.footprint_mission = self.action_config['missions']['update_footprint'] - # Initialize footprints + # Initialize footprints and marker types self.robot_footprint_guid = self.api.footprints_guid_get(self.action_config['footprints']['robot']) self.cart_footprint_guid = self.api.footprints_guid_get(self.action_config['footprints']['cart']) + self.cart_marker_type_guid = self.api.docking_offsets_guid_get(self.action_config['marker_types']['cart']) self.update_footprint(self.robot_footprint_guid) def update_action(self): @@ -185,10 +186,9 @@ def check_pickup(self, pickup: Pickup): assert self.dock_to_cart_mission is not None current_wp_name = pickup.pickup_lots[0] cart_marker_guid = self.api.known_positions[current_wp_name]['guid'] - mission_params = self.api.get_mission_params_with_value(self.dock_to_cart_mission, - 'docking', - 'cart_marker', - cart_marker_guid) + cart_marker_param = self.api.get_mission_params_with_value(self.dock_to_cart_mission, 'docking', 'cart_marker', cart_marker_guid) + marker_type_param = self.api.get_mission_params_with_value(self.dock_to_cart_mission, 'docking', 'cart_marker_type', self.cart_marker_type_guid) + mission_params = cart_marker_param + marker_type_param self.update_footprint(self.robot_footprint_guid) mission_queue_id = self.api.queue_mission_by_name(self.dock_to_cart_mission, mission_params) if not mission_queue_id: @@ -264,6 +264,8 @@ def check_pickup(self, pickup: Pickup): # Latch successfully opened, indicate pickup as success pickup.state = PickupState.PICKUP_SUCCESS pickup.latching = False + # Update robot footprint to accommodate the cart size + self.update_footprint(self.cart_footprint_guid) case PickupState.PICKUP_SUCCESS: # Correct ID, we can end the delivery now @@ -322,6 +324,8 @@ def check_dropoff(self, dropoff: Dropoff): else: if self.api.mission_completed(dropoff.mission_queue_id): self.node.get_logger().info(f'Dropoff mission completed!') + # Update robot footprint + self.update_footprint(self.robot_footprint_guid) if dropoff.execution is not None: dropoff.execution.finished() return True diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index db7948d..7a747f2 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -137,7 +137,7 @@ def __init__( mir_api=self.api, update_handle=self.update_handle, actions=plugin_config['rmf_cart_delivery']['actions'], - missions_json=plugin_config['rmf_cart_delivery']['missions_json'], + missions_json=plugin_config['rmf_cart_delivery'].get('missions_json'), action_config=plugin_config['rmf_cart_delivery'], retrieve_mir_coordinates=self.retrieve_mir_coordinates) # To be added on with other plugins @@ -338,13 +338,6 @@ def navigate(self, destination, execution): self.mission = MissionHandle(execution) self.request_dock(dock_json, self.mission) return - # TODO(XY): remove this hack and make sure dock missions are passed properly to - # the nav command - elif destination.name and 'Charger' in destination.name: - self.mission = MissionHandle(execution, charger=destination.name) - dock_json = {'description': {'end_waypoint': destination.name}, - 'mission_name': self.api.dock_and_charge_mission} - self.request_dock(dock_json, self.mission) if destination.inside_lift is not None: positions_for_lift = self.lift_positions.get(destination.inside_lift.name) diff --git a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py index 8f561ea..d7ee46c 100644 --- a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py +++ b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py @@ -43,12 +43,14 @@ def __init__(self, argv=sys.argv): type=str, help='Fleet name') parser.add_argument('-R', '--robot', required=False, default='', type=str, help='Robot name') - parser.add_argument('-c', '--cart_id', required=True, + parser.add_argument('-c', '--cart_id', required=False, default='', type=str, help='Cart identifier') + parser.add_argument('-g', '--go_to', required=True, + type=str, help='Waypoint name to go to for pickup') parser.add_argument('-p', '--pickup_lot', required=True, - type=str, help='Pick up place') + type=str, help='Name of pickup lot') parser.add_argument('-d', '--dropoff_lot', required=True, - type=str, help='Dropoff lot waypoint name') + type=str, help='Name of dropoff lot') parser.add_argument('-e', '--emergency_lots', required=False, default='', type=str, nargs='+', help='Emergency waypoints') parser.add_argument('-st', '--start_time', @@ -106,49 +108,47 @@ def __init__(self, argv=sys.argv): description["phases"] = [] activities = [] - pickup_lot = self.args.pickup_lot - - # on_cancel = [] - # emergency_lots = [] - # if self.args.emergency_lots: - # for lot in self.args.emergency_lots: - # emergency_lots.append({"waypoint": lot}) - # cancellation_desc = { - # "one_of": emergency_lots, - # "constraints": [{ - # "category": "prefer_same_map", - # "description": "some description" - # }] - # } - # cancellation_activities = [] - # cancellation_activities.append({"category": "perform_action", - # "description": { - # "unix_millis_action_duration_estimate": 60000, - # "category": "dropoff_if_carrying_cart", - # "description": {} - # }}) - # cancellation_activities.append({"category": "go_to_place", - # "description": cancellation_desc}) - # cancellation_activities.append({"category": "perform_action", - # "description": { - # "unix_millis_action_duration_estimate": 60000, - # "category": "delivery_dropoff", - # "description": {} - # }}) - # on_cancel.append( - # {"category": "sequence", - # "description": cancellation_activities}) + on_cancel = [] + emergency_lots = [] + if self.args.emergency_lots: + for lot in self.args.emergency_lots: + emergency_lots.append({"waypoint": lot}) + cancellation_desc = { + "one_of": emergency_lots, + "constraints": [{ + "category": "prefer_same_map", + "description": "some description" + }] + } + cancellation_activities = [] + cancellation_activities.append({"category": "perform_action", + "description": { + "unix_millis_action_duration_estimate": 60000, + "category": "dropoff_if_carrying_cart", + "description": {} + }}) + cancellation_activities.append({"category": "go_to_place", + "description": cancellation_desc}) + cancellation_activities.append({"category": "perform_action", + "description": { + "unix_millis_action_duration_estimate": 60000, + "category": "delivery_dropoff", + "description": {} + }}) + on_cancel.append( + {"category": "sequence", + "description": cancellation_activities}) # Add activities activities.append({"category": "go_to_place", - "description": pickup_lot}) + "description": self.args.go_to}) activities.append({"category": "perform_action", "description": { "unix_millis_action_duration_estimate": 60000, "category": "delivery_pickup", "description": { "cart_id": self.args.cart_id, - "pickup_lot": pickup_lot + "pickup_lot": self.args.pickup_lot }}}) activities.append({"category": "go_to_place", "description": self.args.dropoff_lot}) diff --git a/missions/rmf_cart_missions.json b/missions/rmf_cart_missions.json index 86ce67c..d710062 100644 --- a/missions/rmf_cart_missions.json +++ b/missions/rmf_cart_missions.json @@ -47,8 +47,8 @@ "id": "marker" }, { - "value": "cbd0de8c-467c-11ee-899f-00012983ef2c", - "input_name": null, + "value": "some_placeholder_marker_type_guid", + "input_name": "cart_marker_type", "id": "marker_type" }, { @@ -65,636 +65,12 @@ "action_type": "docking" } ], - "rmf_dropoff_cart_side": [ - { - "priority": 2, - "parameters": [ - { - "value": "mirconst-guid-0000-0001-footprint000", - "input_name": null, - "id": "footprint" - } - ], - "action_type": "set_footprint" - }, - { - "priority": 3, - "parameters": [ - { - "value": -1.0, - "input_name": null, - "id": "x" - }, - { - "value": 0.0, - "input_name": null, - "id": "y" - }, - { - "value": 0.0, - "input_name": null, - "id": "orientation" - }, - { - "value": 0.25, - "input_name": null, - "id": "max_linear_speed" - }, - { - "value": 0.25, - "input_name": null, - "id": "max_angular_speed" - }, - { - "value": true, - "input_name": null, - "id": "collision_detection" - } - ], - "action_type": "relative_move" - }, - { - "priority": 1, - "parameters": [ - { - "value": "mirconst-guid-0000-0011-actionlist00", - "input_name": null, - "id": "mission_id" - } - ], - "action_type": "load_mission" - } - ], - "rmf_dropoff_cart_top": [ - { - "priority": 1, - "parameters": [ - { - "value": "io_module", - "input_name": null, - "id": "compare" - }, - { - "value": "mirconst-guid-0000-0001-internalIO00", - "input_name": null, - "id": "module" - }, - { - "value": 3, - "input_name": null, - "id": "io_port" - }, - { - "value": "1", - "input_name": null, - "id": "register" - }, - { - "value": "==", - "input_name": null, - "id": "operator" - }, - { - "value": 0, - "input_name": null, - "id": "value" - }, - { - "value": "", - "input_name": null, - "id": "true" - }, - { - "value": "", - "input_name": null, - "id": "false" - } - ], - "action_type": "if" - }, - { - "priority": 2, - "parameters": [ - { - "value": "No shelf is lifted according to IO input 3", - "input_name": null, - "id": "message" - } - ], - "action_type": "throw_error" - }, - { - "priority": 3, - "parameters": [ - { - "value": null, - "input_name": null, - "id": "sound" - }, - { - "value": null, - "input_name": null, - "id": "volume" - }, - { - "value": "muted", - "input_name": null, - "id": "front" - }, - { - "value": "muted", - "input_name": null, - "id": "rear" - }, - { - "value": "muted", - "input_name": null, - "id": "sides" - }, - { - "value": "", - "input_name": null, - "id": "content" - } - ], - "action_type": "reduce_protective_fields" - }, - { - "priority": 8, - "parameters": [ - { - "value": "mirconst-guid-0000-0001-footprint007", - "input_name": null, - "id": "footprint" - } - ], - "action_type": "set_footprint" - }, - { - "priority": 9, - "parameters": [ - { - "value": -1, - "input_name": null, - "id": "x" - }, - { - "value": 0, - "input_name": null, - "id": "y" - }, - { - "value": 0, - "input_name": null, - "id": "orientation" - }, - { - "value": 0.25, - "input_name": null, - "id": "max_linear_speed" - }, - { - "value": 0.25, - "input_name": null, - "id": "max_angular_speed" - }, - { - "value": true, - "input_name": null, - "id": "collision_detection" - } - ], - "action_type": "relative_move" - }, - { - "priority": 10, - "parameters": [ - { - "value": "577eaace-be75-11ed-a404-0001297c2278", - "input_name": null, - "id": "footprint" - } - ], - "action_type": "set_footprint" - }, - { - "priority": 4, - "parameters": [ - { - "value": "mirconst-guid-0000-0001-internalIO00", - "input_name": null, - "id": "module" - }, - { - "value": 3, - "input_name": null, - "id": "port" - }, - { - "value": "off", - "input_name": null, - "id": "operation" - }, - { - "value": null, - "input_name": null, - "id": "timeout" - } - ], - "action_type": "set_io" - }, - { - "priority": 5, - "parameters": [ - { - "value": "00:00:01.000000", - "input_name": null, - "id": "time" - } - ], - "action_type": "wait" - }, - { - "priority": 6, - "parameters": [ - { - "value": "mirconst-guid-0000-0001-internalIO00", - "input_name": null, - "id": "module" - }, - { - "value": 2, - "input_name": null, - "id": "port" - }, - { - "value": "on", - "input_name": null, - "id": "operation" - }, - { - "value": "00:00:10.000000", - "input_name": null, - "id": "timeout" - }, - { - "value": "", - "input_name": null, - "id": "content" - } - ], - "action_type": "set_reset_io" - }, - { - "priority": 7, - "parameters": [ - { - "value": "mirconst-guid-0000-0001-internalIO00", - "input_name": null, - "id": "module" - }, - { - "value": 2, - "input_name": null, - "id": "port" - }, - { - "value": "on", - "input_name": null, - "id": "value" - }, - { - "value": "00:00:10.000000", - "input_name": null, - "id": "timeout" - } - ], - "action_type": "wait_for_io" - } - ], - "rmf_pickup_cart_side": [ - { - "priority": 1, - "parameters": [ - { - "value": "13", - "input_name": null, - "id": "register" - }, - { - "value": "set", - "input_name": null, - "id": "action" - }, - { - "value": 0.0, - "input_name": null, - "id": "value" - } - ], - "action_type": "set_plc_register" - }, - { - "priority": 2, - "parameters": [ - { - "value": "14", - "input_name": null, - "id": "register" - }, - { - "value": 1.0, - "input_name": null, - "id": "value" - }, - { - "value": 0.0, - "input_name": null, - "id": "reset_value" - }, - { - "value": "", - "input_name": null, - "id": "content" - } - ], - "action_type": "set_reset_plc" - }, - { - "priority": 3, - "parameters": [ - { - "value": "8", - "input_name": null, - "id": "register" - }, - { - "value": 1.0, - "input_name": null, - "id": "value" - }, - { - "value": "00:01:00.000000", - "input_name": null, - "id": "timeout" - } - ], - "action_type": "wait_for_plc_register" - }, - { - "priority": 4, - "parameters": [ - { - "value": "f65abbec-ea4e-11ed-af33-00012983ef2c", - "input_name": null, - "id": "footprint" - } - ], - "action_type": "set_footprint" - } - ], - "rmf_pickup_cart_top": [ - { - "priority": 7, - "parameters": [ - { - "value": "mirconst-guid-0000-0001-footprint007", - "input_name": null, - "id": "footprint" - } - ], - "action_type": "set_footprint" - }, - { - "priority": 8, - "parameters": [ - { - "value": null, - "input_name": null, - "id": "sound" - }, - { - "value": null, - "input_name": null, - "id": "volume" - }, - { - "value": "muted", - "input_name": null, - "id": "front" - }, - { - "value": "muted", - "input_name": null, - "id": "rear" - }, - { - "value": "muted", - "input_name": null, - "id": "sides" - }, - { - "value": "", - "input_name": null, - "id": "content" - } - ], - "action_type": "reduce_protective_fields" - }, - { - "priority": 9, - "parameters": [ - { - "value": 0, - "input_name": null, - "id": "x" - }, - { - "value": 0, - "input_name": null, - "id": "y" - }, - { - "value": 0, - "input_name": null, - "id": "orientation" - }, - { - "value": 0.25, - "input_name": null, - "id": "max_linear_speed" - }, - { - "value": 0.25, - "input_name": null, - "id": "max_angular_speed" - }, - { - "value": true, - "input_name": null, - "id": "collision_detection" - } - ], - "action_type": "relative_move" - }, - { - "priority": 1, - "parameters": [ - { - "value": "io_module", - "input_name": null, - "id": "compare" - }, - { - "value": "mirconst-guid-0000-0001-internalIO00", - "input_name": null, - "id": "module" - }, - { - "value": 2, - "input_name": null, - "id": "io_port" - }, - { - "value": "1", - "input_name": null, - "id": "register" - }, - { - "value": "==", - "input_name": null, - "id": "operator" - }, - { - "value": 0, - "input_name": null, - "id": "value" - }, - { - "value": "", - "input_name": null, - "id": "true" - }, - { - "value": "", - "input_name": null, - "id": "false" - } - ], - "action_type": "if" - }, - { - "priority": 2, - "parameters": [ - { - "value": "A shelf is already lifted according to IO input 2", - "input_name": null, - "id": "message" - } - ], - "action_type": "throw_error" - }, - { - "priority": 3, - "parameters": [ - { - "value": "mirconst-guid-0000-0001-internalIO00", - "input_name": null, - "id": "module" - }, - { - "value": 2, - "input_name": null, - "id": "port" - }, - { - "value": "off", - "input_name": null, - "id": "operation" - }, - { - "value": null, - "input_name": null, - "id": "timeout" - } - ], - "action_type": "set_io" - }, - { - "priority": 4, - "parameters": [ - { - "value": "00:00:01.000000", - "input_name": null, - "id": "time" - } - ], - "action_type": "wait" - }, - { - "priority": 5, - "parameters": [ - { - "value": "mirconst-guid-0000-0001-internalIO00", - "input_name": null, - "id": "module" - }, - { - "value": 3, - "input_name": null, - "id": "port" - }, - { - "value": "on", - "input_name": null, - "id": "operation" - }, - { - "value": "00:00:10.000000", - "input_name": null, - "id": "timeout" - }, - { - "value": "", - "input_name": null, - "id": "content" - } - ], - "action_type": "set_reset_io" - }, - { - "priority": 6, - "parameters": [ - { - "value": "mirconst-guid-0000-0001-internalIO00", - "input_name": null, - "id": "module" - }, - { - "value": 3, - "input_name": null, - "id": "port" - }, - { - "value": "on", - "input_name": null, - "id": "value" - }, - { - "value": "00:00:10.000000", - "input_name": null, - "id": "timeout" - } - ], - "action_type": "wait_for_io" - } - ], "rmf_update_footprint": [ { "priority": 1, "parameters": [ { - "value": "mirconst-guid-0000-0001-footprint000", + "value": "some_placeholder_footprint_guid", "input_name": "footprint", "id": "footprint" } From 946d98cfc7ce0de76ec249c22546c8f9f0697528 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Mon, 15 Jan 2024 17:09:56 +0800 Subject: [PATCH 07/50] Restore mir/mirfm comparison and add note that mirfm is not actively supported Signed-off-by: Xiyu Oh --- README.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/README.md b/README.md index 2155ba0..6508e15 100644 --- a/README.md +++ b/README.md @@ -37,6 +37,19 @@ It uses the `rmf_fleet_adapter_python` bindings, which allows for communication In effect, it interfaces the MiR REST API with `open-rmf`, all without needing to directly use ROS 2 messages to communicate with `open-rmf`! + +### MiR vs. MiR Fleet + +Since the MiR robots and MiR Fleet work with different sets of endpoints that serve different functions, both `fleet_adapter_mir` and `fleet_adapter_mirfm` packages are availble to demonstrate RMF integration between MiR100/200/250 and MiR Fleet respectively. In addition, the `mir_fleet_client` package provides additional API needed for the MiR Fleet + RMF integration. + +For users who wish to take advantage of MiR Fleet's centralized control system/interface or do not have access to individual MiR robots, the `fleet_adapter_mirfm` package enables RMF integration with MiR Fleet by obtaining individual MiR robot positions and mission GUIDs via the MiR Fleet API and dispatching commands directly to the robots themselves. The current MiR Fleet API does not provide all the necessary endpoints for RMF to perform its task allocation and traffic deconfliction to the full extent, hence the implementation contains direct communication between RMF and MiR robots as well. + +As such, it is recommended to implement the fleet adapter directly with individual MiR robots using `fleet_adapter_mir`. + +**NOTE**: `fleet_adapter_mirfm` is currently not being actively supported. + + + ## Additional Notes ### Different Units for Angles From 725fd0d9c287c1802c6ce374b314846c52118968 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 18 Jan 2024 15:23:06 +0800 Subject: [PATCH 08/50] Make marker type a variable when posting missions Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 2 ++ .../fleet_adapter_mir/mir_api.py | 32 +++++++++---------- missions/rmf_missions.json | 4 +-- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index 9089a1c..e222b30 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -46,6 +46,8 @@ conversions: L2: name: "Level_2_Lift" + marker_types: + "charger": "Narrow asymmetric MiR500/1000 shelf" # RMF Fleet parameters rmf_fleet: diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py index 5201b22..94fedb1 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py @@ -69,6 +69,7 @@ def __init__(self, prefix, headers, conversions, rmf_missions, timeout=10.0, deb self.mission_keys: dict = conversions['missions'] self.mission_actions: dict = {} self.mission_action_types: dict = {} + self.marker_type_keys: dict = conversions['marker_types'] self.attempt_connection() def attempt_connection(self): @@ -160,7 +161,7 @@ def update_known_positions(self): for pos in self.positions_get(): if pos['name'] in self.known_positions and \ pos['type_id'] == self.known_positions[pos['name']]['type_id'] and \ - ('rmf_localize' in pos['name'] or 'cgh_localize' in pos['name']): + ('rmf_localize' in pos['name']): # Delete any duplicate positions self.positions_guid_delete(pos['guid']) elif pos['type_id'] == MiRPositionTypes.ROBOT or \ @@ -301,7 +302,10 @@ def dock(self, mission_name, start_waypoint, end_waypoint): # Check whether we should dock into this end waypoint or not (for charging) elif dock_param: - mission_params = end_param + dock_param + charger_marker_type = self.marker_type_keys['charger'] + charger_marker_type_guid = self.docking_offsets_guid_get(charger_marker_type) + marker_param = self.get_mission_params_with_value(mission_name, 'docking', 'charger_marker_type', charger_marker_type_guid) + mission_params = end_param + dock_param + marker_param else: mission_params = end_param return self.queue_mission_by_name(mission_name, mission_params) @@ -313,10 +317,10 @@ def localize(self, map, estimate, index): ) if index is not None: - position_name = f'cgh_1810_localize_{index}' + position_name = f'rmf_localize_{index}' else: p = estimate - position_name = f'cgh_1810_localize_{map}_{p[0]:.2f}_{p[1]:.2f}' + position_name = f'rmf_localize_{map}_{p[0]:.2f}_{p[1]:.2f}' mir_map = self.map_conversions.rmf_to_mir[map] map_id = self.known_maps[mir_map] position_guid = self.get_position_guid(position_name, map_id, estimate) @@ -450,7 +454,7 @@ def positions_put(self, guid, name, map_id, location): def positions_delete(self): new_known_positions = {} for position in self.known_positions: - if 'rmf_localize' in position or 'cgh_localize' in position: + if 'rmf_localize' in position: pos_guid = self.known_positions[position] try: response = requests.delete( @@ -814,18 +818,12 @@ def docking_offsets_get(self): except Exception as err: print(f"Other error: {err}") - def docking_offsets_guid_get(self, guid: str): - if not self.connected: - return - try: - response = requests.get(self.prefix + f'docking_offsets/{guid}', headers=self.headers, timeout=self.timeout) - if self.debug: - print(f"Response: {response.json()}") - return response.json() - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - except Exception as err: - print(f"Other error: {err}") + def docking_offsets_guid_get(self, offset_name: str): + offsets = self.docking_offsets_get() + for offs in offsets: + if offs['name'] == offset_name: + return offs['guid'] + return None def footprints_get(self): if not self.connected: diff --git a/missions/rmf_missions.json b/missions/rmf_missions.json index 9ba25b3..a7df7d5 100644 --- a/missions/rmf_missions.json +++ b/missions/rmf_missions.json @@ -30,8 +30,8 @@ "id": "marker" }, { - "value": "mirconst-guid-0000-0001-marker000001", - "input_name": null, + "value": "some_placeholder_marker_type_guid", + "input_name": "charger_marker_type", "id": "marker_type" }, { From 8aeb014ec98e7d01ccb5ea11be683e734a42b69b Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 31 Jan 2024 11:58:53 +0800 Subject: [PATCH 09/50] Update dispatch pickup cancellation behavior and configs Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 4 +- .../fleet_adapter_mir/robot_adapter_mir.py | 2 +- .../dispatch_pickup.py | 51 +++++++++++-------- 3 files changed, 32 insertions(+), 25 deletions(-) diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index e222b30..1de4059 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -80,7 +80,7 @@ rmf_fleet: delivery: True clean: False finishing_request: "charge" # [park, charge, nothing] - actions: ["delivery_pickup", "delivery_dropoff", "dropoff_if_carrying_cart"] + actions: ["delivery_pickup", "delivery_dropoff"] robots: mir_1: charger: "Charger_1" @@ -96,7 +96,7 @@ rmf_fleet: plugins: rmf_cart_delivery: - actions: ["delivery_pickup", "delivery_dropoff", "dropoff_if_carrying_cart"] + actions: ["delivery_pickup", "delivery_dropoff"] missions: dock_to_cart: "rmf_dock_to_cart" # Name of MiR mission for robot to dock under a cart - to be created by fleet adapter upon launch pickup: "rmf_pickup_cart" # Name of MiR mission for robot to latch onto a cart - to be created and filled in by user diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 7a747f2..6edcd45 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -201,7 +201,7 @@ def request_update(self): self.api.status_put(MiRStateCode.READY) def is_charging(self, status: MirStatus): - # TODO(XY): check if we can verify that a robot is charging via mode_id instead + # Note: Not the best way to verify that robot is charging but there's currently no other option if status.response.get('mission_text') == "Charging... Waiting for new mission...": return True return False diff --git a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py index d7ee46c..bd310dc 100644 --- a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py +++ b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py @@ -106,8 +106,8 @@ def __init__(self, argv=sys.argv): description = {} # task_description_Compose.json description["category"] = "delivery_pickup" description["phases"] = [] - activities = [] + # Set up cancellation behavior on_cancel = [] emergency_lots = [] if self.args.emergency_lots: @@ -121,12 +121,6 @@ def __init__(self, argv=sys.argv): }] } cancellation_activities = [] - cancellation_activities.append({"category": "perform_action", - "description": { - "unix_millis_action_duration_estimate": 60000, - "category": "dropoff_if_carrying_cart", - "description": {} - }}) cancellation_activities.append({"category": "go_to_place", "description": cancellation_desc}) cancellation_activities.append({"category": "perform_action", @@ -139,30 +133,43 @@ def __init__(self, argv=sys.argv): {"category": "sequence", "description": cancellation_activities}) - # Add activities - activities.append({"category": "go_to_place", - "description": self.args.go_to}) - activities.append({"category": "perform_action", + # Pickup activity + pickup_activity = [] + pickup_activity.append({"category": "go_to_place", + "description": self.args.pickup_lot}) + pickup_activity.append({"category": "perform_action", "description": { "unix_millis_action_duration_estimate": 60000, - "category": "delivery_pickup", + "category": self.args.pickup_type, "description": { "cart_id": self.args.cart_id, "pickup_lot": self.args.pickup_lot }}}) - activities.append({"category": "go_to_place", - "description": self.args.dropoff_lot}) - activities.append({"category": "perform_action", - "description": { - "unix_millis_action_duration_estimate": 60000, - "category": "delivery_dropoff", - "description": {} - }}) - # Add activities to phases description["phases"].append( {"activity": {"category": "sequence", - "description": {"activities": activities}}, + "description": {"activities": pickup_activity}}}) + # GoToPlace activity + go_to_place_activity = [{ + "category": "go_to_place", + "description": self.args.dropoff_lot + }] + description["phases"].append( + {"activity": {"category": "sequence", + "description": {"activities": go_to_place_activity}}, "on_cancel": on_cancel}) + # Dropoff activity + dropoff_activity = [{ + "category": "perform_action", + "description": { + "unix_millis_action_duration_estimate": 60000, + "category": "delivery_dropoff", + "description": {} + } + }] + description["phases"].append( + {"activity": {"category": "sequence", + "description": {"activities": dropoff_activity}}}) + # Consolidate request["description"] = description payload["request"] = request msg.json_msg = json.dumps(payload) From b7f377092d49b55cd15cc56cb3593e0053640315 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Mon, 5 Feb 2024 14:31:54 +0800 Subject: [PATCH 10/50] Remove limits on charging mission Signed-off-by: Xiyu Oh --- missions/rmf_missions.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/missions/rmf_missions.json b/missions/rmf_missions.json index a7df7d5..1cd2652 100644 --- a/missions/rmf_missions.json +++ b/missions/rmf_missions.json @@ -51,12 +51,12 @@ "priority": 3, "parameters": [ { - "value": "00:10:00.000000", + "value": null, "input_name": null, "id": "minimum_time" }, { - "value": 25.0, + "value": null, "input_name": null, "id": "minimum_percentage" }, From 7b747f0a960677c877457ddd3ba6fef7d7448319 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 22 Feb 2024 12:41:05 +0800 Subject: [PATCH 11/50] Fixes bugs for cancelled pickup states, refactor mission info stored in pickup/dropoff, and move action-related code into its own package Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 2 +- .../fleet_adapter_mir_actions/__init__.py | 0 .../fleet_adapter_mir_actions}/mir_action.py | 24 +- .../rmf_cart_delivery.py | 214 +++++++++--------- fleet_adapter_mir_actions/package.xml | 15 ++ .../resource/fleet_adapter_mir_actions | 0 fleet_adapter_mir_actions/setup.cfg | 4 + fleet_adapter_mir_actions/setup.py | 24 ++ 8 files changed, 165 insertions(+), 118 deletions(-) create mode 100644 fleet_adapter_mir_actions/fleet_adapter_mir_actions/__init__.py rename {fleet_adapter_mir/fleet_adapter_mir => fleet_adapter_mir_actions/fleet_adapter_mir_actions}/mir_action.py (70%) rename {fleet_adapter_mir/fleet_adapter_mir => fleet_adapter_mir_actions/fleet_adapter_mir_actions}/rmf_cart_delivery.py (78%) create mode 100644 fleet_adapter_mir_actions/package.xml create mode 100644 fleet_adapter_mir_actions/resource/fleet_adapter_mir_actions create mode 100644 fleet_adapter_mir_actions/setup.cfg create mode 100644 fleet_adapter_mir_actions/setup.py diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 6edcd45..652779e 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -17,7 +17,7 @@ from threading import Lock # Import plugins -from .rmf_cart_delivery import CartDelivery +from ...fleet_adapter_mir_actions.fleet_adapter_mir_actions.rmf_cart_delivery import CartDelivery # Parallel processing solution derived from diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/__init__.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py similarity index 70% rename from fleet_adapter_mir/fleet_adapter_mir/mir_action.py rename to fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index e6ec961..62505bb 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -1,6 +1,6 @@ import json -from .mir_api import MirAPI, MirStatus, MiRStateCode +from ...fleet_adapter_mir.fleet_adapter_mir.mir_api import MirAPI, MirStatus, MiRStateCode class MirAction: @@ -10,31 +10,41 @@ def __init__( name, mir_api: MirAPI, update_handle, - actions: list[str], - missions_json: str | None, action_config: dict | None, ): self.node = node self.name = name self.api = mir_api self.update_handle = update_handle - self.actions = actions self.action_config = action_config + missions_json = self.action_config.get('missions_json') if missions_json: with open(missions_json, 'r') as g: - action_missions = json.load(g) + action_missions = json.load(g) + + # Check if these missions are already created on the robot + missions_created = True + for mission_name in action_missions.keys(): + if not mission_name in self.api.known_missions: + missions_created = False + break + if missions_created: + return + # Create these missions on the robot self.api.create_missions(action_missions) # Update mission actions stored in MirAPI for mission, mission_data in self.api.known_missions.items(): self.api.mission_actions[mission] = self.api.missions_mission_id_actions_get(mission_data['guid']) - def update_action(self): + # This will be called whenever an action has begun + def perform_action(self, category, description, execution): # To be populated in the plugins pass - def perform_action(self, category, description, execution): + # This will be called on every update to check on the action's current state + def update_action(self): # To be populated in the plugins pass diff --git a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py similarity index 78% rename from fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py rename to fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index 47f0c60..070f00e 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -6,36 +6,41 @@ import requests from urllib.error import HTTPError from .mir_action import MirAction -from .mir_api import MirAPI, MirStatus, MiRStateCode +from ...fleet_adapter_mir.fleet_adapter_mir.mir_api import MirAPI, MirStatus, MiRStateCode class PickupState(enum.IntEnum): - PICKUP_ALLOCATED = 0 - MOVE_REQUESTED = 1 - AT_PICKUP = 2 - DOCK_REQUESTED = 3 - DOCK_COMPLETED = 4 - PICKUP_REQUESTED = 5 - PICKUP_SUCCESS = 6 - WARNING_ALERT_PUBLISHED = 7 + PICKUP_INITIALIZED = 0 + PICKUP_ALLOCATED = 1 + MOVE_REQUESTED = 2 + AT_PICKUP = 3 + DOCK_REQUESTED = 4 + DOCK_COMPLETED = 5 + PICKUP_REQUESTED = 6 + PICKUP_SUCCESS = 7 TASK_CANCELLED = 8 +class Mission: + def __init__(self, queue_id: str, start_time: float): + self.queue_id = queue_id + self.start_time = start_time + + @dataclass class Pickup: state: PickupState pickup_lots: list[str] # contains either a single pickup lot or list of pickup lots cart_id: str execution: Any - mission_start_time: float - mission_queue_id: str + mission: Mission latching: bool @dataclass class Dropoff: execution: Any - mission_queue_id: str + mission: Mission class CartDelivery(MirAction): @@ -45,24 +50,18 @@ def __init__( name, mir_api, update_handle, - actions, - missions_json, action_config, - retrieve_mir_coordinates, # callback + retrieve_mir_coordinates # Function for plugin to retrieve information about how to convert between rmf and mir coordinates ): - MirAction.__init__(self, node, name, mir_api, update_handle, actions, - missions_json, action_config) - self.known_positions = self.api.known_positions + MirAction.__init__(self, node, name, mir_api, update_handle, action_config) - # Delivery related params - self.pickup: Pickup = None - self.dropoff: Dropoff = None + self.action: Pickup | Dropoff = None self.search_timeout = self.action_config.get('search_timeout', 60) # seconds # Useful robot adapter callbacks self.retrieve_mir_coordinates = retrieve_mir_coordinates - # Store the mission names to be used later + # Store the mission names to be used during the action self.dock_to_cart_mission = self.action_config['missions']['dock_to_cart'] self.pickup_mission = self.action_config['missions']['pickup'] self.dropoff_mission = self.action_config['missions']['dropoff'] @@ -75,75 +74,61 @@ def __init__( self.cart_marker_type_guid = self.api.docking_offsets_guid_get(self.action_config['marker_types']['cart']) self.update_footprint(self.robot_footprint_guid) - def update_action(self): - if self.pickup is not None: - if self.check_pickup(self.pickup): - self.pickup = None - if self.dropoff is not None: - if self.check_dropoff(self.dropoff): - self.dropoff = None - def perform_action(self, category, description, execution): - if category == 'delivery_pickup': - pickup_lot = description.get('pickup_lot') - cart_id = description.get('cart_id') - self.cart_pickup(execution, pickup_lot, cart_id) - elif category == 'delivery_dropoff': - self.node.get_logger().info(f'Received dropoff request!') - self.dropoff = Dropoff(execution=execution, mission_queue_id=None) - - def cancel_task(self, label: str = None): - def _cancel_success(): - if self.pickup: - self.pickup.state = PickupState.TASK_CANCELLED - def _cancel_fail(): - pass - self.cancel_current_task(_cancel_success, _cancel_fail, label) - - def cart_pickup(self, execution, pickup_lot: str, cart_id: str): - if self.pickup is not None: - # If there is an existing pickup, we'll replace it - if self.pickup.execution is not None and self.pickup.execution.okay(): - self.pickup.execution.finished() - self.pickup = None - - # Check if robot's latch is open - if self.is_latch_open(): - # Latch is open, unable to perform pickup - self.node.get_logger().info(f'Robot [{self.name}] latch is open, unable to perform pickup, cancelling task...') - self.cancel_task() - return + # Check that the perform action category matches + match category: + case 'delivery_pickup': + # Check if the robot's latch is currently open + if self.is_latch_open(): + # Latch is open, unable to perform pickup + self.node.get_logger().info(f'Robot [{self.name}] latch is open, unable to perform pickup, cancelling task...') + self.cancel_task() + return - self.node.get_logger().info(f'New pickup requested for robot [{self.name}]') + self.node.get_logger().info(f'New pickup requested for robot [{self.name}]') + self.pickup = Pickup( + state=PickupState.PICKUP_ALLOCATED, + pickup_lots=[description.get('pickup_lot')], # TODO(XY): Allow multiple pickups? + cart_id=description.get('cart_id'), + execution=execution, + mission=None, + latching=False + ) + case 'delivery_dropoff': + self.node.get_logger().info(f'Received dropoff request!') + self.dropoff = Dropoff( + execution=execution, + mission=None + ) + case _: + self.node.get_logger().info(f'Invalid perform action [{category}] passed to CartDelivery, ending action.') + execution.finished() - # TODO(XY): Allow multiple pickups? - pickup_lots = [pickup_lot] + def update_action(self): + # There should not be a pickup and dropoff being performed simultaneously at any given time, since actions are + # dispatched sequentially + if self.pickup: + return self.update_pickup(self.pickup) + elif self.dropoff: + return self.update_dropoff(self.dropoff) - self.pickup = Pickup( - state=PickupState.PICKUP_ALLOCATED, - pickup_lots=pickup_lots, - cart_id=cart_id, - execution=execution, - mission_start_time=None, - mission_queue_id=None, - latching=False - ) - return + return False - def check_pickup(self, pickup: Pickup): + def update_pickup(self, pickup: Pickup): # If this is a PerformAction pickup, check that the action is underway and valid if pickup.execution is not None and not pickup.execution.okay(): - self.node.get_logger().info(f'[{pickup.type}] action is killed/canceled.') + self.node.get_logger().info(f'[delivery_pickup] action is killed/canceled.') pickup.state = PickupState.TASK_CANCELLED # Start state machine check + now = self.node.get_clock().now().nanoseconds / 1e9 self.node.get_logger().debug(f'PickupState: {pickup.state.name}') match pickup.state: case PickupState.PICKUP_ALLOCATED: # Move to the first pickup place on the list pickup_lot = pickup.pickup_lots[0] self.node.get_logger().info(f'Requested to pickup cart at waypoint {pickup.state.name}') - if self.known_positions.get(pickup_lot) is None: + if self.api.known_positions.get(pickup_lot) is None: self.node.get_logger().info(f'Pickup Lot does not exist in MiR map, please resubmit.') self.cancel_task() pickup.state = PickupState.TASK_CANCELLED @@ -151,12 +136,12 @@ def check_pickup(self, pickup: Pickup): # Find the MiR coordinates of this pickup place mir_pose = self.retrieve_mir_coordinates(pickup_lot) - pickup.mission_queue_id = self.api.navigate(mir_pose) + pickup.mission = Mission(self.api.navigate(mir_pose), now) pickup.state = PickupState.MOVE_REQUESTED case PickupState.MOVE_REQUESTED: # Make sure that there is an rmf_move mission issued - if pickup.mission_queue_id is None: + if not pickup.mission: self.node.get_logger().info(f'Robot [{self.name}] is indicated to be at the MOVE_REQUESTED state but ' f'no mission queue ID stored for this pickup mission! Returning to PICKUP_ALLOCATED state.') pickup.state = PickupState.PICKUP_ALLOCATED @@ -164,9 +149,9 @@ def check_pickup(self, pickup: Pickup): # Robot has reached the pickup lot pickup_lot = pickup.pickup_lots[0] - if self.api.mission_completed(pickup.mission_queue_id): + if self.api.mission_completed(pickup.mission.queue_id): self.node.get_logger().info(f'Robot [{self.name}] has reached the pickup waypoint {pickup_lot}') - pickup.mission_queue_id = None + pickup.mission = None pickup.state = PickupState.AT_PICKUP return @@ -176,9 +161,9 @@ def check_pickup(self, pickup: Pickup): current_pose = self.api.status_get().state.position if self.dist(pickup_pose, current_pose) < self.action_config['pickup_dist_threshold']: # Delete the ongoing mission - self.api.mission_queue_id_delete(pickup.mission_queue_id) + self.api.mission_queue_id_delete(pickup.mission.queue_id) self.node.get_logger().info(f'Robot [{self.name}] is sufficiently near to the pickup waypoint {pickup_lot} for docking') - pickup.mission_queue_id = None + pickup.mission = None pickup.state = PickupState.AT_PICKUP case PickupState.AT_PICKUP: @@ -195,45 +180,40 @@ def check_pickup(self, pickup: Pickup): error_str = f'Mission {self.dock_to_cart_mission} not supported, ignoring' self.node.get_logger().error(error_str) return - pickup.mission_queue_id = mission_queue_id + pickup.mission = Mission(mission_queue_id, now) pickup.state = PickupState.DOCK_REQUESTED self.node.get_logger().info(f'Dock to cart mission requested with mission queue id {mission_queue_id}') case PickupState.DOCK_REQUESTED: # Make sure that there is an rmf_dock_to_cart mission issued - if pickup.mission_queue_id is None: + if not pickup.mission: self.node.get_logger().info(f'Robot [{self.name}] is indicated to be at the DOCK_REQUESTED state but ' f'no mission queue ID stored for this docking mission! Returning to AT_PICKUP state.') pickup.state = PickupState.AT_PICKUP return # Mission completed, move onto the next state - if self.api.mission_completed(pickup.mission_queue_id): - self.node.get_logger().info(f'Robot [{self.name}] dock to cart mission {pickup.mission_queue_id} completed or timed out.') - pickup.mission_queue_id = None - pickup.mission_start_time = None + if self.api.mission_completed(pickup.mission.queue_id): + self.node.get_logger().info(f'Robot [{self.name}] dock to cart mission {pickup.mission.queue_id} completed or timed out.') + pickup.mission = None pickup.state = PickupState.DOCK_COMPLETED return # Mission not yet completed, we check the timeout status to decide if we need to publish any alert - if pickup.mission_start_time is None: - pickup.mission_start_time = self.node.get_clock().now().nanoseconds / 1e9 - now = self.node.get_clock().now().nanoseconds / 1e9 - seconds_passed = now - pickup.mission_start_time + seconds_passed = now - pickup.mission.start_time # Publish update every 10 seconds just to monitor if round(seconds_passed)%10 == 0: self.node.get_logger().info(f'{round(seconds_passed)} seconds have passed since pickup mission requested.') # Mission timeout, cart not found if seconds_passed > self.search_timeout: # Delete mission from mir first - self.api.mission_queue_id_delete(pickup.mission_queue_id) + self.api.mission_queue_id_delete(pickup.mission.queue_id) # Regardless of whether the robot completed docking properly, we move to the next state to check - self.node.get_logger().info(f'Robot [{self.name}] dock to cart mission {pickup.mission_queue_id} timed out! ' + self.node.get_logger().info(f'Robot [{self.name}] dock to cart mission {pickup.mission.queue_id} timed out! ' f'Configured search timeout is {self.search_timeout} seconds.') + pickup.mission = None pickup.state = PickupState.DOCK_COMPLETED return case PickupState.DOCK_COMPLETED: - if pickup.mission_start_time is not None: - pickup.mission_start_time = None # Check if robot docked under the correct cart cart_check = self.is_correct_cart(pickup.cart_id) if cart_check: @@ -244,28 +224,31 @@ def check_pickup(self, pickup: Pickup): error_str = f'Mission {self.pickup_mission} not supported, ignoring' self.node.get_logger().error(error_str) return - pickup.mission_queue_id = mission_queue_id - pickup.mission_start_time = self.node.get_clock().now().nanoseconds / 1e9 + pickup.mission = Mission(mission_queue_id, now) pickup.latching = True self.node.get_logger().info(f'Robot [{self.name}] found the correct cart, pickup mission requested with mission queue id {mission_queue_id}') pickup.state = PickupState.PICKUP_REQUESTED elif cart_check is None: # If cart is missing, cancel this task self.node.get_logger().info(f'Robot [{self.name}] was unable to dock under any carts, please check that cart is present. Cancelling task.') + self.cancel_task() pickup.state = PickupState.TASK_CANCELLED else: # If cart is wrong, cancel this task also but after we exit from the lot self.node.get_logger().info(f'Robot [{self.name}] found the wrong cart, exiting lot and cancelling task.') self.exit_lot() + self.cancel_task() pickup.state = PickupState.TASK_CANCELLED case PickupState.PICKUP_REQUESTED: - if self.is_latch_open(): - # Latch successfully opened, indicate pickup as success + # Pickup mission completed + if self.api.mission_completed(pickup.mission.queue_id): pickup.state = PickupState.PICKUP_SUCCESS + pickup.mission = None pickup.latching = False # Update robot footprint to accommodate the cart size self.update_footprint(self.cart_footprint_guid) + # TODO(XY) consider doing a check for whether the latching time exceeds a configurable timeout case PickupState.PICKUP_SUCCESS: # Correct ID, we can end the delivery now @@ -278,11 +261,12 @@ def check_pickup(self, pickup: Pickup): case PickupState.TASK_CANCELLED: self.node.get_logger().info(f'Robot [{self.name}] is in pickup cancelled state.') # If some MiR mission is in progress, we abort it unless it is latching - if pickup.mission_queue_id is not None and not self.api.mission_completed(pickup.mission_queue_id): + if pickup.mission and not self.api.mission_completed(pickup.mission.queue_id): if pickup.latching: self.node.get_logger().info(f'Robot [{self.name}] is performing latching, cancelling task after this action is complete.') return False - self.api.mission_queue_id_delete(pickup.mission_queue_id) + self.api.mission_queue_id_delete(pickup.mission.queue_id) + pickup.mission = None # Clear any errors self.api.clear_error() self.api.status_put(state_id=MiRStateCode.READY) @@ -292,13 +276,13 @@ def check_pickup(self, pickup: Pickup): return False - def check_dropoff(self, dropoff: Dropoff): + def update_dropoff(self, dropoff: Dropoff): # Check if action is underway or cancelled if dropoff.execution is not None and not dropoff.execution.okay(): self.node.get_logger().info(f'Dropoff action is killed/canceled') # If cart release is in progress, we let it finish first - if dropoff.mission_queue_id is not None and not self.api.mission_completed(dropoff.mission_queue_id): + if dropoff.mission and not self.api.mission_completed(dropoff.mission.queue_id): return False # Task is cancelled and cart is done dropping off/mission is not yet queued anyway, @@ -309,30 +293,40 @@ def check_dropoff(self, dropoff: Dropoff): return True # No mission queued yet - if dropoff.mission_queue_id is None: + if dropoff.mission is None: assert self.dropoff_mission is not None mission_queue_id = self.api.queue_mission_by_name(self.dropoff_mission) if not mission_queue_id: error_str = f'Mission {self.dropoff_mission} not supported, ignoring' self.node.get_logger().error(error_str) return True - self.node.get_logger().info(f'Mission {self.dropoff_mission} added to queue.') - dropoff.mission_queue_id = mission_queue_id - self.node.get_logger().info(f'Dropoff mission requested with mission queue id {mission_queue_id}') + self.node.get_logger().info(f'Mission {self.dropoff_mission} added to queue for robot [{self.name}].') + now = self.node.get_clock().now().nanoseconds / 1e9 + dropoff.mission = Mission(mission_queue_id, + now) + self.node.get_logger().info(f'[{self.name}] Dropoff mission requested with mission queue id {mission_queue_id}') # Mission queued, check completion else: - if self.api.mission_completed(dropoff.mission_queue_id): - self.node.get_logger().info(f'Dropoff mission completed!') + if self.api.mission_completed(dropoff.mission.queue_id): + self.node.get_logger().info(f'[{self.name}] Dropoff mission completed!') # Update robot footprint self.update_footprint(self.robot_footprint_guid) if dropoff.execution is not None: dropoff.execution.finished() return True else: - self.node.get_logger().info(f'Dropoff mission in progress...') + self.node.get_logger().info(f'[{self.name}] Dropoff mission in progress...') return + def cancel_task(self, label: str = None): + def _cancel_success(): + if self.pickup: + self.pickup.state = PickupState.TASK_CANCELLED + def _cancel_fail(): + pass + self.cancel_current_task(_cancel_success, _cancel_fail, label) + def is_latch_open(self): # Checks if the robot's latch is open and carrying a cart # Return True if latch is open, else False diff --git a/fleet_adapter_mir_actions/package.xml b/fleet_adapter_mir_actions/package.xml new file mode 100644 index 0000000..b1cbb03 --- /dev/null +++ b/fleet_adapter_mir_actions/package.xml @@ -0,0 +1,15 @@ + + + + fleet_adapter_mir_actions + 0.1.0 + RMF fleet adapter actions for MiR robots + Xiyu + Apache License 2.0 + + rmf_fleet_adapter_python + + + ament_python + + diff --git a/fleet_adapter_mir_actions/resource/fleet_adapter_mir_actions b/fleet_adapter_mir_actions/resource/fleet_adapter_mir_actions new file mode 100644 index 0000000..e69de29 diff --git a/fleet_adapter_mir_actions/setup.cfg b/fleet_adapter_mir_actions/setup.cfg new file mode 100644 index 0000000..c18a730 --- /dev/null +++ b/fleet_adapter_mir_actions/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/fleet_adapter_mir_actions +[install] +install_scripts=$base/lib/fleet_adapter_mir_actions diff --git a/fleet_adapter_mir_actions/setup.py b/fleet_adapter_mir_actions/setup.py new file mode 100644 index 0000000..e7e69bf --- /dev/null +++ b/fleet_adapter_mir_actions/setup.py @@ -0,0 +1,24 @@ +from setuptools import setup + +package_name = 'fleet_adapter_mir_actions' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Xiyu', + maintainer_email='xiyu@openrobotics.org', + description='RMF fleet adapter actions for MiR robots', + license='Apache License 2.0', + entry_points={ + 'console_scripts': [ + ], + }, +) From 6b256dff5797be6eb689501587035c27cbf2f569 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Mon, 26 Feb 2024 10:58:05 +0800 Subject: [PATCH 12/50] Use ActionFactory to create MirAction objects Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 62 ++++++++++--------- fleet_adapter_mir/package.xml | 1 + .../fleet_adapter_mir_actions/mir_action.py | 44 +++++++++++-- .../rmf_cart_delivery.py | 38 +++++++++--- fleet_adapter_mir_actions/package.xml | 1 + 5 files changed, 102 insertions(+), 44 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 652779e..9111271 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -15,9 +15,9 @@ from rclpy.duration import Duration from .mir_api import MirAPI, MirStatus, MiRStateCode from threading import Lock +import importlib -# Import plugins -from ...fleet_adapter_mir_actions.fleet_adapter_mir_actions.rmf_cart_delivery import CartDelivery +from fleet_adapter_mir_actions.fleet_adapter_mir_actions.mir_action import MirAction # Parallel processing solution derived from @@ -128,19 +128,8 @@ def __init__( self._make_callbacks(), ) - self.plugins = {} - # Available plugins: - if 'rmf_cart_delivery' in plugin_config: - self.plugins['rmf_cart_delivery'] = CartDelivery( - node=node, - name=name, - mir_api=self.api, - update_handle=self.update_handle, - actions=plugin_config['rmf_cart_delivery']['actions'], - missions_json=plugin_config['rmf_cart_delivery'].get('missions_json'), - action_config=plugin_config['rmf_cart_delivery'], - retrieve_mir_coordinates=self.retrieve_mir_coordinates) - # To be added on with other plugins + self.current_action: MirAction = None # Tracks the current ongoing action + self.plugin_config = plugin_config # Stores all the configured plugin action configs @property def activity(self): @@ -191,8 +180,11 @@ def request_update(self): self.update_rmf_finished(mission) # PerformAction related checks - for plugin in self.plugins.values(): - plugin.update_action() + if self.current_action: + if self.current_action.update_action(): + # This means that the action has ended, we can clear the current action object + self.node.get_logger().info(f'Robot [{self.name}] has completed its current action.') + self.current_action = None # Clear error on updates if status is not None and 'errors' in status.response and len(status.response['errors']) > 0: @@ -471,19 +463,33 @@ def request_localize(self, estimate, mission: MissionHandle): mission.set_mission_queue_id(mission_queue_id) def perform_action(self, category, description, execution): - for plugin in self.plugins.values(): - if category in plugin.actions: - plugin.perform_action(category, description, execution) + if self.current_action: + # Should not reach here, but we log an error anyway + self.node.get_logger().info(f'Robot is busy with another perform action! Ignoring new action [{category}]') + execution.finished() + return + + for plugin_name, config in self.plugin_config.items(): + actions = config['actions'] + if category in actions: + # Import relevant plugin + plugin = importlib.import_module(plugin_name, 'fleet_adapter_mir_actions') + # Create the relevant MirAction + action_obj = plugin.ActionFactory().make_action(self.node, + self.name, + self.api, + self.update_handle, + self.fleet_config, + config) + # Begin performing the plugin action + action_obj.perform_action(category, description, execution) + # Keep track of the current action + self.current_action = action_obj return - raise NotImplementedError - def retrieve_mir_coordinates(self, waypoint_name: str): - transform = self.fleet_config.transformations_to_robot_coordinates - transform_current_map = transform.get(self.current_map) - rmf_pose = self.fleet_config.graph.find_waypoint(waypoint_name).location - new_rmf_pose = np.array([rmf_pose[0], rmf_pose[1], 0.0]) - mir_pose = transform_current_map.apply(new_rmf_pose) - return mir_pose + # No relevant perform action found + self.node.get_logger().info(f'Perform action [{category}] was not configured for this fleet') + raise NotImplementedError def dist(self, A, B): assert(len(A) > 1) diff --git a/fleet_adapter_mir/package.xml b/fleet_adapter_mir/package.xml index 05774a9..f0866d4 100644 --- a/fleet_adapter_mir/package.xml +++ b/fleet_adapter_mir/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 rmf_fleet_adapter_python + fleet_adapter_mir_actions ament_python diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 62505bb..0ae6508 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -1,22 +1,30 @@ import json -from ...fleet_adapter_mir.fleet_adapter_mir.mir_api import MirAPI, MirStatus, MiRStateCode +from abc import ABC, abstractmethod +from typing import Callable +import rclpy +import rclpy.node as Node +import rmf_adapter.easy_full_control as rmf_easy +from fleet_adapter_mir.fleet_adapter_mir.mir_api import MirAPI -class MirAction: +class MirAction(ABC): def __init__( self, node, name, mir_api: MirAPI, update_handle, + fleet_config, action_config: dict | None, ): self.node = node self.name = name self.api = mir_api self.update_handle = update_handle + self.fleet_config = fleet_config self.action_config = action_config + self.actions = self.action_config.get('actions') missions_json = self.action_config.get('missions_json') if missions_json: @@ -39,16 +47,25 @@ def __init__( self.api.mission_actions[mission] = self.api.missions_mission_id_actions_get(mission_data['guid']) # This will be called whenever an action has begun - def perform_action(self, category, description, execution): + @abstractmethod + def perform_action(self, + category: str, + description: dict, + execution # rmf_fleet_adapter.ActionExecution + ): # To be populated in the plugins - pass + ... # This will be called on every update to check on the action's current state + @abstractmethod def update_action(self): # To be populated in the plugins - pass + ... - def cancel_current_task(self, cancel_success, cancel_fail, label): + def cancel_current_task(self, + cancel_success: Callable((), None), + cancel_fail: Callable((), None), + label: str = None): current_task_id = self.update_handle.more().current_task_id() self.node.get_logger().info(f'Cancel task requested for [{current_task_id}]') def _on_cancel(result: bool): @@ -59,3 +76,18 @@ def _on_cancel(result: bool): self.node.get_logger().info(f'Failed to cancel task [{current_task_id}]') cancel_fail() self.update_handle.more().cancel_task(current_task_id, [label], lambda result: _on_cancel(result)) + + +class MirActionFactory(ABC): + def __init__(self): + pass + + def make_action(self, + node: Node, + name: str, + mir_api: MirAPI, + update_handle, # rmf_fleet_adapter.RobotUpdateHandle + fleet_config: rmf_easy.FleetConfiguration, + action_config) -> MirAction: + # To be populated in the plugins + pass diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index 070f00e..e1be006 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -1,12 +1,13 @@ import math from icecream import ic import enum +import numpy as np from typing import Any from dataclasses import dataclass import requests from urllib.error import HTTPError -from .mir_action import MirAction -from ...fleet_adapter_mir.fleet_adapter_mir.mir_api import MirAPI, MirStatus, MiRStateCode +from .mir_action import MirAction, MirActionFactory +from fleet_adapter_mir.fleet_adapter_mir.mir_api import MirAPI, MirStatus, MiRStateCode class PickupState(enum.IntEnum): @@ -43,6 +44,17 @@ class Dropoff: mission: Mission +class ActionFactory(MirActionFactory): + def make_action(self, + node, + name, + mir_api, + update_handle, + fleet_config, + action_config) -> MirAction: + return CartDelivery(node, name, mir_api, update_handle, fleet_config, action_config) + + class CartDelivery(MirAction): def __init__( self, @@ -50,17 +62,15 @@ def __init__( name, mir_api, update_handle, - action_config, - retrieve_mir_coordinates # Function for plugin to retrieve information about how to convert between rmf and mir coordinates - ): - MirAction.__init__(self, node, name, mir_api, update_handle, action_config) + fleet_config, + action_config + ): + MirAction.__init__(self, node, name, mir_api, update_handle, fleet_config, action_config) - self.action: Pickup | Dropoff = None + self.pickup: Pickup = None + self.dropoff: Dropoff = None self.search_timeout = self.action_config.get('search_timeout', 60) # seconds - # Useful robot adapter callbacks - self.retrieve_mir_coordinates = retrieve_mir_coordinates - # Store the mission names to be used during the action self.dock_to_cart_mission = self.action_config['missions']['dock_to_cart'] self.pickup_mission = self.action_config['missions']['pickup'] @@ -379,6 +389,14 @@ def update_footprint(self, ft_guid: str): ft_guid) return self.api.queue_mission_by_name(self.footprint_mission, ft_params) + def retrieve_mir_coordinates(self, waypoint_name: str): + transform = self.fleet_config.transformations_to_robot_coordinates + transform_current_map = transform.get(self.current_map) + rmf_pose = self.fleet_config.graph.find_waypoint(waypoint_name).location + new_rmf_pose = np.array([rmf_pose[0], rmf_pose[1], 0.0]) + mir_pose = transform_current_map.apply(new_rmf_pose) + return mir_pose + def dist(self, A, B): assert(len(A) > 1) assert(len(B) > 1) diff --git a/fleet_adapter_mir_actions/package.xml b/fleet_adapter_mir_actions/package.xml index b1cbb03..340b62f 100644 --- a/fleet_adapter_mir_actions/package.xml +++ b/fleet_adapter_mir_actions/package.xml @@ -8,6 +8,7 @@ Apache License 2.0 rmf_fleet_adapter_python + fleet_adapter_mir ament_python From a68026a19c4bd3a8e6e4c679a11071491aeef2ca Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Mon, 4 Mar 2024 11:03:53 +0800 Subject: [PATCH 13/50] Remove fleet_adapter_mir_actions as dependency in fleet_adapter_mir Signed-off-by: Xiyu Oh --- fleet_adapter_mir/fleet_adapter_mir/mir_api.py | 4 ++-- fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py | 7 ++++--- fleet_adapter_mir/package.xml | 2 +- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py index 94fedb1..f2b53ae 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py @@ -27,8 +27,8 @@ class MiRStateCode(enum.IntEnum): class MiRPositionTypes(enum.IntEnum): ROBOT = 0 SHELF = 5 - CHARGING_STATION = 7 - CHARGING_STATION_ENTRY = 8 + CHARGING_STATION = 20 + CHARGING_STATION_ENTRY = 21 CART = 22 LIFT = 25 LIFT_ENTRY = 26 diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 9111271..46f69ce 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -17,7 +17,7 @@ from threading import Lock import importlib -from fleet_adapter_mir_actions.fleet_adapter_mir_actions.mir_action import MirAction +# from fleet_adapter_mir_actions.fleet_adapter_mir_actions.mir_action import MirAction # Parallel processing solution derived from @@ -127,8 +127,9 @@ def __init__( rmf_config, self._make_callbacks(), ) - - self.current_action: MirAction = None # Tracks the current ongoing action + + # TODO(XY): move MirAction into this package + self.current_action = None # Tracks the current ongoing action self.plugin_config = plugin_config # Stores all the configured plugin action configs @property diff --git a/fleet_adapter_mir/package.xml b/fleet_adapter_mir/package.xml index f0866d4..d7298a7 100644 --- a/fleet_adapter_mir/package.xml +++ b/fleet_adapter_mir/package.xml @@ -9,7 +9,7 @@ Apache License 2.0 rmf_fleet_adapter_python - fleet_adapter_mir_actions + ament_python From ee6fbddf3c0fe52fb7c3e0f024a7f5fb5084a590 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Mon, 4 Mar 2024 13:25:17 +0800 Subject: [PATCH 14/50] Move MirAction to fleet_adapter_mir pkg Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir}/mir_action.py | 6 +++--- fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py | 7 +++---- fleet_adapter_mir/package.xml | 1 - .../fleet_adapter_mir_actions/rmf_cart_delivery.py | 2 +- 4 files changed, 7 insertions(+), 9 deletions(-) rename {fleet_adapter_mir_actions/fleet_adapter_mir_actions => fleet_adapter_mir/fleet_adapter_mir}/mir_action.py (94%) diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir/fleet_adapter_mir/mir_action.py similarity index 94% rename from fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py rename to fleet_adapter_mir/fleet_adapter_mir/mir_action.py index 0ae6508..62d19a1 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_action.py @@ -5,7 +5,7 @@ import rclpy import rclpy.node as Node import rmf_adapter.easy_full_control as rmf_easy -from fleet_adapter_mir.fleet_adapter_mir.mir_api import MirAPI +from .mir_api import MirAPI class MirAction(ABC): @@ -63,8 +63,8 @@ def update_action(self): ... def cancel_current_task(self, - cancel_success: Callable((), None), - cancel_fail: Callable((), None), + cancel_success: Callable[[], None], + cancel_fail: Callable[[], None], label: str = None): current_task_id = self.update_handle.more().current_task_id() self.node.get_logger().info(f'Cancel task requested for [{current_task_id}]') diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 46f69ce..914b34b 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -17,7 +17,7 @@ from threading import Lock import importlib -# from fleet_adapter_mir_actions.fleet_adapter_mir_actions.mir_action import MirAction +from .mir_action import MirAction # Parallel processing solution derived from @@ -127,9 +127,8 @@ def __init__( rmf_config, self._make_callbacks(), ) - - # TODO(XY): move MirAction into this package - self.current_action = None # Tracks the current ongoing action + + self.current_action: MirAction = None # Tracks the current ongoing action self.plugin_config = plugin_config # Stores all the configured plugin action configs @property diff --git a/fleet_adapter_mir/package.xml b/fleet_adapter_mir/package.xml index d7298a7..05774a9 100644 --- a/fleet_adapter_mir/package.xml +++ b/fleet_adapter_mir/package.xml @@ -9,7 +9,6 @@ Apache License 2.0 rmf_fleet_adapter_python - ament_python diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index e1be006..9951e1a 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -6,7 +6,7 @@ from dataclasses import dataclass import requests from urllib.error import HTTPError -from .mir_action import MirAction, MirActionFactory +from fleet_adapter_mir.fleet_adapter_mir.mir_action import MirAction, MirActionFactory from fleet_adapter_mir.fleet_adapter_mir.mir_api import MirAPI, MirStatus, MiRStateCode From bdc0218a48f331e20c92eee7906c207fad9b7b97 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 28 Mar 2024 10:30:22 +0800 Subject: [PATCH 15/50] Working plugin module via importlib and modified dispatch_delivery task Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 11 ++-- fleet_adapter_mir/setup.py | 1 + .../fleet_adapter_mir_actions}/mir_action.py | 2 +- .../rmf_cart_delivery.py | 48 +--------------- ...ispatch_pickup.py => dispatch_delivery.py} | 55 ++++++------------- fleet_adapter_mir_tasks/setup.py | 2 +- 6 files changed, 29 insertions(+), 90 deletions(-) rename {fleet_adapter_mir/fleet_adapter_mir => fleet_adapter_mir_actions/fleet_adapter_mir_actions}/mir_action.py (98%) rename fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/{dispatch_pickup.py => dispatch_delivery.py} (78%) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 914b34b..1d0b96c 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -17,8 +17,6 @@ from threading import Lock import importlib -from .mir_action import MirAction - # Parallel processing solution derived from # https://stackoverflow.com/a/59385935 @@ -128,7 +126,7 @@ def __init__( self._make_callbacks(), ) - self.current_action: MirAction = None # Tracks the current ongoing action + self.current_action = None # Tracks the current ongoing action self.plugin_config = plugin_config # Stores all the configured plugin action configs @property @@ -326,8 +324,13 @@ def navigate(self, destination, execution): dock_json = json.loads(destination.dock) if dock_json.get('mission_name') == self.api.dock_and_charge_mission: self.mission = MissionHandle(execution, charger=destination.name) + # Clear the mission queue before requesting dock and charge + self.node.get_logger().info(f'Clearing mission queue for [{self.name}] before submitting a dock_and_charge mission.') + self.api.mission_queue_delete() else: self.mission = MissionHandle(execution) + mission_name = dock_json.get('mission_name') + self.node.get_logger().info(f'Requesting [{mission_name}] mission for [{self.name}].') self.request_dock(dock_json, self.mission) return @@ -473,7 +476,7 @@ def perform_action(self, category, description, execution): actions = config['actions'] if category in actions: # Import relevant plugin - plugin = importlib.import_module(plugin_name, 'fleet_adapter_mir_actions') + plugin = importlib.import_module(f'fleet_adapter_mir_actions.{plugin_name}') # Create the relevant MirAction action_obj = plugin.ActionFactory().make_action(self.node, self.name, diff --git a/fleet_adapter_mir/setup.py b/fleet_adapter_mir/setup.py index c8a5b73..8ccc66f 100644 --- a/fleet_adapter_mir/setup.py +++ b/fleet_adapter_mir/setup.py @@ -20,6 +20,7 @@ entry_points={ 'console_scripts': [ 'fleet_adapter_mir=fleet_adapter_mir.fleet_adapter_mir:main', + 'rmf_cart_delivery=fleet_adapter_mir.rmf_cart_delivery:CartDelivery', ], }, ) diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py similarity index 98% rename from fleet_adapter_mir/fleet_adapter_mir/mir_action.py rename to fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 62d19a1..0576a7a 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -5,7 +5,7 @@ import rclpy import rclpy.node as Node import rmf_adapter.easy_full_control as rmf_easy -from .mir_api import MirAPI +from fleet_adapter_mir.mir_api import MirAPI class MirAction(ABC): diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index 9951e1a..abf23d4 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -6,8 +6,8 @@ from dataclasses import dataclass import requests from urllib.error import HTTPError -from fleet_adapter_mir.fleet_adapter_mir.mir_action import MirAction, MirActionFactory -from fleet_adapter_mir.fleet_adapter_mir.mir_api import MirAPI, MirStatus, MiRStateCode +from fleet_adapter_mir_actions.mir_action import MirAction, MirActionFactory +from fleet_adapter_mir.mir_api import MirAPI, MirStatus, MiRStateCode class PickupState(enum.IntEnum): @@ -97,7 +97,7 @@ def perform_action(self, category, description, execution): self.node.get_logger().info(f'New pickup requested for robot [{self.name}]') self.pickup = Pickup( - state=PickupState.PICKUP_ALLOCATED, + state=PickupState.AT_PICKUP, pickup_lots=[description.get('pickup_lot')], # TODO(XY): Allow multiple pickups? cart_id=description.get('cart_id'), execution=execution, @@ -134,48 +134,6 @@ def update_pickup(self, pickup: Pickup): now = self.node.get_clock().now().nanoseconds / 1e9 self.node.get_logger().debug(f'PickupState: {pickup.state.name}') match pickup.state: - case PickupState.PICKUP_ALLOCATED: - # Move to the first pickup place on the list - pickup_lot = pickup.pickup_lots[0] - self.node.get_logger().info(f'Requested to pickup cart at waypoint {pickup.state.name}') - if self.api.known_positions.get(pickup_lot) is None: - self.node.get_logger().info(f'Pickup Lot does not exist in MiR map, please resubmit.') - self.cancel_task() - pickup.state = PickupState.TASK_CANCELLED - return - - # Find the MiR coordinates of this pickup place - mir_pose = self.retrieve_mir_coordinates(pickup_lot) - pickup.mission = Mission(self.api.navigate(mir_pose), now) - pickup.state = PickupState.MOVE_REQUESTED - - case PickupState.MOVE_REQUESTED: - # Make sure that there is an rmf_move mission issued - if not pickup.mission: - self.node.get_logger().info(f'Robot [{self.name}] is indicated to be at the MOVE_REQUESTED state but ' - f'no mission queue ID stored for this pickup mission! Returning to PICKUP_ALLOCATED state.') - pickup.state = PickupState.PICKUP_ALLOCATED - return - - # Robot has reached the pickup lot - pickup_lot = pickup.pickup_lots[0] - if self.api.mission_completed(pickup.mission.queue_id): - self.node.get_logger().info(f'Robot [{self.name}] has reached the pickup waypoint {pickup_lot}') - pickup.mission = None - pickup.state = PickupState.AT_PICKUP - return - - # If the robot is relatively close to the pickup lot, we also consider it to be at pickup - # and allow the dock_to_cart mission to position the robot in front of the cart - pickup_pose = self.retrieve_mir_coordinates(pickup_lot) - current_pose = self.api.status_get().state.position - if self.dist(pickup_pose, current_pose) < self.action_config['pickup_dist_threshold']: - # Delete the ongoing mission - self.api.mission_queue_id_delete(pickup.mission.queue_id) - self.node.get_logger().info(f'Robot [{self.name}] is sufficiently near to the pickup waypoint {pickup_lot} for docking') - pickup.mission = None - pickup.state = PickupState.AT_PICKUP - case PickupState.AT_PICKUP: # Send rmf_dock_to_cart mission assert self.dock_to_cart_mission is not None diff --git a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py similarity index 78% rename from fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py rename to fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py index bd310dc..3fd951c 100644 --- a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_pickup.py +++ b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py @@ -104,61 +104,38 @@ def __init__(self, argv=sys.argv): # Define task request description with phases description = {} # task_description_Compose.json - description["category"] = "delivery_pickup" + description["category"] = "rmf_cart_delivery" description["phases"] = [] - # Set up cancellation behavior - on_cancel = [] - emergency_lots = [] - if self.args.emergency_lots: - for lot in self.args.emergency_lots: - emergency_lots.append({"waypoint": lot}) - cancellation_desc = { - "one_of": emergency_lots, - "constraints": [{ - "category": "prefer_same_map", - "description": "some description" - }] - } - cancellation_activities = [] - cancellation_activities.append({"category": "go_to_place", - "description": cancellation_desc}) - cancellation_activities.append({"category": "perform_action", - "description": { - "unix_millis_action_duration_estimate": 60000, - "category": "delivery_dropoff", - "description": {} - }}) - on_cancel.append( - {"category": "sequence", - "description": cancellation_activities}) - # Pickup activity - pickup_activity = [] - pickup_activity.append({"category": "go_to_place", - "description": self.args.pickup_lot}) - pickup_activity.append({"category": "perform_action", + go_to_pickup_activity = [{ + "category": "go_to_place", + "description": self.args.go_to + }] + description["phases"].append( + {"activity": {"category": "sequence", + "description": {"activities": go_to_pickup_activity}}}) + pickup_action_activity = [{"category": "perform_action", "description": { "unix_millis_action_duration_estimate": 60000, - "category": self.args.pickup_type, + "category": 'delivery_pickup', "description": { "cart_id": self.args.cart_id, "pickup_lot": self.args.pickup_lot - }}}) + }}}] description["phases"].append( {"activity": {"category": "sequence", - "description": {"activities": pickup_activity}}}) + "description": {"activities": pickup_action_activity}}}) # GoToPlace activity - go_to_place_activity = [{ + go_to_dropoff_activity = [{ "category": "go_to_place", "description": self.args.dropoff_lot }] description["phases"].append( {"activity": {"category": "sequence", - "description": {"activities": go_to_place_activity}}, - "on_cancel": on_cancel}) + "description": {"activities": go_to_dropoff_activity}}}) # Dropoff activity - dropoff_activity = [{ + dropoff_action_activity = [{ "category": "perform_action", "description": { "unix_millis_action_duration_estimate": 60000, @@ -168,7 +145,7 @@ def __init__(self, argv=sys.argv): }] description["phases"].append( {"activity": {"category": "sequence", - "description": {"activities": dropoff_activity}}}) + "description": {"activities": dropoff_action_activity}}}) # Consolidate request["description"] = description payload["request"] = request diff --git a/fleet_adapter_mir_tasks/setup.py b/fleet_adapter_mir_tasks/setup.py index 302fba9..2502dbf 100644 --- a/fleet_adapter_mir_tasks/setup.py +++ b/fleet_adapter_mir_tasks/setup.py @@ -19,7 +19,7 @@ license='Apache License 2.0', entry_points={ 'console_scripts': [ - 'dispatch_pickup = fleet_adapter_mir_tasks.dispatch_pickup:main' + 'dispatch_delivery = fleet_adapter_mir_tasks.dispatch_delivery:main' ], }, ) From bc241bb06e808a5c1d9e87005bda4950d6f081b3 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 28 Mar 2024 11:02:37 +0800 Subject: [PATCH 16/50] Rename actions arg parsed to fleet_adapter_mir to rmf_missions Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 6 +++--- fleet_adapter_mir/README.md | 9 +++------ .../fleet_adapter_mir/fleet_adapter_mir.py | 13 +++++++------ .../fleet_adapter_mir_tasks/dispatch_delivery.py | 2 -- 4 files changed, 13 insertions(+), 17 deletions(-) diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index 1de4059..3460bbf 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -27,9 +27,9 @@ conversions: [1.0, 1.0], [1.0, 0.0]] - # RMF to MIR map names maps: - L1: "Level 1" # Map name stored on MiR + # "RMF map name": "Map name stored on MiR" + L1: "Level 1" L2: "Level 2" missions: @@ -80,7 +80,7 @@ rmf_fleet: delivery: True clean: False finishing_request: "charge" # [park, charge, nothing] - actions: ["delivery_pickup", "delivery_dropoff"] + actions: ["rmf_cart_delivery", "delivery_pickup", "delivery_dropoff"] robots: mir_1: charger: "Charger_1" diff --git a/fleet_adapter_mir/README.md b/fleet_adapter_mir/README.md index a70dc16..6ae44e6 100644 --- a/fleet_adapter_mir/README.md +++ b/fleet_adapter_mir/README.md @@ -59,7 +59,7 @@ An example configuration file, `mir_config.yaml` has been provided. It has been **Chargers** -To use the `rmf_dock_and_charge` mission for charging, you may provide a description similar to the following when adding a `dock_name` to the charging point in your building map yaml/navigation graph: +To use the `rmf_dock_and_charge` mission for charging, use the traffic-editor to set the `dock_name` property of your charging point to a json description like the following: ```json {"description": {"end_waypoint": "charger_name"}, "mission_name": "rmf_dock_and_charge"} ``` @@ -68,11 +68,7 @@ Where you replace `end_waypoint` with the name of your MiR charger. **MiR positions** -For more accurate robot maneuver, you may wish to send the MiR to a Robot Position instead of using coordinates. For such waypoints, you can provide a `dock_name` with the `rmf_move_to_position` mission specified: -```json -{"description": {"end_waypoint": "waypoint_name"}, "mission_name": "rmf_move_to_position"} -``` - +Upon launch, the MiR fleet adapter recognizes MiR positions with identical names to RMF waypoints to be the same location. Hence, when a navigation command is submitted for the robot to a specific waypoint, if this waypoint name also exists as a robot position on the MiR, the fleet adapter would send it directly to the MiR position even if the coordinates are different. ### Plugins @@ -92,6 +88,7 @@ Some relevant MiR missions (docking, exit, update footprint) will be automatical - `rmf_dock_to_cart`: Docks robot under the cart - `rmf_exit_lot`: Calls the robot to exit from under the cart - `rmf_update_footprint`: Updates the robot footprint + They are defined and stored in the `rmf_cart_missions.json` file and do not require any further configuration. However, since there are various types of latching methods available for different MiR models, users will need to set up their custom pickup and dropoff missions on the MiR: diff --git a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py index ebf082f..d4cd279 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py @@ -184,9 +184,10 @@ def main(argv=sys.argv): parser.add_argument("-c", "--config_file", type=str, required=True, help="Input config.yaml file to process") parser.add_argument("-n", "--nav_graph", type=str, required=True, - help="Path to the nav_graph for this fleet adapter") - parser.add_argument("-a", "--actions", type=str, required=False, default='', - help="Path to the RMF mission actions to be created") + help="Path to the nav_graph for this fleet adapter") + parser.add_argument("-m", "--rmf_missions", type=str, + required=False, default='', + help="Path to the RMF missions to be created on robot") parser.add_argument("-m", "--mock", action='store_true', help="Init a mock adapter instead " "(does not require a schedule node, " @@ -199,7 +200,7 @@ def main(argv=sys.argv): config_path = args.config_file nav_graph_path = args.nav_graph - actions_path = args.actions + missions_path = args.rmf_missions fleet_config = rmf_easy.FleetConfiguration.from_config_files( config_path, nav_graph_path @@ -209,10 +210,10 @@ def main(argv=sys.argv): with open(config_path, 'r') as f: config_yaml = yaml.safe_load(f) - if actions_path == '': + if missions_path == '': rmf_missions = None else: - with open(actions_path, 'r') as g: + with open(missions_path, 'r') as g: rmf_missions = json.load(g) dry_run = args.dry_run # For testing diff --git a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py index 3fd951c..b21c35c 100644 --- a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py +++ b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py @@ -51,8 +51,6 @@ def __init__(self, argv=sys.argv): type=str, help='Name of pickup lot') parser.add_argument('-d', '--dropoff_lot', required=True, type=str, help='Name of dropoff lot') - parser.add_argument('-e', '--emergency_lots', required=False, default='', - type=str, nargs='+', help='Emergency waypoints') parser.add_argument('-st', '--start_time', help='Start time from now in secs, default: 0', type=int, default=0) From 3d855a369908d12fccec8cf0783fe00946d06039 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 28 Mar 2024 11:09:37 +0800 Subject: [PATCH 17/50] Add module key to config Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 4 +++- fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index 3460bbf..426f01a 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -96,7 +96,9 @@ rmf_fleet: plugins: rmf_cart_delivery: - actions: ["delivery_pickup", "delivery_dropoff"] + module: "fleet_adapter_mir_actions.rmf_cart_delivery" + class: "CartDelivery" + actions: ["rmf_cart_delivery", "delivery_pickup", "delivery_dropoff"] missions: dock_to_cart: "rmf_dock_to_cart" # Name of MiR mission for robot to dock under a cart - to be created by fleet adapter upon launch pickup: "rmf_pickup_cart" # Name of MiR mission for robot to latch onto a cart - to be created and filled in by user diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 1d0b96c..611f894 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -476,7 +476,8 @@ def perform_action(self, category, description, execution): actions = config['actions'] if category in actions: # Import relevant plugin - plugin = importlib.import_module(f'fleet_adapter_mir_actions.{plugin_name}') + module = config['module'] + plugin = importlib.import_module(module) # Create the relevant MirAction action_obj = plugin.ActionFactory().make_action(self.node, self.name, From 598a2ee4d6d050b9d78bf8aa9b8351943244fa07 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 16 Apr 2024 20:38:44 +0800 Subject: [PATCH 18/50] Remove footprint update from cart delivery plugin and cancel task if pickup pos is not found Signed-off-by: Xiyu Oh --- .../rmf_cart_delivery.py | 27 ++++++++++--------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index abf23d4..fafa836 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -76,13 +76,10 @@ def __init__( self.pickup_mission = self.action_config['missions']['pickup'] self.dropoff_mission = self.action_config['missions']['dropoff'] self.exit_mission = self.action_config['missions']['exit_lot'] - self.footprint_mission = self.action_config['missions']['update_footprint'] + self.footprint_mission = self.action_config['missions'].get('update_footprint') # Optional - # Initialize footprints and marker types - self.robot_footprint_guid = self.api.footprints_guid_get(self.action_config['footprints']['robot']) - self.cart_footprint_guid = self.api.footprints_guid_get(self.action_config['footprints']['cart']) + # Initialize cart marker type self.cart_marker_type_guid = self.api.docking_offsets_guid_get(self.action_config['marker_types']['cart']) - self.update_footprint(self.robot_footprint_guid) def perform_action(self, category, description, execution): # Check that the perform action category matches @@ -138,11 +135,16 @@ def update_pickup(self, pickup: Pickup): # Send rmf_dock_to_cart mission assert self.dock_to_cart_mission is not None current_wp_name = pickup.pickup_lots[0] - cart_marker_guid = self.api.known_positions[current_wp_name]['guid'] + mir_pos = self.api.known_positions.get(current_wp_name) + if not mir_pos: + self.node.get_logger().info(f'No shelf position [{mir_pos}] found on robot [{self.name}], cancelling task') + self.cancel_task() + pickup.state = PickupState.TASK_CANCELLED + return + cart_marker_guid = mir_pos['guid'] cart_marker_param = self.api.get_mission_params_with_value(self.dock_to_cart_mission, 'docking', 'cart_marker', cart_marker_guid) marker_type_param = self.api.get_mission_params_with_value(self.dock_to_cart_mission, 'docking', 'cart_marker_type', self.cart_marker_type_guid) mission_params = cart_marker_param + marker_type_param - self.update_footprint(self.robot_footprint_guid) mission_queue_id = self.api.queue_mission_by_name(self.dock_to_cart_mission, mission_params) if not mission_queue_id: error_str = f'Mission {self.dock_to_cart_mission} not supported, ignoring' @@ -214,9 +216,6 @@ def update_pickup(self, pickup: Pickup): pickup.state = PickupState.PICKUP_SUCCESS pickup.mission = None pickup.latching = False - # Update robot footprint to accommodate the cart size - self.update_footprint(self.cart_footprint_guid) - # TODO(XY) consider doing a check for whether the latching time exceeds a configurable timeout case PickupState.PICKUP_SUCCESS: # Correct ID, we can end the delivery now @@ -279,7 +278,6 @@ def update_dropoff(self, dropoff: Dropoff): if self.api.mission_completed(dropoff.mission.queue_id): self.node.get_logger().info(f'[{self.name}] Dropoff mission completed!') # Update robot footprint - self.update_footprint(self.robot_footprint_guid) if dropoff.execution is not None: dropoff.execution.finished() return True @@ -317,13 +315,13 @@ def is_correct_cart(self, cart_id: str): # ------------------------ # IMPLEMENT YOUR CODE HERE # ------------------------ - return None + return True + # return None def exit_lot(self): if not self.api.connected: return None # Set footprint to robot footprint before exiting lot - self.update_footprint(self.robot_footprint_guid) return self.api.queue_mission_by_name(self.exit_mission) def release_cart(self): @@ -340,7 +338,10 @@ def release_cart(self): self.node.get_logger().info(f'Cart detected above robot [{self.name}] during a release call, exiting lot...') self.exit_lot() + # Optional function for updating robot footprint by providing footprint guid def update_footprint(self, ft_guid: str): + if not self.footprint_mission: + return ft_params = self.api.get_mission_params_with_value(self.footprint_mission, 'set_footprint', 'footprint', From 51585960373b4f49149bde01241291063f7b55eb Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 18 Apr 2024 10:37:35 +0800 Subject: [PATCH 19/50] Configurable transformation params Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/fleet_adapter_mir.py | 37 +++++++++++++------ .../fleet_adapter_mir/robot_adapter_mir.py | 3 +- 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py index d4cd279..5713135 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py @@ -55,19 +55,32 @@ def sanitise_dict(dictionary, inplace=False, recursive=False): def compute_transforms(level, coords, node=None): """Get transforms between RMF and MIR coordinates.""" - rmf_coords = coords['rmf'] - mir_coords = coords['mir'] - tf = nudged.estimate(rmf_coords, mir_coords) - if node: - mse = nudged.estimate_error(tf, rmf_coords, mir_coords) - node.get_logger().info( - f"Transformation error estimate for {level}: {mse}" - ) + rotation = None + scale = None + translation = None + if 'rmf' in coords: + rmf_coords = coords['rmf'] + mir_coords = coords['mir'] + tf = nudged.estimate(rmf_coords, mir_coords) + if node: + mse = nudged.estimate_error(tf, rmf_coords, mir_coords) + node.get_logger().info( + f"Transformation error estimate for {level}: {mse}" + ) + rotation = tf.get_rotation() + scale = tf.get_scale() + translation = tf.get_translation() + elif 'rotation' in coords: + rotation = coords['rotation'] + scale = coords['scale'] + translation = coords['translation'] + else: + return None return Transformation( - tf.get_rotation(), - tf.get_scale(), - tf.get_translation() + rotation, + scale, + translation ) @@ -185,7 +198,7 @@ def main(argv=sys.argv): help="Input config.yaml file to process") parser.add_argument("-n", "--nav_graph", type=str, required=True, help="Path to the nav_graph for this fleet adapter") - parser.add_argument("-m", "--rmf_missions", type=str, + parser.add_argument("-r", "--rmf_missions", type=str, required=False, default='', help="Path to the RMF missions to be created on robot") parser.add_argument("-m", "--mock", action='store_true', diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 611f894..a9c62f3 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -438,9 +438,10 @@ def localize(self, estimate, execution): @parallel def request_localize(self, estimate, mission: MissionHandle): + retry_count = 10 count = 0 mission_queue_id = None - while count < self.retries and not mission_queue_id: + while count < retry_count and not mission_queue_id: count += 1 try: mission_queue_id = self.api.localize( From 6063200566e90ca3afda5cd637bf99e25d7898b9 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 10 May 2024 15:59:47 +0800 Subject: [PATCH 20/50] Add in retry mechanism for navigate cmds Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 34 ++++++++++++++++--- 1 file changed, 29 insertions(+), 5 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index a9c62f3..e1c67d4 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -376,12 +376,36 @@ def request_navigate( mission_handle: MissionHandle ): mission_queue_id = None - if destination.name: - self.node.get_logger().info(f'[{self.name}] is going to MiR position {destination.name}') - mission_queue_id = self.api.go_to_known_position(destination.name) + retry_count = 10 + count = 0 + while count < retry_count and not mission_queue_id: + try: + if destination.name: + self.node.get_logger().info(f'[{self.name}] is going to MiR position {destination.name}') + mission_queue_id = self.api.go_to_known_position(destination.name) + if mission_queue_id is None: + self.node.get_logger().info(f'[{self.name}] is going to MiR coordinates [{destination.position}]') + mission_queue_id = self.api.navigate(destination.position) + else: + self.node.get_logger().info( + f'[{self.name}] Move mission cannot be queued' + f'(no mission queue id provided), retrying...' + ) + except Exception as err: + self.node.get_logger().info( + f'[{self.name}] Failed to request robot to move to ' + f'destination: {err}. Retrying... ({count})' + ) + count += 1 + time.sleep(1) + if mission_queue_id is None: - self.node.get_logger().info(f'[{self.name}] is going to MiR coordinates [{destination.position}]') - mission_queue_id = self.api.navigate(destination.position) + self.node.get_logger().info( + f'[{self.name}] Failed to request robot to move to destination.' + f' Maximum request navigate retries exceeded!') + mission_handle.execution.finished() + return + mission_handle.set_mission_queue_id(mission_queue_id) @parallel From 51d7fba94290bebd1b6c1e8891ae4ffa0d40a840 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 15 May 2024 14:14:20 +0800 Subject: [PATCH 21/50] Add retry mechanism for dock and go to known position Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 160 ++++++++++++++++-- 1 file changed, 144 insertions(+), 16 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index e1c67d4..584f269 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -375,22 +375,39 @@ def request_navigate( destination, mission_handle: MissionHandle ): + # Used for exiting while loop early in the event that for whatever + # reason the robot starts performing a different mission, while the + # original navigation mission is ongoing + navigation_mission_identifier = mission_handle.execution.identifier mission_queue_id = None - retry_count = 10 count = 0 + retry_count = 10 while count < retry_count and not mission_queue_id: + if (navigation_mission_identifier != + mission_handle.execution.identifier): + self.node.get_logger().info( + f'[{self.name}] MissionHandle has changed to ' + f'{mission_handle.execution.identifier}, interrupting ' + f'navigation mission {navigation_mission_identifier}. ' + f'Stopping original navigation mission loop.') + break + try: if destination.name: - self.node.get_logger().info(f'[{self.name}] is going to MiR position {destination.name}') - mission_queue_id = self.api.go_to_known_position(destination.name) + self.node.get_logger().info( + f'[{self.name}] is going to MiR position ' + f'{destination.name}') + mission_queue_id = self.api.go_to_known_position( + destination.name) if mission_queue_id is None: - self.node.get_logger().info(f'[{self.name}] is going to MiR coordinates [{destination.position}]') + self.node.get_logger().info( + f'[{self.name}] is going to MiR coordinates ' + f'[{destination.position}]') mission_queue_id = self.api.navigate(destination.position) else: self.node.get_logger().info( - f'[{self.name}] Move mission cannot be queued' - f'(no mission queue id provided), retrying...' - ) + f'[{self.name}] Move mission cannot be queued ' + f'(no mission queue id provided), retrying...') except Exception as err: self.node.get_logger().info( f'[{self.name}] Failed to request robot to move to ' @@ -399,6 +416,16 @@ def request_navigate( count += 1 time.sleep(1) + if (mission_queue_id is None and + navigation_mission_identifier != + mission_handle.execution.identifier): + self.node.get_logger().info( + f'[{self.name}] Failed to request robot to move to destination.' + f' Navigation step {navigation_mission_identifier} interrupted ' + f'by new mission {mission_handle.execution.identifier}.' + ) + return + if mission_queue_id is None: self.node.get_logger().info( f'[{self.name}] Failed to request robot to move to destination.' @@ -414,7 +441,56 @@ def request_go_to_known_position( position_name: str, mission_handle: MissionHandle ): - mission_queue_id = self.api.go_to_known_position(position_name) + # Used for exiting while loop early in the event that for whatever + # reason the robot starts performing a different mission, while the + # original navigation mission is ongoing + navigation_mission_identifier = mission_handle.execution.identifier + mission_queue_id = None + count = 0 + retry_count = 10 + while count < retry_count and not mission_queue_id: + if (navigation_mission_identifier != + mission_handle.execution.identifier): + self.node.get_logger().info( + f'[{self.name}] MissionHandle has changed to ' + f'{mission_handle.execution.identifier}, interrupting ' + f'navigation mission {navigation_mission_identifier}.' + f' Stopping original navigation mission loop.') + break + + try: + mission_queue_id = self.api.go_to_known_position(position_name) + if mission_queue_id is None: + self.node.get_logger().info( + f'[{self.name}] Move to known positions mission cannot ' + f'be queued (no mission queue id provided), retrying...' + ) + except Exception as err: + self.node.get_logger().info( + f'[{self.name}] Failed to request robot to move to known' + f'position: {err}. Retrying... ({count})' + ) + count += 1 + time.sleep(1) + + if (mission_queue_id is None and + navigation_mission_identifier != + mission_handle.execution.identifier): + self.node.get_logger().info( + f'[{self.name}] Failed to request robot to move to known ' + f'position. Navigation step {navigation_mission_identifier} ' + f'interrupted by new mission ' + f'{mission_handle.execution.identifier}.' + ) + return + + if mission_queue_id is None: + self.node.get_logger().info( + f'[{self.name}] Failed to request robot to move to known ' + f'position. Maximum request navigate retries exceeded!') + mission_handle.execution.finished() + return + mission_handle.set_mission_queue_id(mission_queue_id) @@ -426,15 +502,67 @@ def request_dock( ): if not ('description' in docking_points): return - self.node.get_logger().info(f'Requested dock mission for [{self.name}]: {docking_points}') - end_waypoint = docking_points['description']['end_waypoint'] - if 'start_waypoint' in docking_points['description']: - start_waypoint = docking_points['description']['start_waypoint'] - else: - start_waypoint = None - mission_name = docking_points['mission_name'] + # Used for exiting while loop early in the event that for whatever + # reason the robot starts performing a different mission, while the + # original navigation mission is ongoing + docking_mission_identifier = mission_handle.execution.identifier + mission_queue_id = None + count = 0 + retry_count = 10 + while count < retry_count and not mission_queue_id: + if (docking_mission_identifier != + mission_handle.execution.identifier): + self.node.get_logger().info( + f'[{self.name}] MissionHandle has changed to ' + f'{mission_handle.execution.identifier}, interrupting ' + f'navigation mission {docking_mission_identifier}. ' + f'Stopping original docking mission loop.') + break + + try: + self.node.get_logger().info( + f'Requested dock mission for [{self.name}]: ' + f'{docking_points}') + end_waypoint = docking_points['description']['end_waypoint'] + if 'start_waypoint' in docking_points['description']: + start_waypoint = \ + docking_points['description']['start_waypoint'] + else: + start_waypoint = None + mission_name = docking_points['mission_name'] + + mission_queue_id = self.api.dock(mission_name, + start_waypoint, + end_waypoint) + if mission_queue_id is None: + self.node.get_logger().info( + f'[{self.name}] Dock mission cannot be queued ' + f'(no mission queue id provided), retrying...') + except Exception as err: + self.node.get_logger().info( + f'[{self.name}] Failed to request robot to dock to ' + f'destination: {err}. Retrying... ({count})' + ) + count += 1 + time.sleep(1) + + if (mission_queue_id is None and + docking_mission_identifier != + mission_handle.execution.identifier): + self.node.get_logger().info( + f'[{self.name}] Failed to request robot to dock to destination.' + f' Docking step {docking_mission_identifier} interrupted ' + f'by new mission {mission_handle.execution.identifier}.' + ) + return + + if mission_queue_id is None: + self.node.get_logger().info( + f'[{self.name}] Failed to request robot to dock to destination.' + f' Maximum request navigate retries exceeded!') + mission_handle.execution.finished() + return - mission_queue_id = self.api.dock(mission_name, start_waypoint, end_waypoint) mission_handle.set_mission_queue_id(mission_queue_id) def stop(self, activity): From 27baec2c991f79330f236b22bd3caf1ebd193338 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 21 May 2024 12:58:16 +0800 Subject: [PATCH 22/50] Fix wrong input argument for task cancellation Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir_actions/mir_action.py | 2 +- .../fleet_adapter_mir_actions/rmf_cart_delivery.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 0576a7a..a88694d 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -65,7 +65,7 @@ def update_action(self): def cancel_current_task(self, cancel_success: Callable[[], None], cancel_fail: Callable[[], None], - label: str = None): + label: str = ''): current_task_id = self.update_handle.more().current_task_id() self.node.get_logger().info(f'Cancel task requested for [{current_task_id}]') def _on_cancel(result: bool): diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index fafa836..d3976ad 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -285,7 +285,7 @@ def update_dropoff(self, dropoff: Dropoff): self.node.get_logger().info(f'[{self.name}] Dropoff mission in progress...') return - def cancel_task(self, label: str = None): + def cancel_task(self, label: str = ''): def _cancel_success(): if self.pickup: self.pickup.state = PickupState.TASK_CANCELLED From 01ad9ccf66f99553fa9b273d7a82f7ea9964bc32 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 26 Jun 2024 13:43:44 +0100 Subject: [PATCH 23/50] Separate cart detection related functions into another plugin Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 1 + fleet_adapter_mir/setup.py | 1 + .../rmf_cart_delivery.py | 32 ++---- .../rmf_cart_detection.py | 104 ++++++++++++++++++ 4 files changed, 118 insertions(+), 20 deletions(-) create mode 100644 fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index 426f01a..8eeb723 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -99,6 +99,7 @@ plugins: module: "fleet_adapter_mir_actions.rmf_cart_delivery" class: "CartDelivery" actions: ["rmf_cart_delivery", "delivery_pickup", "delivery_dropoff"] + cart_detection_module: "fleet_adapter_mir_actions.rmf_cart_detection" missions: dock_to_cart: "rmf_dock_to_cart" # Name of MiR mission for robot to dock under a cart - to be created by fleet adapter upon launch pickup: "rmf_pickup_cart" # Name of MiR mission for robot to latch onto a cart - to be created and filled in by user diff --git a/fleet_adapter_mir/setup.py b/fleet_adapter_mir/setup.py index 8ccc66f..1ce52ce 100644 --- a/fleet_adapter_mir/setup.py +++ b/fleet_adapter_mir/setup.py @@ -21,6 +21,7 @@ 'console_scripts': [ 'fleet_adapter_mir=fleet_adapter_mir.fleet_adapter_mir:main', 'rmf_cart_delivery=fleet_adapter_mir.rmf_cart_delivery:CartDelivery', + 'rmf_cart_detection=fleet_adapter_mir.rmf_cart_detection:CartDetection', ], }, ) diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index d3976ad..d1b5886 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -1,9 +1,9 @@ import math -from icecream import ic import enum import numpy as np from typing import Any from dataclasses import dataclass +import importlib import requests from urllib.error import HTTPError from fleet_adapter_mir_actions.mir_action import MirAction, MirActionFactory @@ -81,6 +81,14 @@ def __init__( # Initialize cart marker type self.cart_marker_type_guid = self.api.docking_offsets_guid_get(self.action_config['marker_types']['cart']) + # Import CartDetection module if provided + detection_module = self.action_config.get('cart_detection_module') + assert (detection_module is not None, + 'CartDetection module is required for CartDelivery plugin, ' + \ + 'but it is not found!') + detection_plugin = importlib.import_module(detection_module) + self.cart_detection = detection_plugin.CartDetection(mir_api, action_config) + def perform_action(self, category, description, execution): # Check that the perform action category matches match category: @@ -294,29 +302,13 @@ def _cancel_fail(): self.cancel_current_task(_cancel_success, _cancel_fail, label) def is_latch_open(self): - # Checks if the robot's latch is open and carrying a cart - # Return True if latch is open, else False - # ------------------------ - # IMPLEMENT YOUR CODE HERE - # ------------------------ - return False + return self.cart_detection.is_latch_open() def is_under_cart(self): - # Checks if the robot is docked under a cart - # Return True if robot is under any carts, else False - # ------------------------ - # IMPLEMENT YOUR CODE HERE - # ------------------------ - return False + return self.cart_detection.is_under_cart() def is_correct_cart(self, cart_id: str): - # Checks if the detected cart identifier matches the target cart_id - # Return True if cart is correct, False if cart is wrong, None if no cart detected - # ------------------------ - # IMPLEMENT YOUR CODE HERE - # ------------------------ - return True - # return None + return self.cart_detection.is_correct_cart(cart_id) def exit_lot(self): if not self.api.connected: diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py new file mode 100644 index 0000000..c209270 --- /dev/null +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py @@ -0,0 +1,104 @@ +import requests +from urllib.error import HTTPError + +class CartDetection: + def __init__( + self, + mir_api, + action_config + ): + self.api = mir_api + self.action_config = action_config + + def is_latch_open(self): + ''' + Checks if the robot's latch is open and carrying a cart + Return True if latch is open, else False + ''' + # ------------------------ + # IMPLEMENT YOUR CODE HERE + # ------------------------ + return False + + def is_under_cart(self): + ''' + Checks if the robot is docked under a cart + Return True if robot is under any carts, else False + ''' + # ------------------------ + # IMPLEMENT YOUR CODE HERE + # ------------------------ + return False + + def is_correct_cart(self, cart_id: str): + ''' + Checks if the detected cart identifier matches the target cart_id + Return True if cart is correct, False if cart is wrong, None if no + cart detected + ''' + # ------------------------ + # IMPLEMENT YOUR CODE HERE + # ------------------------ + return None + + # -------------------------------------------------------------------------- + # HELPFUL FUNCTIONS FOR INTERACTING WITH MIR REST API + # -------------------------------------------------------------------------- + + def register_get(self, register: int): + if not self.api.connected: + return None + try: + response = requests.get(self.api.prefix + f'registers/{register}', + headers=self.api.headers, + timeout=self.api.timeout) + if self.api.debug: + print(f"Response: {response.headers}") + # Response value is string, return integer of value + return int(response.json().get('value', 0)) + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + return None + except Exception as err: + print(f"Other error: {err}") + return None + + def io_module_guid_status_get(self, io_guid: str): + if not self.api.connected: + return None + if io_guid is None: + return None + try: + response = requests.get(self.api.prefix + + f'io_modules/{io_guid}/status', + headers=self.api.headers, + timeout=self.api.timeout) + if self.api.debug: + print(f"Response: {response.headers}") + if 'input_state' not in response.json(): + return None + return response.json()['input_state'] + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + return None + except Exception as err: + print(f"Other error: {err}") + return None + + def io_modules_get(self): + if not self.api.connected: + return None + try: + response = requests.get(self.api.prefix + f'io_modules', + headers=self.api.headers, + timeout=self.api.timeout) + if self.api.debug: + print(f"Response: {response.headers}") + # Response value is string, return integer of value + return response.json() + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + return None + except Exception as err: + print(f"Other error: {err}") + return None \ No newline at end of file From c48e0af96e0d8aa662581c8f3966768b88024c61 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 17 Jul 2024 16:49:30 +0800 Subject: [PATCH 24/50] Add example for module config and minor cleanup Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 3 +++ .../fleet_adapter_mir_actions/rmf_cart_delivery.py | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index 8eeb723..b0177c9 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -114,3 +114,6 @@ plugins: missions_json: "/home/some_user/mir_ws/src/fleet_adapter_mir/missions/rmf_cart_missions.json" # Filepath to RMF cart delivery missions search_timeout: 60 pickup_dist_threshold: 3.0 # metres + # Examples of additional fields for cart detection module + io_name: "MiR internal IOs" + latch_io: 2 diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index d1b5886..7188c87 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -84,7 +84,7 @@ def __init__( # Import CartDetection module if provided detection_module = self.action_config.get('cart_detection_module') assert (detection_module is not None, - 'CartDetection module is required for CartDelivery plugin, ' + \ + 'CartDetection module is required for CartDelivery plugin, ' + 'but it is not found!') detection_plugin = importlib.import_module(detection_module) self.cart_detection = detection_plugin.CartDetection(mir_api, action_config) From 9c25969613ed44fe875e2e1aa7597c7fdd35a81d Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 30 Jul 2024 18:01:38 +0800 Subject: [PATCH 25/50] Codestyle cleanup Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/fleet_adapter_mir.py | 25 +- .../fleet_adapter_mir/mir_api.py | 253 +++++++++++++----- .../fleet_adapter_mir/robot_adapter_mir.py | 218 +++++++++------ .../fleet_adapter_mir_actions/mir_action.py | 26 +- .../rmf_cart_delivery.py | 240 +++++++++++------ .../rmf_cart_detection.py | 25 +- .../dispatch_delivery.py | 28 +- 7 files changed, 542 insertions(+), 273 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py index 5713135..3c0cc7c 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py @@ -18,6 +18,7 @@ from rmf_adapter import Transformation from .robot_adapter_mir import RobotAdapterMiR + ############################################################################### # HELPER FUNCTIONS AND CLASSES ############################################################################### @@ -110,7 +111,7 @@ async def state_updates(self): for robot in self.robot_handles: robot_updaters.append(robot.update_loop(self.period)) await asyncio.gather(*robot_updaters) - + def update_loop(self): asyncio.set_event_loop(self.event_loop) self.event_loop.run_until_complete(self.state_updates()) @@ -131,14 +132,17 @@ def start(self): rclpy.shutdown() -def create_fleet(fleet_config, config_yaml, cmd_node, rmf_missions) -> FleetAdapterMiR: +def create_fleet( + fleet_config, config_yaml, cmd_node, rmf_missions) -> FleetAdapterMiR: """Create RMF Adapter and fleet handle""" - for level, coords in config_yaml['conversions']['reference_coordinates'].items(): + for level, coords in \ + config_yaml['conversions']['reference_coordinates'].items(): tf = compute_transforms(level, coords, cmd_node) fleet_config.add_robot_coordinates_transformation(level, tf) # RMF_CORE Fleet Adapter: Manages delivery or loop requests - adapter = rmf_adapter.Adapter.make(config_yaml['node_names']['rmf_fleet_adapter']) + adapter = rmf_adapter.Adapter.make( + config_yaml['node_names']['rmf_fleet_adapter']) assert adapter, ("Adapter could not be init! " "Ensure a ROS2 scheduler node is running") @@ -160,8 +164,10 @@ def create_fleet(fleet_config, config_yaml, cmd_node, rmf_missions) -> FleetAdap event_loop = asyncio.new_event_loop() robots_handles = [] - for robot_name, rmf_config in fleet_config.known_robot_configurations.items(): - mir_config = config_yaml['rmf_fleet']['robots'][robot_name]['mir_config'] + for robot_name, rmf_config in \ + fleet_config.known_robot_configurations.items(): + mir_config = \ + config_yaml['rmf_fleet']['robots'][robot_name]['mir_config'] robots_handles.append(RobotAdapterMiR( robot_name, rmf_config, @@ -176,7 +182,9 @@ def create_fleet(fleet_config, config_yaml, cmd_node, rmf_missions) -> FleetAdap debug, )) - return FleetAdapterMiR(cmd_node, adapter, fleet_handle, robots_handles, update_frequency, event_loop) + return FleetAdapterMiR( + cmd_node, adapter, fleet_handle, robots_handles, update_frequency, + event_loop) ############################################################################### @@ -246,7 +254,8 @@ def main(argv=sys.argv): print() # Create a node to use inside of the Python code for logging - cmd_node = rclpy.node.Node(config_yaml['node_names']['robot_command_handle']) + cmd_node = rclpy.node.Node( + config_yaml['node_names']['robot_command_handle']) # Create the fleet, including the robots that are in the config file fleet = create_fleet(fleet_config, config_yaml, cmd_node, rmf_missions) diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py index f2b53ae..6f34769 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py @@ -36,6 +36,7 @@ class MiRPositionTypes(enum.IntEnum): LocalizationParamPosition = 'position_estimate' + class MapConversions: def __init__(self, rmf_to_mir: dict): self.rmf_to_mir = rmf_to_mir @@ -43,7 +44,10 @@ def __init__(self, rmf_to_mir: dict): class MirStatus: - def __init__(self, response: dict, map_conversions: MapConversions, map_name: str): + def __init__(self, + response: dict, + map_conversions: MapConversions, + map_name: str): self.response = response p = response['position'] self.state = RobotState( @@ -54,9 +58,14 @@ def __init__(self, response: dict, map_conversions: MapConversions, map_name: st class MirAPI: - def __init__(self, prefix, headers, conversions, rmf_missions, timeout=10.0, debug=False): - #HTTP connection - self.prefix = prefix + def __init__(self, + prefix, + headers, + conversions, + rmf_missions, + timeout=10.0, + debug=False): + self.prefix = prefix self.debug = False self.headers = headers self.timeout = timeout @@ -74,7 +83,10 @@ def __init__(self, prefix, headers, conversions, rmf_missions, timeout=10.0, deb def attempt_connection(self): try: - requests.get(self.prefix + 'wifi/connections', headers=self.headers, timeout=self.timeout) + requests.get( + self.prefix + 'wifi/connections', + headers=self.headers, + timeout=self.timeout) except HTTPError as http_err: print(f"HTTP error: {http_err}") return @@ -104,7 +116,8 @@ def attempt_connection(self): f'{dock_and_charge_key} mission must be specified in fleet config ' f'file under conversions -> missions' ) - self.dock_and_charge_mission: str = self.mission_keys[dock_and_charge_key] + self.dock_and_charge_mission: str = \ + self.mission_keys[dock_and_charge_key] assert self.dock_and_charge_mission in self.known_missions, ( f'RMF dock and charge mission [{self.dock_and_charge_mission}] ' f'has not yet been defined as a mission in the MiR robot ' @@ -137,8 +150,8 @@ def make_optional_mission(key): self.localize_params = action['parameters'] break assert self.localize_params is not None, ( - f'No switch_map action in the mission {self.localize_mission}:\n' - f'{localize_actions}' + f'No switch_map action in the mission {self.localize_mission}:' + f'\n{localize_actions}' ) found_position_estimate_param = False for param in self.localize_params: @@ -151,17 +164,18 @@ def make_optional_mission(key): # Retrieve mission parameters for mission, mission_data in self.known_missions.items(): - self.mission_actions[mission] = self.missions_mission_id_actions_get( - mission_data['guid']) + self.mission_actions[mission] = \ + self.missions_mission_id_actions_get(mission_data['guid']) self.created_by_id = self.me_get()['guid'] def update_known_positions(self): self.known_positions = {} for pos in self.positions_get(): - if pos['name'] in self.known_positions and \ - pos['type_id'] == self.known_positions[pos['name']]['type_id'] and \ - ('rmf_localize' in pos['name']): + if (pos['name'] in self.known_positions and + (pos['type_id'] == + self.known_positions[pos['name']]['type_id']) and + ('rmf_localize' in pos['name'])): # Delete any duplicate positions self.positions_guid_delete(pos['guid']) elif pos['type_id'] == MiRPositionTypes.ROBOT or \ @@ -194,12 +208,18 @@ def create_missions(self, rmf_missions): for mission_name, mission_json in rmf_missions.items(): if mission_name not in self.known_missions: # Create the relevant mission on MiR - mission = self.missions_post(mission_name, rmf_mission_group_id) + mission = self.missions_post( + mission_name, rmf_mission_group_id) self.known_missions[mission['name']] = mission # Fill in mission actions - self.create_rmf_mission_actions(mission_name, mission['guid'], mission_json) - - def create_rmf_mission_actions(self, mission_name: str, mission_id: str, mission_json: list[dict]): + self.create_rmf_mission_actions( + mission_name, mission['guid'], mission_json) + + def create_rmf_mission_actions( + self, + mission_name: str, + mission_id: str, + mission_json: list[dict]): for action in mission_json: # Find schema for action type action_type = action.get('action_type') @@ -218,17 +238,21 @@ def create_rmf_mission_actions(self, mission_name: str, mission_id: str, mission param_body['input_name'] = param['input_name'] param_body['value'] = param['value'] - # If the input type is a position, we would need a valid GUID for the value field - # Let's find a placeholder position GUID from the list of known positions - if param_body['id'] == 'position' or param_body['id'] == 'entry_position': + # If the input type is a position, we would need a valid GUID + # for the value field. Let's find a placeholder position GUID + # from the list of known positions + if (param_body['id'] == 'position' or + param_body['id'] == 'entry_position'): default_pos = self.positions_get()[0]['guid'] param_body['value'] = default_pos - # Similarly, if the input type is a marker type, we'll need to find a placeholder - # marker type GUID from the list of known marker types + # Similarly, if the input type is a marker type, we'll need to + # find a placeholder marker type GUID from the list of known + # marker types if param_body['id'] == 'marker_type': default_marker = self.docking_offsets_get()[0]['guid'] param_body['value'] = default_marker - # If the input type is a footprint, we'll also use a placeholder footprint GUID + # If the input type is a footprint, we'll also use a + # placeholder footprint GUID if param_body['id'] == 'footprint': default_footprint = self.footprints_get()[0]['guid'] param_body['value'] = default_footprint @@ -246,7 +270,8 @@ def create_rmf_mission_actions(self, mission_name: str, mission_id: str, mission param_body['input_name'] = param.get('input_name') default_value = None - if 'constraints' in param and 'default' in param['constraints']: + if ('constraints' in param and + 'default' in param['constraints']): default_value = param['constraints']['default'] param_body['value'] = default_value action_body_param.append(param_body) @@ -270,7 +295,7 @@ def navigate(self, position): return self.queue_mission_by_name(self.move_mission, mission_params) def go_to_known_position(self, position_name): - if not position_name in self.known_positions: + if position_name not in self.known_positions: return None return self.dock(self.go_to, None, position_name) @@ -285,8 +310,10 @@ def dock(self, mission_name, start_waypoint, end_waypoint): assert end_wp is not None end_wp_guid = end_wp.get('guid') assert end_wp_guid is not None - end_param = self.get_mission_params_with_value(mission_name, 'move', 'end_waypoint', end_wp_guid) - dock_param = self.get_mission_params_with_value(mission_name, 'docking', 'end_waypoint', end_wp_guid) + end_param = self.get_mission_params_with_value( + mission_name, 'move', 'end_waypoint', end_wp_guid) + dock_param = self.get_mission_params_with_value( + mission_name, 'docking', 'end_waypoint', end_wp_guid) # Start waypoint is optional if start_waypoint is not None: @@ -297,14 +324,18 @@ def dock(self, mission_name, start_waypoint, end_waypoint): assert start_wp is not None start_wp_guid = start_wp.get('guid') assert start_wp_guid is not None - start_param = self.get_mission_params_with_value(mission_name, 'move', 'start_waypoint', start_wp_guid) + start_param = self.get_mission_params_with_value( + mission_name, 'move', 'start_waypoint', start_wp_guid) mission_params = start_param + end_param - # Check whether we should dock into this end waypoint or not (for charging) + # Check whether we should dock into this end waypoint (for charging) elif dock_param: charger_marker_type = self.marker_type_keys['charger'] - charger_marker_type_guid = self.docking_offsets_guid_get(charger_marker_type) - marker_param = self.get_mission_params_with_value(mission_name, 'docking', 'charger_marker_type', charger_marker_type_guid) + charger_marker_type_guid = self.docking_offsets_guid_get( + charger_marker_type) + marker_param = self.get_mission_params_with_value( + mission_name, 'docking', 'charger_marker_type', + charger_marker_type_guid) mission_params = end_param + dock_param + marker_param else: mission_params = end_param @@ -336,16 +367,15 @@ def localize(self, map, estimate, index): mission_params=mission_params ) - def get_position_guid(self, name, map_id, location): attempts = 0 max_attempts = 10 while True: - attempts +=1 + attempts += 1 if attempts >= max_attempts: print( - f'Too many attempts [{max_attempts}] to set a localization ' - 'position.' + f'Too many attempts [{max_attempts}] to set a ' + f'localization position.' ) return None @@ -373,12 +403,12 @@ def position_matches(pos): # positions and retry this loop. self.update_known_positions() elif not position_matches(position): - if self.positions_put(position['guid'], name, map_id, location): + if self.positions_put( + position['guid'], name, map_id, location): return position['guid'] else: return position['guid'] - def queue_mission_by_name(self, mission_name, mission_params=None): mir_mission = self.known_missions.get(mission_name) if mir_mission is None: @@ -448,7 +478,7 @@ def positions_put(self, guid, name, map_id, location): print(f'Response: {response}') self.known_positions[response['name']] = response return response['guid'] - except: + except Exception as err: pass def positions_delete(self): @@ -463,17 +493,18 @@ def positions_delete(self): timeout=self.timeout ).json() return response['guid'] - except: + except Exception as err: pass else: - new_known_positions[position] = copy.deepcopy(self.known_positions[position]) + new_known_positions[position] = \ + copy.deepcopy(self.known_positions[position]) self.known_positions = {} self.known_positions = new_known_positions def positions_guid_delete(self, guid): try: response = requests.delete( - self.prefix + f'positions/{guid}', + self.prefix + f'positions/{guid}', headers=self.headers, timeout=self.timeout ).json() @@ -486,7 +517,10 @@ def status_get(self) -> MirStatus: if not self.connected: return try: - response = requests.get(self.prefix + f'status', headers=self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + f'status', + headers=self.headers, + timeout=self.timeout) # To prevent adapter crashing in case of error if response.json() is None or 'position' not in response.json(): return None @@ -520,7 +554,10 @@ def me_get(self): if not self.connected: return try: - response = requests.get(self.prefix + 'users/me', headers=self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + 'users/me', + headers=self.headers, + timeout=self.timeout) if self.debug: print(f'Response: {response.json()}') return response.json() @@ -533,7 +570,10 @@ def mission_groups_get(self): if not self.connected: return try: - response = requests.get(self.prefix + f'mission_groups', headers = self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + f'mission_groups', + headers=self.headers, + timeout=self.timeout) return response.json() except HTTPError as http_err: print(f"HTTP error: {http_err}") @@ -544,7 +584,10 @@ def actions_get(self): if not self.connected: return try: - response = requests.get(self.prefix + 'actions', headers=self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + 'actions', + headers=self.headers, + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") return response.json() @@ -557,7 +600,10 @@ def action_type_get(self, action_type: str): if not self.connected: return try: - response = requests.get(self.prefix + f'actions/{action_type}', headers=self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + f'actions/{action_type}', + headers=self.headers, + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") return response.json() @@ -570,7 +616,10 @@ def missions_get(self): if not self.connected: return try: - response = requests.get(self.prefix + 'missions', headers=self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + 'missions', + headers=self.headers, + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") return response.json() @@ -583,7 +632,10 @@ def positions_get(self): if not self.connected: return try: - response = requests.get(self.prefix + 'positions' , headers=self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + 'positions', + headers=self.headers, + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") return response.json() @@ -596,7 +648,10 @@ def mission_queue_get(self): if not self.connected: return try: - response = requests.get(self.prefix + f'mission_queue', headers = self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + f'mission_queue', + headers=self.headers, + timeout=self.timeout) return response.json() except HTTPError as http_err: print(f"HTTP error: {http_err}") @@ -607,7 +662,10 @@ def mission_queue_id_get(self, mission_queue_id): if not self.connected: return try: - response = requests.get(self.prefix + f'mission_queue/{mission_queue_id}', headers = self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + f'mission_queue/{mission_queue_id}', + headers=self.headers, + timeout=self.timeout) return response.json() except HTTPError as http_err: print(f"HTTP error: {http_err}") @@ -619,14 +677,19 @@ def mission_queue_post(self, mission_id, full_mission_description=None): return data = {'mission_id': mission_id} if full_mission_description is not None: - # print(f'---------->>> {full_mission_description}') data = full_mission_description if mission_id != full_mission_description['mission_id']: - print(f'Inconsistent mission id, provided [{mission_id}], full_mission_description: [{full_mission_description}]') + print(f'Inconsistent mission id, provided [{mission_id}], ' + f'full_mission_description: ' + f'[{full_mission_description}]') return try: - response = requests.post(self.prefix + 'mission_queue' , headers = self.headers, data=json.dumps(data), timeout = self.timeout) + response = requests.post( + self.prefix + 'mission_queue', + headers=self.headers, + data=json.dumps(data), + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") return response.json() @@ -639,7 +702,11 @@ def missions_mission_id_actions_post(self, mission_id, body): if not self.connected: return try: - response = requests.post(self.prefix + 'missions/' + mission_id +'/actions' , headers = self.headers, data=json.dumps(body), timeout = self.timeout) + response = requests.post( + self.prefix + 'missions/' + mission_id + '/actions', + headers=self.headers, + data=json.dumps(body), + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") print(response.status_code) @@ -653,7 +720,10 @@ def missions_mission_id_actions_get(self, mission_id): if not self.connected: return try: - response = requests.get(self.prefix + 'missions/' + str(mission_id) +'/actions' , headers = self.headers, timeout = self.timeout) + response = requests.get( + self.prefix + 'missions/' + str(mission_id) + '/actions', + headers=self.headers, + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") print(response.status_code) @@ -673,7 +743,11 @@ def mission_groups_post(self, group_name): 'icon': '' } try: - response = requests.post(self.prefix + 'mission_groups' , headers = self.headers, data=json.dumps(data), timeout = self.timeout) + response = requests.post( + self.prefix + 'mission_groups', + headers=self.headers, + data=json.dumps(data), + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") return response.json() @@ -691,7 +765,11 @@ def missions_post(self, mission, mission_group_id): 'group_id': mission_group_id } try: - response = requests.post(self.prefix + 'missions' , headers = self.headers, data=json.dumps(data), timeout = self.timeout) + response = requests.post( + self.prefix + 'missions', + headers=self.headers, + data=json.dumps(data), + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") return response.json() @@ -704,7 +782,10 @@ def positions_guid_get(self, guid): if not self.connected: return try: - response = requests.get(self.prefix + 'positions/'+ guid, headers=self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + 'positions/' + guid, + headers=self.headers, + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") return response.json() @@ -718,9 +799,13 @@ def status_put(self, state_id): return data = {"state_id": state_id} try: - response = requests.put(self.prefix + 'status', headers = self.headers, data=json.dumps(data), timeout=self.timeout) + response = requests.put( + self.prefix + 'status', + headers=self.headers, + data=json.dumps(data), + timeout=self.timeout) if self.debug: - print(f"Response: {response.json()}") + print(f"Response: {response.json()}") return response.json() except HTTPError as http_err: print(f"HTTP error: {http_err}") @@ -732,9 +817,13 @@ def clear_error(self): return data = {"clear_error": True} try: - response = requests.put(self.prefix + 'status', headers = self.headers, data=json.dumps(data), timeout=self.timeout) + response = requests.put( + self.prefix + 'status', + headers=self.headers, + data=json.dumps(data), + timeout=self.timeout) if self.debug: - print(f"Response: {response.json()}") + print(f"Response: {response.json()}") return response.json() except HTTPError as http_err: print(f"HTTP error: {http_err}") @@ -745,7 +834,10 @@ def mission_queue_delete(self): if not self.connected: return try: - response = requests.delete(self.prefix + 'mission_queue' , headers = self.headers, timeout = self.timeout) + response = requests.delete( + self.prefix + 'mission_queue', + headers=self.headers, + timeout=self.timeout) if self.debug: print(f"Response: {response.headers}") return True @@ -762,8 +854,8 @@ def mission_queue_id_delete(self, mission_queue_id): try: response = requests.delete( self.prefix + 'mission_queue/' + str(mission_queue_id), - headers = self.headers, - timeout = self.timeout + headers=self.headers, + timeout=self.timeout ) if self.debug: print(f"Response: {response.headers}") @@ -779,7 +871,10 @@ def missions_guid_delete(self, guid): if not self.connected: return try: - response = requests.delete(self.prefix + 'missions/' +guid , headers = self.headers, timeout = self.timeout) + response = requests.delete( + self.prefix + 'missions/' + guid, + headers=self.headers, + timeout=self.timeout) if self.debug: print(f"Response: {response.headers}") return True @@ -794,7 +889,10 @@ def maps_get(self): if not self.connected: return [] try: - response = requests.get(self.prefix + 'maps', headers = self.headers, timeout = self.timeout) + response = requests.get( + self.prefix + 'maps', + headers=self.headers, + timeout=self.timeout) if self.debug: print(f"Response: {response.headers}") return response.json() @@ -809,7 +907,10 @@ def docking_offsets_get(self): if not self.connected: return try: - response = requests.get(self.prefix + 'docking_offsets', headers=self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + 'docking_offsets', + headers=self.headers, + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") return response.json() @@ -829,7 +930,10 @@ def footprints_get(self): if not self.connected: return try: - response = requests.get(self.prefix + 'footprints', headers=self.headers, timeout=self.timeout) + response = requests.get( + self.prefix + 'footprints', + headers=self.headers, + timeout=self.timeout) if self.debug: print(f"Response: {response.json()}") return response.json() @@ -849,12 +953,17 @@ def mission_completed(self, mission_queue_id): mission_status = self.mission_queue_id_get(mission_queue_id) if not mission_status: return False - if 'finished' in mission_status and mission_status['finished'] is not None: - # if 'state' in mission_status and mission_status['state'] == 'Done': + if ('finished' in mission_status and + mission_status['finished'] is not None): return True return False - def get_mission_params_with_value(self, mission_name: str, action_type: str, param_name: str, value: str): + def get_mission_params_with_value( + self, + mission_name: str, + action_type: str, + param_name: str, + value: str): mission_actions = self.mission_actions.get(mission_name) mission_params = None for d in mission_actions: diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 584f269..3b3d296 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -58,6 +58,7 @@ def activity(self): return execution.identifier return None + class RobotAdapterMiR: def __init__( self, @@ -114,7 +115,9 @@ def __init__( f'MiR is missing the position [{position_name}] needed as ' f'a lift position on level [{level}] for lift [{lift}]' ) - self.nav_issue_ticket = None # We should only have one issue ticket at a time to manage unsuccessful missions + # We should only have one issue ticket at a time to manage unsuccessful + # navigation missions + self.nav_issue_ticket = None self.requested_replan = False self.replan_counts = 0 @@ -126,8 +129,11 @@ def __init__( self._make_callbacks(), ) - self.current_action = None # Tracks the current ongoing action - self.plugin_config = plugin_config # Stores all the configured plugin action configs + # Track the current ongoing action + self.current_action = None + + # Store all the configured plugin action configs + self.plugin_config = plugin_config @property def activity(self): @@ -141,7 +147,7 @@ def activity(self): async def update_loop(self, period): while rclpy.ok(): now = self.node.get_clock().now() - next_wakeup = now + Duration(nanoseconds = period * 1e9) + next_wakeup = now + Duration(nanoseconds=period * 1e9) await self.request_update() while self.node.get_clock().now() < next_wakeup: time.sleep(0.01) @@ -152,10 +158,13 @@ def request_update(self): status = self.api.status_get() if status is None: self.disconnect = True - self.node.get_logger().warn(f'Unable to retrieve status from robot [{self.name}]!') + self.node.get_logger().warn( + f'Unable to retrieve status from robot [{self.name}]!') return if self.disconnect: - self.node.get_logger().info(f'Robot [{self.name}] connectivity resumed, received status from robot successfully.') + self.node.get_logger().info( + f'Robot [{self.name}] connectivity resumed, received status ' + f'from robot successfully.') self.disconnect = False more = self.update_handle.more() if more is not None: @@ -166,7 +175,8 @@ def request_update(self): self.update_mission_status(status, mission) # Update RMF with the latest RobotState - if mission is None or (mission.localize and mission.done) or not mission.localize: + if (mission is None or (mission.localize and mission.done) or + not mission.localize): self.update_handle.update(status.state, self.activity) self.last_known_status = status else: @@ -180,19 +190,26 @@ def request_update(self): # PerformAction related checks if self.current_action: if self.current_action.update_action(): - # This means that the action has ended, we can clear the current action object - self.node.get_logger().info(f'Robot [{self.name}] has completed its current action.') + # This means that the action has ended, we can clear the + # current action object + self.node.get_logger().info( + f'Robot [{self.name}] has completed its current action.') self.current_action = None # Clear error on updates - if status is not None and 'errors' in status.response and len(status.response['errors']) > 0: + if (status is not None and 'errors' in status.response and + len(status.response['errors']) > 0): self.api.clear_error() - if status is not None and (status.response['state_id'] == MiRStateCode.PAUSE or status.response['state_id'] == MiRStateCode.ERROR): + if (status is not None and + (status.response['state_id'] == MiRStateCode.PAUSE or + status.response['state_id'] == MiRStateCode.ERROR)): self.api.status_put(MiRStateCode.READY) def is_charging(self, status: MirStatus): - # Note: Not the best way to verify that robot is charging but there's currently no other option - if status.response.get('mission_text') == "Charging... Waiting for new mission...": + # Note: Not the best way to verify that robot is charging but there's + # currently no other option + if (status.response.get('mission_text') == + "Charging... Waiting for new mission..."): return True return False @@ -202,7 +219,8 @@ def update_rmf_finished(self, mission: MissionHandle): mission.execution.finished() mission.execution = None - def update_mission_status(self, status: MirStatus, mission: MissionHandle): + def update_mission_status( + self, status: MirStatus, mission: MissionHandle): if mission is None or mission.mission_queue_id is None: # Either we don't have a mission or we don't know the # mission_queue_id yet so just continue as normal for now @@ -212,13 +230,15 @@ def update_mission_status(self, status: MirStatus, mission: MissionHandle): # This mission is already finished, so we return early return - mission_status = self.api.mission_queue_id_get(mission.mission_queue_id) + mission_status = self.api.mission_queue_id_get( + mission.mission_queue_id) executing_mission = status.response.get('mission_queue_id') if executing_mission == mission.mission_queue_id: # The current mission is still being executed, so we don't # need to change anything if mission.charger is not None and self.is_charging(status): - self.node.get_logger().info(f'Robot [{self.name}] has begun charging...') + self.node.get_logger().info( + f'Robot [{self.name}] has begun charging...') mission.docking = False mission.done = True return @@ -227,17 +247,21 @@ def update_mission_status(self, status: MirStatus, mission: MissionHandle): # the mission has finished or it has not started yet. We need a second # API call to figure out which it is. if mission_status is None or mission_status.get('state') is None: - # It shouldn't come to here, but we'll prevent any potential crashes + # It shouldn't come to here, but we'll prevent any potential + # crashes # with a check here return if mission_status['state'] == 'Done': - # The mission has finished so let's trigger execution.finished() and - # then clear it out - self.node.get_logger().info(f'MiR [{self.name}] has completed mission {mission.mission_queue_id}, ' - f'marking ActionExecution object as finished.') + # The mission has finished so let's trigger execution.finished() + # and then clear it out + self.node.get_logger().info( + f'MiR [{self.name}] has completed mission ' + f'{mission.mission_queue_id}, marking ActionExecution object ' + f'as finished.') mission.docking = False mission.done = True - # If the mission previously issued a ticket, let's resolve that ticket + # If the mission previously issued a ticket, + # let's resolve that ticket if self.nav_issue_ticket is not None: msg = {} self.nav_issue_ticket.resolve(msg) @@ -254,24 +278,31 @@ def update_mission_status(self, status: MirStatus, mission: MissionHandle): 'message': 'Mission has been aborted.' } # Save the issue ticket somewhere so that we can resolve it later - self.nav_issue_ticket = self.update_handle.more().create_issue(tier, category, detail) - self.node.get_logger().info(f'Created [{category}] issue ticket for mission queue ID [{mission.mission_queue_id}]') + self.nav_issue_ticket = \ + self.update_handle.more().create_issue(tier, category, detail) + self.node.get_logger().info( + f'Created [{category}] issue ticket for mission queue ID ' + f'[{mission.mission_queue_id}]') # After issuing a ticket, let's request a replan mission.done = True self.update_handle.more().replan() - # We keep track of the number of times we are replanning for the same mission + # We keep track of the number of times we are replanning for the + # same mission self.replan_counts += 1 - self.node.get_logger().info(f'[{self.name}] Replan count: {self.replan_counts}') + self.node.get_logger().info( + f'[{self.name}] Replan count: {self.replan_counts}') # Ensure that robot is charging - if mission.done and mission.charger is not None and not self.is_charging(status): - # Charging mission is marked as done but robot is not charging. Let's send - # the robot to the charger again. + if (mission.done and mission.charger is not None and + not self.is_charging(status)): + # Charging mission is marked as done but robot is not charging. + # Let's send the robot to the charger again. mission.done = False self.update_handle.more().replan() self.replan_counts += 1 - self.node.get_logger().info(f'[{self.name}] Replan count: {self.replan_counts}') + self.node.get_logger().info( + f'[{self.name}] Replan count: {self.replan_counts}') def _make_callbacks(self): callbacks = rmf_easy.RobotCallbacks( @@ -291,51 +322,56 @@ def _make_callbacks(self): return callbacks def navigate(self, destination, execution): - # If the nav command coming in is to bring the robot to a charger, but the robot is - # already charging at the same charger, we ignore this nav command + # If the nav command coming in is to bring the robot to a charger, but + # the robot is already charging at the same charger, we ignore this + # nav command status = self.last_known_status if status and self.is_charging(status): ignore_charging_cmd = False mission = self.mission - # If the robot's previous mission was a charging mission to the same charger - if mission and mission.charger is not None and destination.name == mission.charger: - ignore_charging_cmd = True - # If the robot was already charging at the same charger before any missions were - # sent/stored - elif destination.map == self.current_map and \ - (self.dist(destination.position, status.state.position) < - self.update_handle.max_merge_waypoint_distance()): + # If the robot's previous mission was a charging mission to the + # same charger + if (mission and mission.charger is not None and + destination.name == mission.charger): ignore_charging_cmd = True if ignore_charging_cmd: - self.node.get_logger().info(f'[{self.name}] Received navigation command to dock to ' - f'charger {destination.name} when robot is already charging ' - f'at the same charger, ignoring command and marking it as ' - f'finished.') + self.node.get_logger().info( + f'[{self.name}] Received navigation command to dock to ' + f'charger {destination.name} when robot is already ' + f'charging at the same charger, ignoring command and ' + f'marking it as finished.') execution.finished() return # If the robot already has a mission, cancel it - self.node.get_logger().info(f'[{self.name}] Calling stop for new navigation command.') + self.node.get_logger().info( + f'[{self.name}] Calling stop for new navigation command.') self.request_stop(self.mission) # If this is a docking task, send the appropriate docking mission if destination.dock is not None: dock_json = json.loads(destination.dock) - if dock_json.get('mission_name') == self.api.dock_and_charge_mission: - self.mission = MissionHandle(execution, charger=destination.name) + if (dock_json.get('mission_name') == + self.api.dock_and_charge_mission): + self.mission = MissionHandle( + execution, charger=destination.name) # Clear the mission queue before requesting dock and charge - self.node.get_logger().info(f'Clearing mission queue for [{self.name}] before submitting a dock_and_charge mission.') + self.node.get_logger().info( + f'Clearing mission queue for [{self.name}] before ' + f'submitting a dock_and_charge mission.') self.api.mission_queue_delete() else: self.mission = MissionHandle(execution) mission_name = dock_json.get('mission_name') - self.node.get_logger().info(f'Requesting [{mission_name}] mission for [{self.name}].') + self.node.get_logger().info( + f'Requesting [{mission_name}] mission for [{self.name}].') self.request_dock(dock_json, self.mission) return if destination.inside_lift is not None: - positions_for_lift = self.lift_positions.get(destination.inside_lift.name) + positions_for_lift = self.lift_positions.get( + destination.inside_lift.name) if positions_for_lift is not None: position_info = positions_for_lift.get(destination.map) if position_info is not None: @@ -353,7 +389,8 @@ def navigate(self, destination, execution): # We are already close enough to the point, so we # will just have the robot stop and tell RMF to go # ahead. - self.node.get_logger().info(f'[{self.name}] Calling stop in the lift.') + self.node.get_logger().info( + f'[{self.name}] Calling stop in the lift.') self.request_stop(self.mission) self.mission = None execution.finished() @@ -362,7 +399,8 @@ def navigate(self, destination, execution): # desired center point, so we will ask the robot to # simply move there self.mission = MissionHandle(execution) - self.request_go_to_known_position(position_name, self.mission) + self.request_go_to_known_position( + position_name, self.mission) return # If this is a move to task, send rmf_move mission @@ -420,16 +458,17 @@ def request_navigate( navigation_mission_identifier != mission_handle.execution.identifier): self.node.get_logger().info( - f'[{self.name}] Failed to request robot to move to destination.' - f' Navigation step {navigation_mission_identifier} interrupted ' - f'by new mission {mission_handle.execution.identifier}.' + f'[{self.name}] Failed to request robot to move to ' + f'destination. Navigation step ' + f'{navigation_mission_identifier} interrupted by new mission ' + f'{mission_handle.execution.identifier}.' ) return if mission_queue_id is None: self.node.get_logger().info( - f'[{self.name}] Failed to request robot to move to destination.' - f' Maximum request navigate retries exceeded!') + f'[{self.name}] Failed to request robot to move to ' + f'destination. Maximum request navigate retries exceeded!') mission_handle.execution.finished() return @@ -462,8 +501,9 @@ def request_go_to_known_position( mission_queue_id = self.api.go_to_known_position(position_name) if mission_queue_id is None: self.node.get_logger().info( - f'[{self.name}] Move to known positions mission cannot ' - f'be queued (no mission queue id provided), retrying...' + f'[{self.name}] Move to known positions mission ' + f'cannot be queued (no mission queue id provided), ' + f'retrying...' ) except Exception as err: self.node.get_logger().info( @@ -493,7 +533,6 @@ def request_go_to_known_position( mission_handle.set_mission_queue_id(mission_queue_id) - @parallel def request_dock( self, @@ -531,9 +570,8 @@ def request_dock( start_waypoint = None mission_name = docking_points['mission_name'] - mission_queue_id = self.api.dock(mission_name, - start_waypoint, - end_waypoint) + mission_queue_id = self.api.dock( + mission_name, start_waypoint, end_waypoint) if mission_queue_id is None: self.node.get_logger().info( f'[{self.name}] Dock mission cannot be queued ' @@ -550,16 +588,17 @@ def request_dock( docking_mission_identifier != mission_handle.execution.identifier): self.node.get_logger().info( - f'[{self.name}] Failed to request robot to dock to destination.' - f' Docking step {docking_mission_identifier} interrupted ' - f'by new mission {mission_handle.execution.identifier}.' + f'[{self.name}] Failed to request robot to dock to ' + f'destination. Docking step {docking_mission_identifier} ' + f'interrupted by new mission ' + f'{mission_handle.execution.identifier}.' ) return if mission_queue_id is None: self.node.get_logger().info( - f'[{self.name}] Failed to request robot to dock to destination.' - f' Maximum request navigate retries exceeded!') + f'[{self.name}] Failed to request robot to dock to ' + f'destination. Maximum request navigate retries exceeded!') mission_handle.execution.finished() return @@ -569,10 +608,14 @@ def stop(self, activity): mission = self.mission if mission is not None: if mission.docking: - self.node.get_logger().info(f'Robot is performing docking mission, ignoring stop issued by RMF') + self.node.get_logger().info( + f'Robot [{self.name}] is performing docking mission, ' + f'ignoring stop issued by RMF') return - if mission.execution is not None and activity.is_same(mission.execution.identifier): - self.node.get_logger().info(f'[{self.name}] Stop requested from RMF!') + if (mission.execution is not None and + activity.is_same(mission.execution.identifier)): + self.node.get_logger().info( + f'[{self.name}] Stop requested from RMF!') self.request_stop(mission) self.mission = None @@ -580,7 +623,8 @@ def stop(self, activity): def request_stop(self, mission: MissionHandle): if mission is not None: with mission.mutex: - if mission.mission_queue_id is not None and not mission.do_not_cancel: + if (mission.mission_queue_id is not None and + not mission.do_not_cancel): self.api.mission_queue_id_delete(mission.mission_queue_id) def localize(self, estimate, execution): @@ -600,17 +644,21 @@ def request_localize(self, estimate, mission: MissionHandle): estimate.map, estimate.position, estimate.graph_index ) if mission_queue_id is not None: - self.node.get_logger().info(f'Localize mission is successfully queued!') + self.node.get_logger().info( + f'[{self.name}] Localize mission is successfully ' + f'queued!') break except Exception as err: self.node.get_logger().info( - f'Failed to localize on map {estimate.map}: {err}. Retrying...' + f'Failed to localize on map {estimate.map}: {err}. ' + f'Retrying...' ) time.sleep(1) if not mission_queue_id: self.node.get_logger().info( - f'Failed to localize on map {estimate.map}. Maximum localize retries exceeded!' + f'[{self.name}] Failed to localize on map {estimate.map}. ' + f'Maximum localize retries exceeded!' ) mission.execution.finished() mission_queue_id = None @@ -621,7 +669,9 @@ def request_localize(self, estimate, mission: MissionHandle): def perform_action(self, category, description, execution): if self.current_action: # Should not reach here, but we log an error anyway - self.node.get_logger().info(f'Robot is busy with another perform action! Ignoring new action [{category}]') + self.node.get_logger().info( + f'Robot [{self.name}] is busy with another perform action! ' + f'Ignoring new action [{category}]') execution.finished() return @@ -632,12 +682,9 @@ def perform_action(self, category, description, execution): module = config['module'] plugin = importlib.import_module(module) # Create the relevant MirAction - action_obj = plugin.ActionFactory().make_action(self.node, - self.name, - self.api, - self.update_handle, - self.fleet_config, - config) + action_obj = plugin.ActionFactory().make_action( + self.node, self.name, self.api, self.update_handle, + self.fleet_config, config) # Begin performing the plugin action action_obj.perform_action(category, description, execution) # Keep track of the current action @@ -645,10 +692,11 @@ def perform_action(self, category, description, execution): return # No relevant perform action found - self.node.get_logger().info(f'Perform action [{category}] was not configured for this fleet') + self.node.get_logger().info( + f'Perform action [{category}] was not configured for this fleet') raise NotImplementedError def dist(self, A, B): - assert(len(A) > 1) - assert(len(B) > 1) + assert (len(A) > 1) + assert (len(B) > 1) return math.sqrt((A[0] - B[0])**2 + (A[1] - B[1])**2) diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index a88694d..658c4bd 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -34,7 +34,7 @@ def __init__( # Check if these missions are already created on the robot missions_created = True for mission_name in action_missions.keys(): - if not mission_name in self.api.known_missions: + if mission_name not in self.api.known_missions: missions_created = False break if missions_created: @@ -44,19 +44,21 @@ def __init__( self.api.create_missions(action_missions) # Update mission actions stored in MirAPI for mission, mission_data in self.api.known_missions.items(): - self.api.mission_actions[mission] = self.api.missions_mission_id_actions_get(mission_data['guid']) + self.api.mission_actions[mission] = \ + self.api.missions_mission_id_actions_get( + mission_data['guid']) # This will be called whenever an action has begun @abstractmethod def perform_action(self, category: str, description: dict, - execution # rmf_fleet_adapter.ActionExecution - ): + execution): # rmf_fleet_adapter.ActionExecution # To be populated in the plugins ... - # This will be called on every update to check on the action's current state + # This will be called on every update to check on the action's + # current state @abstractmethod def update_action(self): # To be populated in the plugins @@ -67,15 +69,21 @@ def cancel_current_task(self, cancel_fail: Callable[[], None], label: str = ''): current_task_id = self.update_handle.more().current_task_id() - self.node.get_logger().info(f'Cancel task requested for [{current_task_id}]') + self.node.get_logger().info( + f'[{self.name}] Cancel task requested for [{current_task_id}]') + def _on_cancel(result: bool): if result: - self.node.get_logger().info(f'Found task [{current_task_id}], cancelling...') + self.node.get_logger().info( + f'[{self.name}] Found task [{current_task_id}], ' + f'cancelling...') cancel_success() else: - self.node.get_logger().info(f'Failed to cancel task [{current_task_id}]') + self.node.get_logger().info( + f'[{self.name}] Failed to cancel task [{current_task_id}]') cancel_fail() - self.update_handle.more().cancel_task(current_task_id, [label], lambda result: _on_cancel(result)) + self.update_handle.more().cancel_task( + current_task_id, [label], lambda result: _on_cancel(result)) class MirActionFactory(ABC): diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index 7188c87..194f3a0 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -31,7 +31,7 @@ def __init__(self, queue_id: str, start_time: float): @dataclass class Pickup: state: PickupState - pickup_lots: list[str] # contains either a single pickup lot or list of pickup lots + pickup_lots: list[str] # contains list of pickup lots cart_id: str execution: Any mission: Mission @@ -52,7 +52,8 @@ def make_action(self, update_handle, fleet_config, action_config) -> MirAction: - return CartDelivery(node, name, mir_api, update_handle, fleet_config, action_config) + return CartDelivery( + node, name, mir_api, update_handle, fleet_config, action_config) class CartDelivery(MirAction): @@ -63,23 +64,32 @@ def __init__( mir_api, update_handle, fleet_config, - action_config - ): - MirAction.__init__(self, node, name, mir_api, update_handle, fleet_config, action_config) + action_config): + MirAction.__init__( + self, node, name, mir_api, update_handle, fleet_config, + action_config) self.pickup: Pickup = None self.dropoff: Dropoff = None - self.search_timeout = self.action_config.get('search_timeout', 60) # seconds + self.search_timeout = \ + self.action_config.get('search_timeout', 60) # seconds # Store the mission names to be used during the action - self.dock_to_cart_mission = self.action_config['missions']['dock_to_cart'] - self.pickup_mission = self.action_config['missions']['pickup'] - self.dropoff_mission = self.action_config['missions']['dropoff'] - self.exit_mission = self.action_config['missions']['exit_lot'] - self.footprint_mission = self.action_config['missions'].get('update_footprint') # Optional + self.dock_to_cart_mission = \ + self.action_config['missions']['dock_to_cart'] + self.pickup_mission = \ + self.action_config['missions']['pickup'] + self.dropoff_mission = \ + self.action_config['missions']['dropoff'] + self.exit_mission = \ + self.action_config['missions']['exit_lot'] + self.footprint_mission = \ + self.action_config['missions'].get('update_footprint') # Optional # Initialize cart marker type - self.cart_marker_type_guid = self.api.docking_offsets_guid_get(self.action_config['marker_types']['cart']) + self.cart_marker_type_guid = \ + self.api.docking_offsets_guid_get( + self.action_config['marker_types']['cart']) # Import CartDetection module if provided detection_module = self.action_config.get('cart_detection_module') @@ -87,7 +97,8 @@ def __init__( 'CartDetection module is required for CartDelivery plugin, ' + 'but it is not found!') detection_plugin = importlib.import_module(detection_module) - self.cart_detection = detection_plugin.CartDetection(mir_api, action_config) + self.cart_detection = \ + detection_plugin.CartDetection(mir_api, action_config) def perform_action(self, category, description, execution): # Check that the perform action category matches @@ -96,32 +107,39 @@ def perform_action(self, category, description, execution): # Check if the robot's latch is currently open if self.is_latch_open(): # Latch is open, unable to perform pickup - self.node.get_logger().info(f'Robot [{self.name}] latch is open, unable to perform pickup, cancelling task...') + self.node.get_logger().info( + f'Robot [{self.name}] latch is open, unable to ' + f'perform pickup, cancelling task...') self.cancel_task() return - self.node.get_logger().info(f'New pickup requested for robot [{self.name}]') + self.node.get_logger().info( + f'New pickup requested for robot [{self.name}]') self.pickup = Pickup( state=PickupState.AT_PICKUP, - pickup_lots=[description.get('pickup_lot')], # TODO(XY): Allow multiple pickups? + pickup_lots=[description.get('pickup_lot')], cart_id=description.get('cart_id'), execution=execution, mission=None, latching=False ) case 'delivery_dropoff': - self.node.get_logger().info(f'Received dropoff request!') + self.node.get_logger().info( + f'[{self.name}] Received dropoff request!') self.dropoff = Dropoff( execution=execution, mission=None ) case _: - self.node.get_logger().info(f'Invalid perform action [{category}] passed to CartDelivery, ending action.') + self.node.get_logger().info( + f'Invalid perform action [{category}] passed to ' + f'CartDelivery, ending action.') execution.finished() def update_action(self): - # There should not be a pickup and dropoff being performed simultaneously at any given time, since actions are - # dispatched sequentially + # There should not be a pickup and dropoff being performed + # simultaneously at any given time, since actions are dispatched + # sequentially if self.pickup: return self.update_pickup(self.pickup) elif self.dropoff: @@ -130,9 +148,11 @@ def update_action(self): return False def update_pickup(self, pickup: Pickup): - # If this is a PerformAction pickup, check that the action is underway and valid + # If this is a PerformAction pickup, check that the action is underway + # and valid if pickup.execution is not None and not pickup.execution.okay(): - self.node.get_logger().info(f'[delivery_pickup] action is killed/canceled.') + self.node.get_logger().info( + f'[delivery_pickup] action is killed/canceled.') pickup.state = PickupState.TASK_CANCELLED # Start state machine check @@ -145,48 +165,74 @@ def update_pickup(self, pickup: Pickup): current_wp_name = pickup.pickup_lots[0] mir_pos = self.api.known_positions.get(current_wp_name) if not mir_pos: - self.node.get_logger().info(f'No shelf position [{mir_pos}] found on robot [{self.name}], cancelling task') + self.node.get_logger().info( + f'No shelf position [{mir_pos}] found on robot ' + f'[{self.name}], cancelling task') self.cancel_task() pickup.state = PickupState.TASK_CANCELLED return cart_marker_guid = mir_pos['guid'] - cart_marker_param = self.api.get_mission_params_with_value(self.dock_to_cart_mission, 'docking', 'cart_marker', cart_marker_guid) - marker_type_param = self.api.get_mission_params_with_value(self.dock_to_cart_mission, 'docking', 'cart_marker_type', self.cart_marker_type_guid) + cart_marker_param = self.api.get_mission_params_with_value( + self.dock_to_cart_mission, + 'docking', + 'cart_marker', + cart_marker_guid) + marker_type_param = self.api.get_mission_params_with_value( + self.dock_to_cart_mission, + 'docking', + 'cart_marker_type', + self.cart_marker_type_guid) mission_params = cart_marker_param + marker_type_param - mission_queue_id = self.api.queue_mission_by_name(self.dock_to_cart_mission, mission_params) + mission_queue_id = self.api.queue_mission_by_name( + self.dock_to_cart_mission, mission_params) if not mission_queue_id: - error_str = f'Mission {self.dock_to_cart_mission} not supported, ignoring' + error_str = \ + f'Mission {self.dock_to_cart_mission} not ' + \ + f'supported, ignoring' self.node.get_logger().error(error_str) return pickup.mission = Mission(mission_queue_id, now) pickup.state = PickupState.DOCK_REQUESTED - self.node.get_logger().info(f'Dock to cart mission requested with mission queue id {mission_queue_id}') + self.node.get_logger().info( + f'[{self.name}] Dock to cart mission requested with ' + f'mission queue id {mission_queue_id}') case PickupState.DOCK_REQUESTED: # Make sure that there is an rmf_dock_to_cart mission issued if not pickup.mission: - self.node.get_logger().info(f'Robot [{self.name}] is indicated to be at the DOCK_REQUESTED state but ' - f'no mission queue ID stored for this docking mission! Returning to AT_PICKUP state.') + self.node.get_logger().info( + f'Robot [{self.name}] is indicated to be at the ' + f'DOCK_REQUESTED state but no mission queue ID stored ' + f'for this docking mission! Returning to AT_PICKUP ' + f'state.') pickup.state = PickupState.AT_PICKUP return # Mission completed, move onto the next state if self.api.mission_completed(pickup.mission.queue_id): - self.node.get_logger().info(f'Robot [{self.name}] dock to cart mission {pickup.mission.queue_id} completed or timed out.') + self.node.get_logger().info( + f'Robot [{self.name}] dock to cart mission ' + f'{pickup.mission.queue_id} completed or timed out.') pickup.mission = None pickup.state = PickupState.DOCK_COMPLETED return - # Mission not yet completed, we check the timeout status to decide if we need to publish any alert + # Mission not yet completed, we check the timeout status to + # decide if we need to publish any alert seconds_passed = now - pickup.mission.start_time # Publish update every 10 seconds just to monitor - if round(seconds_passed)%10 == 0: - self.node.get_logger().info(f'{round(seconds_passed)} seconds have passed since pickup mission requested.') + if round(seconds_passed) % 10 == 0: + self.node.get_logger().info( + f'{round(seconds_passed)} seconds have passed since ' + f'pickup mission requested.') # Mission timeout, cart not found if seconds_passed > self.search_timeout: # Delete mission from mir first self.api.mission_queue_id_delete(pickup.mission.queue_id) - # Regardless of whether the robot completed docking properly, we move to the next state to check - self.node.get_logger().info(f'Robot [{self.name}] dock to cart mission {pickup.mission.queue_id} timed out! ' - f'Configured search timeout is {self.search_timeout} seconds.') + # Regardless of whether the robot completed docking + # properly, we move to the next state to check + self.node.get_logger().info( + f'Robot [{self.name}] dock to cart mission ' + f'{pickup.mission.queue_id} timed out! Configured ' + f'search timeout is {self.search_timeout} seconds.') pickup.mission = None pickup.state = PickupState.DOCK_COMPLETED return @@ -197,23 +243,35 @@ def update_pickup(self, pickup: Pickup): if cart_check: # If cart is correct, send pickup mission assert self.pickup_mission is not None - mission_queue_id = self.api.queue_mission_by_name(self.pickup_mission) + mission_queue_id = self.api.queue_mission_by_name( + self.pickup_mission) if not mission_queue_id: - error_str = f'Mission {self.pickup_mission} not supported, ignoring' + error_str = \ + f'Mission {self.pickup_mission} not supported,' + \ + f' ignoring' self.node.get_logger().error(error_str) return pickup.mission = Mission(mission_queue_id, now) pickup.latching = True - self.node.get_logger().info(f'Robot [{self.name}] found the correct cart, pickup mission requested with mission queue id {mission_queue_id}') + self.node.get_logger().info( + f'Robot [{self.name}] found the correct cart, pickup ' + f'mission requested with mission queue id ' + f'{mission_queue_id}') pickup.state = PickupState.PICKUP_REQUESTED elif cart_check is None: # If cart is missing, cancel this task - self.node.get_logger().info(f'Robot [{self.name}] was unable to dock under any carts, please check that cart is present. Cancelling task.') + self.node.get_logger().info( + f'Robot [{self.name}] was unable to dock under any ' + f'carts, please check that cart is present. ' + f'Cancelling task.') self.cancel_task() pickup.state = PickupState.TASK_CANCELLED else: - # If cart is wrong, cancel this task also but after we exit from the lot - self.node.get_logger().info(f'Robot [{self.name}] found the wrong cart, exiting lot and cancelling task.') + # If cart is wrong, cancel this task also but after we + # exit from the lot + self.node.get_logger().info( + f'Robot [{self.name}] found the wrong cart, exiting ' + f'lot and cancelling task.') self.exit_lot() self.cancel_task() pickup.state = PickupState.TASK_CANCELLED @@ -227,18 +285,25 @@ def update_pickup(self, pickup: Pickup): case PickupState.PICKUP_SUCCESS: # Correct ID, we can end the delivery now - self.node.get_logger().info(f'Robot [{self.name}] successfully received cart, exiting lot with cart and ending mission') + self.node.get_logger().info( + f'Robot [{self.name}] successfully received cart, ' + f'exiting lot with cart and ending mission') self.exit_lot() if pickup.execution is not None: pickup.execution.finished() return True case PickupState.TASK_CANCELLED: - self.node.get_logger().info(f'Robot [{self.name}] is in pickup cancelled state.') - # If some MiR mission is in progress, we abort it unless it is latching - if pickup.mission and not self.api.mission_completed(pickup.mission.queue_id): + self.node.get_logger().info( + f'Robot [{self.name}] is in pickup cancelled state.') + # If some MiR mission is in progress, we abort it unless + # it is latching + if pickup.mission and not self.api.mission_completed( + pickup.mission.queue_id): if pickup.latching: - self.node.get_logger().info(f'Robot [{self.name}] is performing latching, cancelling task after this action is complete.') + self.node.get_logger().info( + f'Robot [{self.name}] is performing latching, ' + f'cancelling task after this action is complete.') return False self.api.mission_queue_id_delete(pickup.mission.queue_id) pickup.mission = None @@ -257,11 +322,12 @@ def update_dropoff(self, dropoff: Dropoff): self.node.get_logger().info(f'Dropoff action is killed/canceled') # If cart release is in progress, we let it finish first - if dropoff.mission and not self.api.mission_completed(dropoff.mission.queue_id): + if dropoff.mission and not self.api.mission_completed( + dropoff.mission.queue_id): return False - # Task is cancelled and cart is done dropping off/mission is not yet queued anyway, - # we can mark it as completed at this point + # Task is cancelled and cart is done dropping off/mission is not + # yet queued anyway, we can mark it as completed at this point self.api.clear_error() self.api.status_put(state_id=MiRStateCode.READY) # Mark dropoff session as completed @@ -270,33 +336,42 @@ def update_dropoff(self, dropoff: Dropoff): # No mission queued yet if dropoff.mission is None: assert self.dropoff_mission is not None - mission_queue_id = self.api.queue_mission_by_name(self.dropoff_mission) + mission_queue_id = self.api.queue_mission_by_name( + self.dropoff_mission) if not mission_queue_id: - error_str = f'Mission {self.dropoff_mission} not supported, ignoring' + error_str = \ + f'Mission {self.dropoff_mission} not supported, ignoring' self.node.get_logger().error(error_str) return True - self.node.get_logger().info(f'Mission {self.dropoff_mission} added to queue for robot [{self.name}].') + self.node.get_logger().info( + f'Mission {self.dropoff_mission} added to queue for robot ' + f'[{self.name}].') now = self.node.get_clock().now().nanoseconds / 1e9 dropoff.mission = Mission(mission_queue_id, now) - self.node.get_logger().info(f'[{self.name}] Dropoff mission requested with mission queue id {mission_queue_id}') + self.node.get_logger().info( + f'[{self.name}] Dropoff mission requested with mission queue ' + f'id {mission_queue_id}') # Mission queued, check completion else: if self.api.mission_completed(dropoff.mission.queue_id): - self.node.get_logger().info(f'[{self.name}] Dropoff mission completed!') + self.node.get_logger().info( + f'[{self.name}] Dropoff mission completed!') # Update robot footprint if dropoff.execution is not None: dropoff.execution.finished() return True else: - self.node.get_logger().info(f'[{self.name}] Dropoff mission in progress...') + self.node.get_logger().info( + f'[{self.name}] Dropoff mission in progress...') return def cancel_task(self, label: str = ''): def _cancel_success(): if self.pickup: self.pickup.state = PickupState.TASK_CANCELLED + def _cancel_fail(): pass self.cancel_current_task(_cancel_success, _cancel_fail, label) @@ -319,50 +394,57 @@ def exit_lot(self): def release_cart(self): # If robot latch is open, close it if self.is_latch_open(): - self.node.get_logger().info(f'Robot [{self.name}] detected to have latch open, dropping off cart...') + self.node.get_logger().info( + f'Robot [{self.name}] detected to have latch open, dropping ' + f'off cart...') self.dropoff = Dropoff( execution=None, mission_queue_id=None) - # Dropoff mission will take care of the lot exit, so we can return here + # Dropoff mission will take care of the lot exit, + # so we can return here return # If robot is under a cart, exit lot if self.is_under_cart(): - self.node.get_logger().info(f'Cart detected above robot [{self.name}] during a release call, exiting lot...') + self.node.get_logger().info( + f'Cart detected above robot [{self.name}] during a release ' + f'call, exiting lot...') self.exit_lot() - # Optional function for updating robot footprint by providing footprint guid + # Optional function for updating robot footprint by providing + # footprint guid def update_footprint(self, ft_guid: str): if not self.footprint_mission: return - ft_params = self.api.get_mission_params_with_value(self.footprint_mission, - 'set_footprint', - 'footprint', - ft_guid) - return self.api.queue_mission_by_name(self.footprint_mission, ft_params) + ft_params = self.api.get_mission_params_with_value( + self.footprint_mission, 'set_footprint', 'footprint', ft_guid) + return self.api.queue_mission_by_name( + self.footprint_mission, ft_params) def retrieve_mir_coordinates(self, waypoint_name: str): transform = self.fleet_config.transformations_to_robot_coordinates transform_current_map = transform.get(self.current_map) - rmf_pose = self.fleet_config.graph.find_waypoint(waypoint_name).location + rmf_pose = \ + self.fleet_config.graph.find_waypoint(waypoint_name).location new_rmf_pose = np.array([rmf_pose[0], rmf_pose[1], 0.0]) mir_pose = transform_current_map.apply(new_rmf_pose) return mir_pose def dist(self, A, B): - assert(len(A) > 1) - assert(len(B) > 1) + assert (len(A) > 1) + assert (len(B) > 1) return math.sqrt((A[0] - B[0])**2 + (A[1] - B[1])**2) - - # ------------------------------------------------------------------------------------------------------------------ + # -------------------------------------------------------------------------- # MIR API FOR CART RELATED MISSIONS - # ------------------------------------------------------------------------------------------------------------------ - + # -------------------------------------------------------------------------- def register_get(self, register: int): if not self.api.connected: return None try: - response = requests.get(self.api.prefix + f'registers/{register}', headers = self.api.headers, timeout = self.api.timeout) + response = requests.get( + self.api.prefix + f'registers/{register}', + headers=self.api.headers, + timeout=self.api.timeout) if self.api.debug: print(f"Response: {response.headers}") # Response value is string, return integer of value @@ -380,7 +462,10 @@ def io_module_guid_status_get(self, io_guid: str): if io_guid is None: return None try: - response = requests.get(self.api.prefix + f'io_modules/{io_guid}/status', headers = self.api.headers, timeout = self.api.timeout) + response = requests.get( + self.api.prefix + f'io_modules/{io_guid}/status', + headers=self.api.headers, + timeout=self.api.timeout) if self.api.debug: print(f"Response: {response.headers}") if 'input_state' not in response.json(): @@ -397,7 +482,10 @@ def io_modules_get(self): if not self.api.connected: return None try: - response = requests.get(self.api.prefix + f'io_modules', headers = self.api.headers, timeout = self.api.timeout) + response = requests.get( + self.api.prefix + f'io_modules', + headers=self.api.headers, + timeout=self.api.timeout) if self.api.debug: print(f"Response: {response.headers}") # Response value is string, return integer of value diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py index c209270..f563155 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py @@ -1,6 +1,7 @@ import requests from urllib.error import HTTPError + class CartDetection: def __init__( self, @@ -49,9 +50,10 @@ def register_get(self, register: int): if not self.api.connected: return None try: - response = requests.get(self.api.prefix + f'registers/{register}', - headers=self.api.headers, - timeout=self.api.timeout) + response = requests.get( + self.api.prefix + f'registers/{register}', + headers=self.api.headers, + timeout=self.api.timeout) if self.api.debug: print(f"Response: {response.headers}") # Response value is string, return integer of value @@ -69,10 +71,10 @@ def io_module_guid_status_get(self, io_guid: str): if io_guid is None: return None try: - response = requests.get(self.api.prefix + - f'io_modules/{io_guid}/status', - headers=self.api.headers, - timeout=self.api.timeout) + response = requests.get( + self.api.prefix + f'io_modules/{io_guid}/status', + headers=self.api.headers, + timeout=self.api.timeout) if self.api.debug: print(f"Response: {response.headers}") if 'input_state' not in response.json(): @@ -89,9 +91,10 @@ def io_modules_get(self): if not self.api.connected: return None try: - response = requests.get(self.api.prefix + f'io_modules', - headers=self.api.headers, - timeout=self.api.timeout) + response = requests.get( + self.api.prefix + f'io_modules', + headers=self.api.headers, + timeout=self.api.timeout) if self.api.debug: print(f"Response: {response.headers}") # Response value is string, return integer of value @@ -101,4 +104,4 @@ def io_modules_get(self): return None except Exception as err: print(f"Other error: {err}") - return None \ No newline at end of file + return None diff --git a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py index b21c35c..15b3246 100644 --- a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py +++ b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py @@ -112,18 +112,20 @@ def __init__(self, argv=sys.argv): }] description["phases"].append( {"activity": {"category": "sequence", - "description": {"activities": go_to_pickup_activity}}}) - pickup_action_activity = [{"category": "perform_action", - "description": { - "unix_millis_action_duration_estimate": 60000, - "category": 'delivery_pickup', - "description": { - "cart_id": self.args.cart_id, - "pickup_lot": self.args.pickup_lot - }}}] + "description": { + "activities": go_to_pickup_activity}}}) + pickup_action_activity = [{ + "category": "perform_action", + "description": { + "unix_millis_action_duration_estimate": 60000, + "category": 'delivery_pickup', + "description": { + "cart_id": self.args.cart_id, + "pickup_lot": self.args.pickup_lot}}}] description["phases"].append( {"activity": {"category": "sequence", - "description": {"activities": pickup_action_activity}}}) + "description": { + "activities": pickup_action_activity}}}) # GoToPlace activity go_to_dropoff_activity = [{ "category": "go_to_place", @@ -131,7 +133,8 @@ def __init__(self, argv=sys.argv): }] description["phases"].append( {"activity": {"category": "sequence", - "description": {"activities": go_to_dropoff_activity}}}) + "description": { + "activities": go_to_dropoff_activity}}}) # Dropoff activity dropoff_action_activity = [{ "category": "perform_action", @@ -143,7 +146,8 @@ def __init__(self, argv=sys.argv): }] description["phases"].append( {"activity": {"category": "sequence", - "description": {"activities": dropoff_action_activity}}}) + "description": { + "activities": dropoff_action_activity}}}) # Consolidate request["description"] = description payload["request"] = request From 933358f8a73b2050d60473ee1b5c28926f0e9020 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 30 Jul 2024 18:11:22 +0800 Subject: [PATCH 26/50] Update README with instruction to write own CartDetection module Signed-off-by: Xiyu Oh --- fleet_adapter_mir/README.md | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/fleet_adapter_mir/README.md b/fleet_adapter_mir/README.md index 6ae44e6..e9b13c2 100644 --- a/fleet_adapter_mir/README.md +++ b/fleet_adapter_mir/README.md @@ -73,7 +73,7 @@ Upon launch, the MiR fleet adapter recognizes MiR positions with identical names ### Plugins -The MiR is capable of performing various types of custom missions and tasks. You can now easily set up plugins offered in this repo instead of writing your own perform action for common use cases. +The MiR is capable of performing various types of custom missions and tasks. You can now easily set up plugins offered in this repo instead of writing your own perform action for common use cases. These plugins offered are available under the `fleet_adapter_mir_actions` package. For cart deliveries from point A to B: @@ -91,17 +91,18 @@ Some relevant MiR missions (docking, exit, update footprint) will be automatical They are defined and stored in the `rmf_cart_missions.json` file and do not require any further configuration. -However, since there are various types of latching methods available for different MiR models, users will need to set up their custom pickup and dropoff missions on the MiR: +However, since there are various types of latching methods available for different MiR models, users will need to set up their custom pickup and dropoff missions on the MiR, as well as implement their own `CartDetection` plugin module with the appropriate APIs to detect latching states. 1. Create 2 missions on the MiR: - `rmf_pickup_cart`: Triggers the robot's latching module to open - `rmf_dropoff_cart`: Triggers the robot's latching module to close and release the cart, then exit from under the cart (relative move -1 metre in the X-direction) 2. Fill in the MiR mission names in the plugin config under `missions`. The default mission names are `rmf_pickup_cart` and `rmf_dropoff_cart`. 3. Fill in the footprints and marker types to be used for your specific robot and cart in the plugin config. -4. In `rmf_cart_delivery.py`, fill in the logic to check whether the robot's latching module is open in blanks marked `# IMPLEMENT YOUR CODE HERE`. Some API calls to check the MiR's PLC registers and IO modules are provided in case you may want to use them. +4. Create your own `CartDetection` plugin. You may use `rmf_cart_detection.py` as a reference for how your plugin module should be implemented. Fill in the logic to check whether the robot's latching module is open in blanks marked `# IMPLEMENT YOUR CODE HERE`. Some API calls to check the MiR's PLC registers and IO modules are provided in case you may want to use them. +5. In the plugin config, update the `cart_detection_module` field to point to your own written module. -To submit a cart delivery task, you may use the `dispatch_pickup` task script: +To submit a cart delivery task, you may use the `dispatch_delivery` task script found in the `fleet_adapter_mir_tasks` package: ```bash -ros2 run fleet_adapter_mir_tasks dispatch_pickup -g go_to_waypoint -p pickup_lot -d dropoff_lot -c some_cart_id +ros2 run fleet_adapter_mir_tasks dispatch_delivery -g go_to_waypoint -p pickup_lot -d dropoff_lot -c some_cart_id ``` - `-g`: Takes in an existing waypoint name for the robot to travel to before performing the pickup - `-p`: Name of the pickup lot. This name should be identical to the shelf position configured on the MiR. From 52c4fbdce74e86deaaa96792baececbb491ee0ce Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 2 Aug 2024 11:29:00 +0800 Subject: [PATCH 27/50] Remove position_put logic when localizing to prevent overwriting existing MiR coords Signed-off-by: Xiyu Oh --- fleet_adapter_mir/fleet_adapter_mir/mir_api.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py index 6f34769..385c5e3 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py @@ -397,15 +397,14 @@ def position_matches(pos): # The position does not exist so we need to create a new one position_guid = self.positions_post(name, map_id, location) if position_guid is not None: + print( + f'Successfully posted position {name} on MiR with ' + f'location {location}' + ) return position_guid - # For some reason posting the new position failed. Maybe there - # was a timeout or an argument error. We will update the known - # positions and retry this loop. - self.update_known_positions() - elif not position_matches(position): - if self.positions_put( - position['guid'], name, map_id, location): - return position['guid'] + print( + f'Unable to post position {name} on MiR!' + ) else: return position['guid'] From 47d5488f62f8866bbb2c35b8f73aecdd69643b87 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 2 Aug 2024 14:06:16 +0800 Subject: [PATCH 28/50] Add disconnect mutex Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 137 ++++++++++++------ 1 file changed, 89 insertions(+), 48 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 3b3d296..7f648bd 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -82,6 +82,8 @@ def __init__( self.debug = debug self.disconnect = False + self.disconnect_mutex = Lock() + self.replan_after_disconnect = False prefix = mir_config['base_url'] headers = { @@ -154,56 +156,67 @@ async def update_loop(self, period): @parallel def request_update(self): - # Retrieve the latest MiR status from robot - status = self.api.status_get() - if status is None: - self.disconnect = True - self.node.get_logger().warn( - f'Unable to retrieve status from robot [{self.name}]!') - return - if self.disconnect: - self.node.get_logger().info( - f'Robot [{self.name}] connectivity resumed, received status ' - f'from robot successfully.') - self.disconnect = False - more = self.update_handle.more() - if more is not None: - more.unstable_debug_positions(self.debug) - - # Update the stored mission status from MiR - mission = self.mission - self.update_mission_status(status, mission) - - # Update RMF with the latest RobotState - if (mission is None or (mission.localize and mission.done) or - not mission.localize): - self.update_handle.update(status.state, self.activity) - self.last_known_status = status - else: - self.node.get_logger().info( - f'Mission is None / Robot is localizing, ignore status update') - - # Update RMF to mark the ActionExecution as finished - if mission is not None and mission.done: - self.update_rmf_finished(mission) + with self.disconnect_mutex: + # Retrieve the latest MiR status from robot + status = self.api.status_get() + if status is None: + self.disconnect = True + self.node.get_logger().warn( + f'Unable to retrieve status from robot [{self.name}]!') + return + if self.disconnect: + self.node.get_logger().info( + f'Robot [{self.name}] connectivity resumed, received status ' + f'from robot successfully.') + self.disconnect = False + more = self.update_handle.more() + if more is not None: + more.unstable_debug_positions(self.debug) + + # Update the stored mission status from MiR + mission = self.mission + self.update_mission_status(status, mission) - # PerformAction related checks - if self.current_action: - if self.current_action.update_action(): - # This means that the action has ended, we can clear the - # current action object + # Update RMF with the latest RobotState + if (mission is None or (mission.localize and mission.done) or + not mission.localize): + self.update_handle.update(status.state, self.activity) + self.last_known_status = status + else: self.node.get_logger().info( - f'Robot [{self.name}] has completed its current action.') - self.current_action = None - - # Clear error on updates - if (status is not None and 'errors' in status.response and - len(status.response['errors']) > 0): - self.api.clear_error() - if (status is not None and - (status.response['state_id'] == MiRStateCode.PAUSE or - status.response['state_id'] == MiRStateCode.ERROR)): - self.api.status_put(MiRStateCode.READY) + f'Mission is None / Robot is localizing, ignore status update') + + # Update RMF to mark the ActionExecution as finished + if mission is not None and mission.done: + self.update_rmf_finished(mission) + + # Check if we need to replan after a disconnect, making sure that + # we have completed any ongoing mission and have updated the EFC + # handle with our latest robot status + if self.replan_after_disconnect and ( + mission is None or (mission and mission.done) + ): + if more is not None: + more.replan() + self.replan_after_disconnect = False + + # PerformAction related checks + if self.current_action: + if self.current_action.update_action(): + # This means that the action has ended, we can clear the + # current action object + self.node.get_logger().info( + f'Robot [{self.name}] has completed its current action.') + self.current_action = None + + # Clear error on updates + if (status is not None and 'errors' in status.response and + len(status.response['errors']) > 0): + self.api.clear_error() + if (status is not None and + (status.response['state_id'] == MiRStateCode.PAUSE or + status.response['state_id'] == MiRStateCode.ERROR)): + self.api.status_put(MiRStateCode.READY) def is_charging(self, status: MirStatus): # Note: Not the best way to verify that robot is charging but there's @@ -322,6 +335,22 @@ def _make_callbacks(self): return callbacks def navigate(self, destination, execution): + # If robot is disconnected and we are unable to retrieve any robot + # status, we'll ignore all commands from RMF until robot is re-connected + with self.disconnect_mutex: + if self.disconnect: + self.node.get_logger().info( + f'Robot [{self.name}] received new navigation command while' + f' disconnected, ignoring command and requesting replan ' + f'after robot is re-connected.' + ) + self.replan_after_disconnect = True + return + + self.node.get_logger().info( + f'New navigation command received for [{self.name}]' + ) + # If the nav command coming in is to bring the robot to a charger, but # the robot is already charging at the same charger, we ignore this # nav command @@ -605,6 +634,18 @@ def request_dock( mission_handle.set_mission_queue_id(mission_queue_id) def stop(self, activity): + # If robot is disconnected and we are unable to retrieve any robot + # status, we'll ignore all commands from RMF until robot is re-connected + with self.disconnect_mutex: + if self.disconnect: + self.node.get_logger().info( + f'Robot [{self.name}] received new stop command while' + f' disconnected, ignoring command and requesting replan ' + f'after robot is re-connected.' + ) + self.replan_after_disconnect = True + return + mission = self.mission if mission is not None: if mission.docking: From d3bf9fe2148167faa702fb39f7fcb30f6b633673 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 2 Aug 2024 16:11:06 +0800 Subject: [PATCH 29/50] Check config for lift or localize positions before requesting for localize missions Signed-off-by: Xiyu Oh --- fleet_adapter_mir/fleet_adapter_mir/mir_api.py | 14 ++++++++------ .../fleet_adapter_mir/robot_adapter_mir.py | 14 +++++++++++++- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py index 385c5e3..40dd5a9 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py @@ -341,17 +341,19 @@ def dock(self, mission_name, start_waypoint, end_waypoint): mission_params = end_param return self.queue_mission_by_name(mission_name, mission_params) - def localize(self, map, estimate, index): + def localize(self, map, estimate, index, position_name=None): if self.localize_mission is None: raise Exception( 'The fleet was not configured with a localize mission' ) - if index is not None: - position_name = f'rmf_localize_{index}' - else: - p = estimate - position_name = f'rmf_localize_{map}_{p[0]:.2f}_{p[1]:.2f}' + # If a MiR position is provided, we'll use that position directly + if position_name is None: + if index is not None: + position_name = f'rmf_localize_{index}' + else: + p = estimate + position_name = f'rmf_localize_{map}_{p[0]:.2f}_{p[1]:.2f}' mir_map = self.map_conversions.rmf_to_mir[map] map_id = self.known_maps[mir_map] position_guid = self.get_position_guid(position_name, map_id, estimate) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 7f648bd..2f91715 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -680,9 +680,21 @@ def request_localize(self, estimate, mission: MissionHandle): mission_queue_id = None while count < retry_count and not mission_queue_id: count += 1 + + # If this localize request is being made for a map switch, and a + # MiR position is provided in the fleet config for this RMF lift + # waypoint, we can localize to that position directly + mir_position = None + if estimate.inside_lift: + lift_name = estimate.inside_lift.name + this_lift = self.lift_positions.get(lift_name) + if this_lift and this_lift.get(estimate.map): + mir_position = this_lift[estimate.map].get('name') + try: mission_queue_id = self.api.localize( - estimate.map, estimate.position, estimate.graph_index + estimate.map, estimate.position, estimate.graph_index, + mir_position ) if mission_queue_id is not None: self.node.get_logger().info( From c2ecf17c9e8b0c51dd2e81c519c3d076f6ab63e5 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 2 Aug 2024 16:55:05 +0800 Subject: [PATCH 30/50] Style Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 2f91715..ac65831 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -166,8 +166,8 @@ def request_update(self): return if self.disconnect: self.node.get_logger().info( - f'Robot [{self.name}] connectivity resumed, received status ' - f'from robot successfully.') + f'Robot [{self.name}] connectivity resumed, received ' + f'status from robot successfully.') self.disconnect = False more = self.update_handle.more() if more is not None: @@ -184,7 +184,8 @@ def request_update(self): self.last_known_status = status else: self.node.get_logger().info( - f'Mission is None / Robot is localizing, ignore status update') + f'Mission is None / Robot is localizing, ignore status ' + f'update') # Update RMF to mark the ActionExecution as finished if mission is not None and mission.done: @@ -206,7 +207,8 @@ def request_update(self): # This means that the action has ended, we can clear the # current action object self.node.get_logger().info( - f'Robot [{self.name}] has completed its current action.') + f'Robot [{self.name}] has completed its current ' + f'action.') self.current_action = None # Clear error on updates @@ -215,7 +217,7 @@ def request_update(self): self.api.clear_error() if (status is not None and (status.response['state_id'] == MiRStateCode.PAUSE or - status.response['state_id'] == MiRStateCode.ERROR)): + status.response['state_id'] == MiRStateCode.ERROR)): self.api.status_put(MiRStateCode.READY) def is_charging(self, status: MirStatus): @@ -336,13 +338,14 @@ def _make_callbacks(self): def navigate(self, destination, execution): # If robot is disconnected and we are unable to retrieve any robot - # status, we'll ignore all commands from RMF until robot is re-connected + # status, we'll ignore all commands from RMF until robot is + # re-connected with self.disconnect_mutex: if self.disconnect: self.node.get_logger().info( - f'Robot [{self.name}] received new navigation command while' - f' disconnected, ignoring command and requesting replan ' - f'after robot is re-connected.' + f'Robot [{self.name}] received new navigation command ' + f'while disconnected, ignoring command and requesting ' + f'replan after robot is re-connected.' ) self.replan_after_disconnect = True return @@ -635,7 +638,8 @@ def request_dock( def stop(self, activity): # If robot is disconnected and we are unable to retrieve any robot - # status, we'll ignore all commands from RMF until robot is re-connected + # status, we'll ignore all commands from RMF until robot is + # re-connected with self.disconnect_mutex: if self.disconnect: self.node.get_logger().info( From 561044d22f4802b8ad9a280b1342ffbdb32e4bd5 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 2 Aug 2024 18:26:14 +0800 Subject: [PATCH 31/50] Cleanup and add license declaration Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/fleet_adapter_mir.py | 14 ++++++++++ .../fleet_adapter_mir/mir_api.py | 27 ++++++++++--------- .../fleet_adapter_mir/robot_adapter_mir.py | 20 +++++++++----- .../fleet_adapter_mir_actions/mir_action.py | 13 +++++++++ .../rmf_cart_delivery.py | 14 ++++++++++ .../rmf_cart_detection.py | 14 ++++++++++ .../dispatch_delivery.py | 2 +- 7 files changed, 85 insertions(+), 19 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py index 3c0cc7c..0c36e34 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py @@ -1,3 +1,17 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# 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. + import asyncio import sys import yaml diff --git a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py index 40dd5a9..a6544a9 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/mir_api.py +++ b/fleet_adapter_mir/fleet_adapter_mir/mir_api.py @@ -1,10 +1,23 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# 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. + from collections import namedtuple import copy import enum import math import requests import json -from icecream import ic from urllib.error import HTTPError from rmf_adapter.easy_full_control import RobotState @@ -66,7 +79,7 @@ def __init__(self, timeout=10.0, debug=False): self.prefix = prefix - self.debug = False + self.debug = debug self.headers = headers self.timeout = timeout self.connected = False @@ -385,16 +398,6 @@ def get_position_guid(self, name, map_id, location): # the MiR server has an acceptable position for us to use for # localization position = self.known_positions.get(name) - - def position_matches(pos): - if abs(pos['pos_x'] - location[0]) > 0.01: - return False - if abs(pos['pos_y'] - location[1]) > 0.01: - return False - if pos['map_id'] != map_id: - return False - return True - if position is None: # The position does not exist so we need to create a new one position_guid = self.positions_post(name, map_id, location) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index ac65831..3bc578e 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -1,13 +1,22 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# 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. + import math -import numpy as np -from icecream import ic -import enum -import copy import time from typing import Any from dataclasses import dataclass import json -import datetime from rmf_adapter.robot_update_handle import Tier import rmf_adapter.easy_full_control as rmf_easy import rclpy @@ -65,7 +74,6 @@ def __init__( name: str, rmf_config: rmf_easy.RobotConfiguration, mir_config: dict, - # pickup_config: dict, conversions: dict, rmf_missions: dict, fleet_handle, diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 658c4bd..179b236 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -1,3 +1,16 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# 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. import json from abc import ABC, abstractmethod diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index 194f3a0..7c50d20 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -1,3 +1,17 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# 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. + import math import enum import numpy as np diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py index f563155..fe25fb1 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py @@ -1,3 +1,17 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# 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. + import requests from urllib.error import HTTPError diff --git a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py index 15b3246..b1d453e 100644 --- a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py +++ b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright 2022 Open Source Robotics Foundation, Inc. +# Copyright 2024 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. From fc2e76993df4cffb2bc4b7e3555fc2aa2380750c Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 7 Aug 2024 11:13:15 +0800 Subject: [PATCH 32/50] Cleanup Signed-off-by: Xiyu Oh --- fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py index 0c36e34..58e4cfb 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py @@ -212,7 +212,7 @@ def main(argv=sys.argv): args_without_ros = rclpy.utilities.remove_ros_args(argv) parser = argparse.ArgumentParser( - prog="cgh_fleet_adapter_mir", + prog="fleet_adapter_mir", description="Configure and spin up fleet adapters for MiR 100 robots " "that interface between the " "MiR REST API, ROS2, and rmf_core!") From a153c8dc0fece9ac39249497a39d108f9642ea06 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 4 Oct 2024 08:32:24 +0000 Subject: [PATCH 33/50] Replace execution.identifier with activity Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 3bc578e..37edbda 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -456,16 +456,16 @@ def request_navigate( # Used for exiting while loop early in the event that for whatever # reason the robot starts performing a different mission, while the # original navigation mission is ongoing - navigation_mission_identifier = mission_handle.execution.identifier + navigation_mission_identifier = mission_handle.activity mission_queue_id = None count = 0 retry_count = 10 while count < retry_count and not mission_queue_id: if (navigation_mission_identifier != - mission_handle.execution.identifier): + mission_handle.activity): self.node.get_logger().info( f'[{self.name}] MissionHandle has changed to ' - f'{mission_handle.execution.identifier}, interrupting ' + f'{mission_handle.activity}, interrupting ' f'navigation mission {navigation_mission_identifier}. ' f'Stopping original navigation mission loop.') break @@ -496,12 +496,12 @@ def request_navigate( if (mission_queue_id is None and navigation_mission_identifier != - mission_handle.execution.identifier): + mission_handle.activity): self.node.get_logger().info( f'[{self.name}] Failed to request robot to move to ' f'destination. Navigation step ' f'{navigation_mission_identifier} interrupted by new mission ' - f'{mission_handle.execution.identifier}.' + f'{mission_handle.activity}.' ) return @@ -523,16 +523,16 @@ def request_go_to_known_position( # Used for exiting while loop early in the event that for whatever # reason the robot starts performing a different mission, while the # original navigation mission is ongoing - navigation_mission_identifier = mission_handle.execution.identifier + navigation_mission_identifier = mission_handle.activity mission_queue_id = None count = 0 retry_count = 10 while count < retry_count and not mission_queue_id: if (navigation_mission_identifier != - mission_handle.execution.identifier): + mission_handle.activity): self.node.get_logger().info( f'[{self.name}] MissionHandle has changed to ' - f'{mission_handle.execution.identifier}, interrupting ' + f'{mission_handle.activity}, interrupting ' f'navigation mission {navigation_mission_identifier}.' f' Stopping original navigation mission loop.') break @@ -555,12 +555,12 @@ def request_go_to_known_position( if (mission_queue_id is None and navigation_mission_identifier != - mission_handle.execution.identifier): + mission_handle.activity): self.node.get_logger().info( f'[{self.name}] Failed to request robot to move to known ' f'position. Navigation step {navigation_mission_identifier} ' f'interrupted by new mission ' - f'{mission_handle.execution.identifier}.' + f'{mission_handle.activity}.' ) return @@ -584,16 +584,16 @@ def request_dock( # Used for exiting while loop early in the event that for whatever # reason the robot starts performing a different mission, while the # original navigation mission is ongoing - docking_mission_identifier = mission_handle.execution.identifier + docking_mission_identifier = mission_handle.activity mission_queue_id = None count = 0 retry_count = 10 while count < retry_count and not mission_queue_id: if (docking_mission_identifier != - mission_handle.execution.identifier): + mission_handle.activity): self.node.get_logger().info( f'[{self.name}] MissionHandle has changed to ' - f'{mission_handle.execution.identifier}, interrupting ' + f'{mission_handle.activity}, interrupting ' f'navigation mission {docking_mission_identifier}. ' f'Stopping original docking mission loop.') break @@ -626,12 +626,12 @@ def request_dock( if (mission_queue_id is None and docking_mission_identifier != - mission_handle.execution.identifier): + mission_handle.activity): self.node.get_logger().info( f'[{self.name}] Failed to request robot to dock to ' f'destination. Docking step {docking_mission_identifier} ' f'interrupted by new mission ' - f'{mission_handle.execution.identifier}.' + f'{mission_handle.activity}.' ) return @@ -666,7 +666,7 @@ def stop(self, activity): f'ignoring stop issued by RMF') return if (mission.execution is not None and - activity.is_same(mission.execution.identifier)): + activity.is_same(mission.activity)): self.node.get_logger().info( f'[{self.name}] Stop requested from RMF!') self.request_stop(mission) From cecab1e49d24a92baa657b85341dc7f73a64839b Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 4 Oct 2024 08:47:54 +0000 Subject: [PATCH 34/50] Create nav_issue_ticket if no mission_queue_id received for missions Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 67 +++++++++++++------ 1 file changed, 46 insertions(+), 21 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 37edbda..ea5ccb3 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -292,22 +292,12 @@ def update_mission_status( self.node.get_logger().info(f'Issue ticket is resolved!') self.replan_counts = 0 elif mission_status['state'] == 'Aborted': - # The robot is unable to perform the mission for some reason, so we - # raise an issue and re-attempt the mission. - tier = Tier.Error - category = mission_status['state'] - detail = { - 'mission_queue_id': mission.mission_queue_id, - 'message': 'Mission has been aborted.' - } - # Save the issue ticket somewhere so that we can resolve it later - self.nav_issue_ticket = \ - self.update_handle.more().create_issue(tier, category, detail) - self.node.get_logger().info( - f'Created [{category}] issue ticket for mission queue ID ' - f'[{mission.mission_queue_id}]') - - # After issuing a ticket, let's request a replan + # Create navigation issue ticket and replan + self.nav_issue_ticket = self.create_nav_issue_ticket( + mission_status['state'], + 'Mission has been aborted.', + mission.mission_queue_id + ) mission.done = True self.update_handle.more().replan() # We keep track of the number of times we are replanning for the @@ -509,7 +499,12 @@ def request_navigate( self.node.get_logger().info( f'[{self.name}] Failed to request robot to move to ' f'destination. Maximum request navigate retries exceeded!') - mission_handle.execution.finished() + # Create navigation issue ticket and replan + self.nav_issue_ticket = self.create_nav_issue_ticket( + 'Unable to post mission', + 'No mission queue ID received for request_navigate' + ) + self.update_handle.more().replan() return mission_handle.set_mission_queue_id(mission_queue_id) @@ -568,7 +563,12 @@ def request_go_to_known_position( self.node.get_logger().info( f'[{self.name}] Failed to request robot to move to known ' f'position. Maximum request navigate retries exceeded!') - mission_handle.execution.finished() + # Create navigation issue ticket and replan + self.nav_issue_ticket = self.create_nav_issue_ticket( + 'Unable to post mission', + 'No mission queue ID received for request_go_to_known_position' + ) + self.update_handle.more().replan() return mission_handle.set_mission_queue_id(mission_queue_id) @@ -639,7 +639,12 @@ def request_dock( self.node.get_logger().info( f'[{self.name}] Failed to request robot to dock to ' f'destination. Maximum request navigate retries exceeded!') - mission_handle.execution.finished() + # Create navigation issue ticket and replan + self.nav_issue_ticket = self.create_nav_issue_ticket( + 'Unable to post mission', + 'No mission queue ID received for request_dock' + ) + self.update_handle.more().replan() return mission_handle.set_mission_queue_id(mission_queue_id) @@ -725,8 +730,12 @@ def request_localize(self, estimate, mission: MissionHandle): f'[{self.name}] Failed to localize on map {estimate.map}. ' f'Maximum localize retries exceeded!' ) - mission.execution.finished() - mission_queue_id = None + # Create navigation issue ticket and replan + self.nav_issue_ticket = self.create_nav_issue_ticket( + 'Unable to post mission', + 'No mission queue ID received for request_localize' + ) + self.update_handle.more().replan() return mission.set_mission_queue_id(mission_queue_id) @@ -765,3 +774,19 @@ def dist(self, A, B): assert (len(A) > 1) assert (len(B) > 1) return math.sqrt((A[0] - B[0])**2 + (A[1] - B[1])**2) + + def create_nav_issue_ticket(self, category, msg, mission_queue_id=None): + # The robot is unable to perform the mission for some reason, so we + # raise an issue and re-attempt the mission. + tier = Tier.Error + detail = { + 'mission_queue_id': mission_queue_id, + 'message': msg + } + # Save the issue ticket somewhere so that we can resolve it later + nav_issue_ticket = \ + self.update_handle.more().create_issue(tier, category, detail) + self.node.get_logger().info( + f'Created [{category}] issue ticket for robot [{self.name}] with ' + f'mission queue ID [{mission_queue_id}]') + return nav_issue_ticket From 3cf3740c7e73342d070fa4d98fac55e13c8f40f1 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Mon, 7 Oct 2024 02:51:48 +0000 Subject: [PATCH 35/50] Import modules at init and store action factories with plugin configs Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 35 +++++++++++++------ .../fleet_adapter_mir_actions/mir_action.py | 19 ++++++---- .../rmf_cart_delivery.py | 10 ++++-- 3 files changed, 45 insertions(+), 19 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index ea5ccb3..6a737b3 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -142,8 +142,27 @@ def __init__( # Track the current ongoing action self.current_action = None - # Store all the configured plugin action configs - self.plugin_config = plugin_config + # Import and store plugin actions and action factories + self.action_factories = {} + for plugin_name, action_config in plugin_config.items(): + try: + module = action_config['module'] + plugin = importlib.import_module(module) + action_factory = plugin.ActionFactory(action_config) + self.action_factories[plugin_name] = action_factory + except KeyError: + self.node.get_logger().info( + f'Unable to create ActionFactory for {plugin_name}! ' + f'Configured plugin config is invalid. ' + f'Robot [{self.name}] will not be able to perform ' + f'actions associated with this plugin.' + ) + except ImportError: + self.node.get_logger().info( + f'Unable to import module for {plugin_name}! ' + f'Robot [{self.name}] will not be able to perform ' + f'actions associated with this plugin.' + ) @property def activity(self): @@ -749,16 +768,12 @@ def perform_action(self, category, description, execution): execution.finished() return - for plugin_name, config in self.plugin_config.items(): - actions = config['actions'] - if category in actions: - # Import relevant plugin - module = config['module'] - plugin = importlib.import_module(module) + for _, action_factory in self.action_factories.items(): + if category in action_factory.actions: # Create the relevant MirAction - action_obj = plugin.ActionFactory().make_action( + action_obj = action_factory.make_action( self.node, self.name, self.api, self.update_handle, - self.fleet_config, config) + self.fleet_config) # Begin performing the plugin action action_obj.perform_action(category, description, execution) # Keep track of the current action diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 179b236..ad765cf 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -70,8 +70,10 @@ def perform_action(self, # To be populated in the plugins ... - # This will be called on every update to check on the action's - # current state + ''' + This method is called on every update by the robot adapter to monitor the + progress and completion of the action + ''' @abstractmethod def update_action(self): # To be populated in the plugins @@ -100,15 +102,20 @@ def _on_cancel(result: bool): class MirActionFactory(ABC): - def __init__(self): - pass + def __init__(self, action_config): + self.config = action_config + self.actions = action_config['actions'] + ''' + This method creates a MirAction object for the robot adapter to begin and + interact with an action. + ''' + @abstractmethod def make_action(self, node: Node, name: str, mir_api: MirAPI, update_handle, # rmf_fleet_adapter.RobotUpdateHandle - fleet_config: rmf_easy.FleetConfiguration, - action_config) -> MirAction: + fleet_config: rmf_easy.FleetConfiguration) -> MirAction: # To be populated in the plugins pass diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index 7c50d20..6d1fef1 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -59,15 +59,19 @@ class Dropoff: class ActionFactory(MirActionFactory): + def __init__(self, action_config): + MirActionFactory.__init__(action_config) + # TODO(@xiyuoh) raise KeyError if config file is invalid + pass + def make_action(self, node, name, mir_api, update_handle, - fleet_config, - action_config) -> MirAction: + fleet_config) -> MirAction: return CartDelivery( - node, name, mir_api, update_handle, fleet_config, action_config) + node, name, mir_api, update_handle, fleet_config, self.config) class CartDelivery(MirAction): From 55b8877af7b06f678b27875cb978bba622e76357 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Mon, 7 Oct 2024 09:05:24 +0000 Subject: [PATCH 36/50] Raise errors if action config is missing required keys Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir_actions/mir_action.py | 4 ++ .../rmf_cart_delivery.py | 50 ++++++++++++++++--- 2 files changed, 48 insertions(+), 6 deletions(-) diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index ad765cf..1045053 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -104,6 +104,10 @@ def _on_cancel(result: bool): class MirActionFactory(ABC): def __init__(self, action_config): self.config = action_config + if 'actions' not in action_config: + raise KeyError( + f'List of supported actions is not provided in the action ' + f'config! Unable to instantiate an ActionFactory.') self.actions = action_config['actions'] ''' diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index 6d1fef1..eb1cf53 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -61,8 +61,49 @@ class Dropoff: class ActionFactory(MirActionFactory): def __init__(self, action_config): MirActionFactory.__init__(action_config) - # TODO(@xiyuoh) raise KeyError if config file is invalid - pass + # Raise error if config file is invalid + # Note(@xiyuoh) Using if-else to check for valid keys in the action + # config is not the most scalable. Consider other ways to + # do this. + if 'cart_detection_module' not in action_config: + raise KeyError( + f'CartDelivery MirAction requires a cart detection module, but ' + f'path to [cart_detection_module] is not provided in the ' + f'action config! Unable to instantiate an ActionFactory.') + elif 'marker_types' not in action_config: + raise KeyError( + f'CartDelivery MirAction requires configured marker types, but ' + f'[marker_types] not provided in the action config! ' + f'Unable to instantiate an ActionFactory.') + elif 'missions' not in action_config: + raise KeyError( + f'CartDelivery MirAction requires configured missions, but ' + f'[missions] not provided in the action config! ' + f'Unable to instantiate an ActionFactory.') + # Check if the required mission names have been configured + # TODO(@xiyuoh) Remove requirement for configuring dock_to_cart and + # exit lot as they will be created on the robot by the + # fleet adapter, hence will not need to be configured + elif 'dock_to_cart' not in action_config['missions']: + raise KeyError( + f'CartDelivery MirAction requires the configured MiR mission ' + f'name for [dock_to_cart], but it is not provided in the action ' + f'config! Unable to instantiate an ActionFactory.') + elif 'pickup' not in action_config['missions']: + raise KeyError( + f'CartDelivery MirAction requires the configured MiR mission ' + f'name for [pickup], but it is not provided in the action ' + f'config! Unable to instantiate an ActionFactory.') + elif 'dropoff' not in action_config['missions']: + raise KeyError( + f'CartDelivery MirAction requires the configured MiR mission ' + f'name for [dropoff], but it is not provided in the action ' + f'config! Unable to instantiate an ActionFactory.') + elif 'exit_lot' not in action_config['missions']: + raise KeyError( + f'CartDelivery MirAction requires the configured MiR mission ' + f'name for [exit_lot], but it is not provided in the action ' + f'config! Unable to instantiate an ActionFactory.') def make_action(self, node, @@ -110,10 +151,7 @@ def __init__( self.action_config['marker_types']['cart']) # Import CartDetection module if provided - detection_module = self.action_config.get('cart_detection_module') - assert (detection_module is not None, - 'CartDetection module is required for CartDelivery plugin, ' + - 'but it is not found!') + detection_module = self.action_config['cart_detection_module'] detection_plugin = importlib.import_module(detection_module) self.cart_detection = \ detection_plugin.CartDetection(mir_api, action_config) From 0f0e835ff0a889114968f70cefd647a906c9a820 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Mon, 7 Oct 2024 12:14:33 +0000 Subject: [PATCH 37/50] Add in ActionContext and refactor perform action calls Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 28 +- .../fleet_adapter_mir_actions/mir_action.py | 74 +-- .../rmf_cart_delivery.py | 451 ++++++------------ .../rmf_cart_detection.py | 40 +- 4 files changed, 225 insertions(+), 368 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 6a737b3..ead2939 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -68,6 +68,22 @@ def activity(self): return None +class ActionContext: + def __init__(self, + node: Node, + name: str, + mir_api: MirAPI, + update_handle, # rmf_fleet_adapter.RobotUpdateHandle + fleet_config: rmf_easy.FleetConfiguration, + execution): + self.node = node + self.name = name + self.api = mir_api + self.update_handle = update_handle + self.fleet_config = fleet_config + self.execution = execution + + class RobotAdapterMiR: def __init__( self, @@ -770,13 +786,13 @@ def perform_action(self, category, description, execution): for _, action_factory in self.action_factories.items(): if category in action_factory.actions: - # Create the relevant MirAction - action_obj = action_factory.make_action( + action_context = ActionContext( self.node, self.name, self.api, self.update_handle, - self.fleet_config) - # Begin performing the plugin action - action_obj.perform_action(category, description, execution) - # Keep track of the current action + self.fleet_config, execution + ) + action_obj = action_factory.perform_action( + category, description, action_context + ) self.current_action = action_obj return diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 1045053..caceba1 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -18,28 +18,15 @@ import rclpy import rclpy.node as Node import rmf_adapter.easy_full_control as rmf_easy +from fleet_adapter_mir.robot_adapter_mir import ActionContext from fleet_adapter_mir.mir_api import MirAPI class MirAction(ABC): - def __init__( - self, - node, - name, - mir_api: MirAPI, - update_handle, - fleet_config, - action_config: dict | None, - ): - self.node = node - self.name = name - self.api = mir_api - self.update_handle = update_handle - self.fleet_config = fleet_config - self.action_config = action_config - self.actions = self.action_config.get('actions') + def __init__(self, context: ActionContext): + self.context = context - missions_json = self.action_config.get('missions_json') + missions_json = context.action_config.get('missions_json') if missions_json: with open(missions_json, 'r') as g: action_missions = json.load(g) @@ -47,29 +34,20 @@ def __init__( # Check if these missions are already created on the robot missions_created = True for mission_name in action_missions.keys(): - if mission_name not in self.api.known_missions: + if mission_name not in context.api.known_missions: missions_created = False break if missions_created: return # Create these missions on the robot - self.api.create_missions(action_missions) + context.api.create_missions(action_missions) # Update mission actions stored in MirAPI - for mission, mission_data in self.api.known_missions.items(): - self.api.mission_actions[mission] = \ - self.api.missions_mission_id_actions_get( + for mission, mission_data in context.api.known_missions.items(): + context.api.mission_actions[mission] = \ + context.api.missions_mission_id_actions_get( mission_data['guid']) - # This will be called whenever an action has begun - @abstractmethod - def perform_action(self, - category: str, - description: dict, - execution): # rmf_fleet_adapter.ActionExecution - # To be populated in the plugins - ... - ''' This method is called on every update by the robot adapter to monitor the progress and completion of the action @@ -83,31 +61,31 @@ def cancel_current_task(self, cancel_success: Callable[[], None], cancel_fail: Callable[[], None], label: str = ''): - current_task_id = self.update_handle.more().current_task_id() - self.node.get_logger().info( - f'[{self.name}] Cancel task requested for [{current_task_id}]') + current_task_id = self.context.update_handle.more().current_task_id() + self.context.node.get_logger().info( + f'[{self.context.name}] Cancel task requested for [{current_task_id}]') def _on_cancel(result: bool): if result: - self.node.get_logger().info( - f'[{self.name}] Found task [{current_task_id}], ' + self.context.node.get_logger().info( + f'[{self.context.name}] Found task [{current_task_id}], ' f'cancelling...') cancel_success() else: - self.node.get_logger().info( - f'[{self.name}] Failed to cancel task [{current_task_id}]') + self.context.node.get_logger().info( + f'[{self.context.name}] Failed to cancel task [{current_task_id}]') cancel_fail() - self.update_handle.more().cancel_task( + self.context.update_handle.more().cancel_task( current_task_id, [label], lambda result: _on_cancel(result)) class MirActionFactory(ABC): - def __init__(self, action_config): - self.config = action_config + def __init__(self, action_config: dict): if 'actions' not in action_config: raise KeyError( f'List of supported actions is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') + self.action_config = action_config self.actions = action_config['actions'] ''' @@ -115,11 +93,11 @@ def __init__(self, action_config): interact with an action. ''' @abstractmethod - def make_action(self, - node: Node, - name: str, - mir_api: MirAPI, - update_handle, # rmf_fleet_adapter.RobotUpdateHandle - fleet_config: rmf_easy.FleetConfiguration) -> MirAction: + def perform_action( + self, + category: str, + description: dict, + context: ActionContext, + ) -> MirAction: # To be populated in the plugins - pass + ... diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index eb1cf53..f880450 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -12,16 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -import math import enum -import numpy as np -from typing import Any from dataclasses import dataclass import importlib -import requests -from urllib.error import HTTPError from fleet_adapter_mir_actions.mir_action import MirAction, MirActionFactory -from fleet_adapter_mir.mir_api import MirAPI, MirStatus, MiRStateCode +from fleet_adapter_mir.robot_adapter_mir import ActionContext +from fleet_adapter_mir.mir_api import MiRStateCode class PickupState(enum.IntEnum): @@ -47,19 +43,17 @@ class Pickup: state: PickupState pickup_lots: list[str] # contains list of pickup lots cart_id: str - execution: Any mission: Mission latching: bool @dataclass class Dropoff: - execution: Any mission: Mission class ActionFactory(MirActionFactory): - def __init__(self, action_config): + def __init__(self, action_config: dict): MirActionFactory.__init__(action_config) # Raise error if config file is invalid # Note(@xiyuoh) Using if-else to check for valid keys in the action @@ -105,168 +99,134 @@ def __init__(self, action_config): f'name for [exit_lot], but it is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') - def make_action(self, - node, - name, - mir_api, - update_handle, - fleet_config) -> MirAction: - return CartDelivery( - node, name, mir_api, update_handle, fleet_config, self.config) - - -class CartDelivery(MirAction): - def __init__( - self, - node, - name, - mir_api, - update_handle, - fleet_config, - action_config): - MirAction.__init__( - self, node, name, mir_api, update_handle, fleet_config, - action_config) - - self.pickup: Pickup = None - self.dropoff: Dropoff = None - self.search_timeout = \ - self.action_config.get('search_timeout', 60) # seconds + def perform_action( + self, + category: str, + description: dict, + context: ActionContext, + ) -> MirAction: + match category: + case 'delivery_pickup': + return CartPickup(description, context) + case 'delivery_dropoff': + return CartDropoff(description, context) + - # Store the mission names to be used during the action +class CartPickup(MirAction): + def __init__(self, description: dict, context: ActionContext): + MirAction.__init__(self, context) + + # Import CartDetection module + # TODO(@xiyuoh) Import this only once during ActionFactory instantiation + detection_module = context.action_config['cart_detection_module'] + detection_plugin = importlib.import_module(detection_module) + self.cart_detection = detection_plugin.CartDetection(context) + + # Mission names to be used during pickup self.dock_to_cart_mission = \ - self.action_config['missions']['dock_to_cart'] + self.context.action_config['missions']['dock_to_cart'] self.pickup_mission = \ - self.action_config['missions']['pickup'] - self.dropoff_mission = \ - self.action_config['missions']['dropoff'] + self.context.action_config['missions']['pickup'] self.exit_mission = \ - self.action_config['missions']['exit_lot'] - self.footprint_mission = \ - self.action_config['missions'].get('update_footprint') # Optional + self.context.action_config['missions']['exit_lot'] - # Initialize cart marker type + self.search_timeout = \ + self.context.action_config.get('search_timeout', 60) # seconds self.cart_marker_type_guid = \ - self.api.docking_offsets_guid_get( - self.action_config['marker_types']['cart']) + self.context.api.docking_offsets_guid_get( + self.context.action_config['marker_types']['cart']) - # Import CartDetection module if provided - detection_module = self.action_config['cart_detection_module'] - detection_plugin = importlib.import_module(detection_module) - self.cart_detection = \ - detection_plugin.CartDetection(mir_api, action_config) - - def perform_action(self, category, description, execution): - # Check that the perform action category matches - match category: - case 'delivery_pickup': - # Check if the robot's latch is currently open - if self.is_latch_open(): - # Latch is open, unable to perform pickup - self.node.get_logger().info( - f'Robot [{self.name}] latch is open, unable to ' - f'perform pickup, cancelling task...') - self.cancel_task() - return + # Begin action + # Check if the robot's latch is currently open + if self.is_latch_open(): + # Latch is open, unable to perform pickup + self.context.node.get_logger().info( + f'Robot [{self.context.name}] latch is open, unable to ' + f'perform pickup, cancelling task...') + self.cancel_task() + return - self.node.get_logger().info( - f'New pickup requested for robot [{self.name}]') - self.pickup = Pickup( - state=PickupState.AT_PICKUP, - pickup_lots=[description.get('pickup_lot')], - cart_id=description.get('cart_id'), - execution=execution, - mission=None, - latching=False - ) - case 'delivery_dropoff': - self.node.get_logger().info( - f'[{self.name}] Received dropoff request!') - self.dropoff = Dropoff( - execution=execution, - mission=None - ) - case _: - self.node.get_logger().info( - f'Invalid perform action [{category}] passed to ' - f'CartDelivery, ending action.') - execution.finished() + self.context.node.get_logger().info( + f'New pickup requested for robot [{self.context.name}]') + self.pickup = Pickup( + state=PickupState.AT_PICKUP, + pickup_lots=[description.get('pickup_lot')], + cart_id=description.get('cart_id'), + mission=None, + latching=False + ) def update_action(self): - # There should not be a pickup and dropoff being performed - # simultaneously at any given time, since actions are dispatched - # sequentially - if self.pickup: - return self.update_pickup(self.pickup) - elif self.dropoff: - return self.update_dropoff(self.dropoff) - + if self.update_pickup(self.pickup): + if self.context.execution is not None: + self.context.execution.finished() + return True return False def update_pickup(self, pickup: Pickup): # If this is a PerformAction pickup, check that the action is underway # and valid - if pickup.execution is not None and not pickup.execution.okay(): - self.node.get_logger().info( + if self.context.execution is not None and not self.context.execution.okay(): + self.context.node.get_logger().info( f'[delivery_pickup] action is killed/canceled.') pickup.state = PickupState.TASK_CANCELLED # Start state machine check - now = self.node.get_clock().now().nanoseconds / 1e9 - self.node.get_logger().debug(f'PickupState: {pickup.state.name}') + now = self.context.node.get_clock().now().nanoseconds / 1e9 + self.context.node.get_logger().debug(f'PickupState: {pickup.state.name}') match pickup.state: case PickupState.AT_PICKUP: # Send rmf_dock_to_cart mission assert self.dock_to_cart_mission is not None current_wp_name = pickup.pickup_lots[0] - mir_pos = self.api.known_positions.get(current_wp_name) + mir_pos = self.context.api.known_positions.get(current_wp_name) if not mir_pos: - self.node.get_logger().info( + self.context.node.get_logger().info( f'No shelf position [{mir_pos}] found on robot ' - f'[{self.name}], cancelling task') + f'[{self.context.name}], cancelling task') self.cancel_task() pickup.state = PickupState.TASK_CANCELLED return cart_marker_guid = mir_pos['guid'] - cart_marker_param = self.api.get_mission_params_with_value( + cart_marker_param = self.context.api.get_mission_params_with_value( self.dock_to_cart_mission, 'docking', 'cart_marker', cart_marker_guid) - marker_type_param = self.api.get_mission_params_with_value( + marker_type_param = self.context.api.get_mission_params_with_value( self.dock_to_cart_mission, 'docking', 'cart_marker_type', self.cart_marker_type_guid) mission_params = cart_marker_param + marker_type_param - mission_queue_id = self.api.queue_mission_by_name( + mission_queue_id = self.context.api.queue_mission_by_name( self.dock_to_cart_mission, mission_params) if not mission_queue_id: error_str = \ f'Mission {self.dock_to_cart_mission} not ' + \ f'supported, ignoring' - self.node.get_logger().error(error_str) + self.context.node.get_logger().error(error_str) return pickup.mission = Mission(mission_queue_id, now) pickup.state = PickupState.DOCK_REQUESTED - self.node.get_logger().info( - f'[{self.name}] Dock to cart mission requested with ' + self.context.node.get_logger().info( + f'[{self.context.name}] Dock to cart mission requested with ' f'mission queue id {mission_queue_id}') case PickupState.DOCK_REQUESTED: # Make sure that there is an rmf_dock_to_cart mission issued if not pickup.mission: - self.node.get_logger().info( - f'Robot [{self.name}] is indicated to be at the ' + self.context.node.get_logger().info( + f'Robot [{self.context.name}] is indicated to be at the ' f'DOCK_REQUESTED state but no mission queue ID stored ' f'for this docking mission! Returning to AT_PICKUP ' f'state.') pickup.state = PickupState.AT_PICKUP return # Mission completed, move onto the next state - if self.api.mission_completed(pickup.mission.queue_id): - self.node.get_logger().info( - f'Robot [{self.name}] dock to cart mission ' + if self.context.api.mission_completed(pickup.mission.queue_id): + self.context.node.get_logger().info( + f'Robot [{self.context.name}] dock to cart mission ' f'{pickup.mission.queue_id} completed or timed out.') pickup.mission = None pickup.state = PickupState.DOCK_COMPLETED @@ -276,17 +236,17 @@ def update_pickup(self, pickup: Pickup): seconds_passed = now - pickup.mission.start_time # Publish update every 10 seconds just to monitor if round(seconds_passed) % 10 == 0: - self.node.get_logger().info( + self.context.node.get_logger().info( f'{round(seconds_passed)} seconds have passed since ' f'pickup mission requested.') # Mission timeout, cart not found if seconds_passed > self.search_timeout: # Delete mission from mir first - self.api.mission_queue_id_delete(pickup.mission.queue_id) + self.context.api.mission_queue_id_delete(pickup.mission.queue_id) # Regardless of whether the robot completed docking # properly, we move to the next state to check - self.node.get_logger().info( - f'Robot [{self.name}] dock to cart mission ' + self.context.node.get_logger().info( + f'Robot [{self.context.name}] dock to cart mission ' f'{pickup.mission.queue_id} timed out! Configured ' f'search timeout is {self.search_timeout} seconds.') pickup.mission = None @@ -299,25 +259,25 @@ def update_pickup(self, pickup: Pickup): if cart_check: # If cart is correct, send pickup mission assert self.pickup_mission is not None - mission_queue_id = self.api.queue_mission_by_name( + mission_queue_id = self.context.api.queue_mission_by_name( self.pickup_mission) if not mission_queue_id: error_str = \ f'Mission {self.pickup_mission} not supported,' + \ f' ignoring' - self.node.get_logger().error(error_str) + self.context.node.get_logger().error(error_str) return pickup.mission = Mission(mission_queue_id, now) pickup.latching = True - self.node.get_logger().info( - f'Robot [{self.name}] found the correct cart, pickup ' + self.context.node.get_logger().info( + f'Robot [{self.context.name}] found the correct cart, pickup ' f'mission requested with mission queue id ' f'{mission_queue_id}') pickup.state = PickupState.PICKUP_REQUESTED elif cart_check is None: # If cart is missing, cancel this task - self.node.get_logger().info( - f'Robot [{self.name}] was unable to dock under any ' + self.context.node.get_logger().info( + f'Robot [{self.context.name}] was unable to dock under any ' f'carts, please check that cart is present. ' f'Cancelling task.') self.cancel_task() @@ -325,8 +285,8 @@ def update_pickup(self, pickup: Pickup): else: # If cart is wrong, cancel this task also but after we # exit from the lot - self.node.get_logger().info( - f'Robot [{self.name}] found the wrong cart, exiting ' + self.context.node.get_logger().info( + f'Robot [{self.context.name}] found the wrong cart, exiting ' f'lot and cancelling task.') self.exit_lot() self.cancel_task() @@ -334,221 +294,128 @@ def update_pickup(self, pickup: Pickup): case PickupState.PICKUP_REQUESTED: # Pickup mission completed - if self.api.mission_completed(pickup.mission.queue_id): + if self.context.api.mission_completed(pickup.mission.queue_id): pickup.state = PickupState.PICKUP_SUCCESS pickup.mission = None pickup.latching = False case PickupState.PICKUP_SUCCESS: # Correct ID, we can end the delivery now - self.node.get_logger().info( - f'Robot [{self.name}] successfully received cart, ' + self.context.node.get_logger().info( + f'Robot [{self.context.name}] successfully received cart, ' f'exiting lot with cart and ending mission') self.exit_lot() - if pickup.execution is not None: - pickup.execution.finished() return True case PickupState.TASK_CANCELLED: - self.node.get_logger().info( - f'Robot [{self.name}] is in pickup cancelled state.') + self.context.node.get_logger().info( + f'Robot [{self.context.name}] is in pickup cancelled state.') # If some MiR mission is in progress, we abort it unless # it is latching - if pickup.mission and not self.api.mission_completed( + if pickup.mission and not self.context.api.mission_completed( pickup.mission.queue_id): if pickup.latching: - self.node.get_logger().info( - f'Robot [{self.name}] is performing latching, ' + self.context.node.get_logger().info( + f'Robot [{self.context.name}] is performing latching, ' f'cancelling task after this action is complete.') return False - self.api.mission_queue_id_delete(pickup.mission.queue_id) + self.context.api.mission_queue_id_delete(pickup.mission.queue_id) pickup.mission = None # Clear any errors - self.api.clear_error() - self.api.status_put(state_id=MiRStateCode.READY) - self.release_cart() - # Mark pickup session as completed + self.context.api.clear_error() + self.context.api.status_put(state_id=MiRStateCode.READY) + # NOTE(@xiyuoh) If this task is cancelled in the middle of a + # cart pickup (latching), the users should implement a + # cancellation behavior in the task JSON to ensure that this + # robot can continue with its next task without having a cart + # latched on. return True return False + def is_latch_open(self): + return self.cart_detection.is_latch_open() + + def is_correct_cart(self, cart_id: str): + return self.cart_detection.is_correct_cart(cart_id) + + def is_under_cart(self): + return self.cart_detection.is_under_cart() + + def exit_lot(self): + if not self.context.api.connected: + return None + # Set footprint to robot footprint before exiting lot + return self.context.api.queue_mission_by_name(self.exit_mission) + + def cancel_task(self, label: str = ''): + def _cancel_success(): + if self.pickup: + self.pickup.state = PickupState.TASK_CANCELLED + def _cancel_fail(): + pass + self.cancel_current_task(_cancel_success, _cancel_fail, label) + + +class CartDropoff(MirAction): + def __init__(self, description: dict, context: ActionContext): + MirAction.__init__(self, context) + + self.dropoff: Dropoff = None + self.dropoff_mission = \ + self.context.action_config['missions']['dropoff'] + + def update_action(self): + if self.update_dropoff(self.dropoff): + if self.context.execution is not None: + self.context.execution.finished() + return True + return False + def update_dropoff(self, dropoff: Dropoff): # Check if action is underway or cancelled - if dropoff.execution is not None and not dropoff.execution.okay(): - self.node.get_logger().info(f'Dropoff action is killed/canceled') + if self.context.execution is not None and not self.context.execution.okay(): + self.context.node.get_logger().info( + f'[delivery_dropoff] action is killed/canceled') # If cart release is in progress, we let it finish first - if dropoff.mission and not self.api.mission_completed( + if dropoff.mission and not self.context.api.mission_completed( dropoff.mission.queue_id): return False # Task is cancelled and cart is done dropping off/mission is not # yet queued anyway, we can mark it as completed at this point - self.api.clear_error() - self.api.status_put(state_id=MiRStateCode.READY) + self.context.api.clear_error() + self.context.api.status_put(state_id=MiRStateCode.READY) # Mark dropoff session as completed return True # No mission queued yet if dropoff.mission is None: assert self.dropoff_mission is not None - mission_queue_id = self.api.queue_mission_by_name( + mission_queue_id = self.context.api.queue_mission_by_name( self.dropoff_mission) if not mission_queue_id: error_str = \ f'Mission {self.dropoff_mission} not supported, ignoring' - self.node.get_logger().error(error_str) + self.context.node.get_logger().error(error_str) return True - self.node.get_logger().info( + self.context.node.get_logger().info( f'Mission {self.dropoff_mission} added to queue for robot ' - f'[{self.name}].') - now = self.node.get_clock().now().nanoseconds / 1e9 - dropoff.mission = Mission(mission_queue_id, - now) - self.node.get_logger().info( - f'[{self.name}] Dropoff mission requested with mission queue ' + f'[{self.context.name}].') + now = self.context.node.get_clock().now().nanoseconds / 1e9 + dropoff.mission = Mission(mission_queue_id, now) + self.context.node.get_logger().info( + f'[{self.context.name}] Dropoff mission requested with mission queue ' f'id {mission_queue_id}') # Mission queued, check completion else: - if self.api.mission_completed(dropoff.mission.queue_id): - self.node.get_logger().info( - f'[{self.name}] Dropoff mission completed!') - # Update robot footprint - if dropoff.execution is not None: - dropoff.execution.finished() + if self.context.api.mission_completed(dropoff.mission.queue_id): + self.context.node.get_logger().info( + f'[{self.context.name}] Dropoff mission completed!') return True else: - self.node.get_logger().info( - f'[{self.name}] Dropoff mission in progress...') + self.context.node.get_logger().info( + f'[{self.context.name}] Dropoff mission in progress...') return - - def cancel_task(self, label: str = ''): - def _cancel_success(): - if self.pickup: - self.pickup.state = PickupState.TASK_CANCELLED - - def _cancel_fail(): - pass - self.cancel_current_task(_cancel_success, _cancel_fail, label) - - def is_latch_open(self): - return self.cart_detection.is_latch_open() - - def is_under_cart(self): - return self.cart_detection.is_under_cart() - - def is_correct_cart(self, cart_id: str): - return self.cart_detection.is_correct_cart(cart_id) - - def exit_lot(self): - if not self.api.connected: - return None - # Set footprint to robot footprint before exiting lot - return self.api.queue_mission_by_name(self.exit_mission) - - def release_cart(self): - # If robot latch is open, close it - if self.is_latch_open(): - self.node.get_logger().info( - f'Robot [{self.name}] detected to have latch open, dropping ' - f'off cart...') - self.dropoff = Dropoff( - execution=None, - mission_queue_id=None) - # Dropoff mission will take care of the lot exit, - # so we can return here - return - # If robot is under a cart, exit lot - if self.is_under_cart(): - self.node.get_logger().info( - f'Cart detected above robot [{self.name}] during a release ' - f'call, exiting lot...') - self.exit_lot() - - # Optional function for updating robot footprint by providing - # footprint guid - def update_footprint(self, ft_guid: str): - if not self.footprint_mission: - return - ft_params = self.api.get_mission_params_with_value( - self.footprint_mission, 'set_footprint', 'footprint', ft_guid) - return self.api.queue_mission_by_name( - self.footprint_mission, ft_params) - - def retrieve_mir_coordinates(self, waypoint_name: str): - transform = self.fleet_config.transformations_to_robot_coordinates - transform_current_map = transform.get(self.current_map) - rmf_pose = \ - self.fleet_config.graph.find_waypoint(waypoint_name).location - new_rmf_pose = np.array([rmf_pose[0], rmf_pose[1], 0.0]) - mir_pose = transform_current_map.apply(new_rmf_pose) - return mir_pose - - def dist(self, A, B): - assert (len(A) > 1) - assert (len(B) > 1) - return math.sqrt((A[0] - B[0])**2 + (A[1] - B[1])**2) - - # -------------------------------------------------------------------------- - # MIR API FOR CART RELATED MISSIONS - # -------------------------------------------------------------------------- - def register_get(self, register: int): - if not self.api.connected: - return None - try: - response = requests.get( - self.api.prefix + f'registers/{register}', - headers=self.api.headers, - timeout=self.api.timeout) - if self.api.debug: - print(f"Response: {response.headers}") - # Response value is string, return integer of value - return int(response.json().get('value', 0)) - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - return None - except Exception as err: - print(f"Other error: {err}") - return None - - def io_module_guid_status_get(self, io_guid: str): - if not self.api.connected: - return None - if io_guid is None: - return None - try: - response = requests.get( - self.api.prefix + f'io_modules/{io_guid}/status', - headers=self.api.headers, - timeout=self.api.timeout) - if self.api.debug: - print(f"Response: {response.headers}") - if 'input_state' not in response.json(): - return None - return response.json()['input_state'] - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - return None - except Exception as err: - print(f"Other error: {err}") - return None - - def io_modules_get(self): - if not self.api.connected: - return None - try: - response = requests.get( - self.api.prefix + f'io_modules', - headers=self.api.headers, - timeout=self.api.timeout) - if self.api.debug: - print(f"Response: {response.headers}") - # Response value is string, return integer of value - return response.json() - except HTTPError as http_err: - print(f"HTTP error: {http_err}") - return None - except Exception as err: - print(f"Other error: {err}") - return None diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py index fe25fb1..c6f5729 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py @@ -14,16 +14,12 @@ import requests from urllib.error import HTTPError +from fleet_adapter_mir.robot_adapter_mir import ActionContext class CartDetection: - def __init__( - self, - mir_api, - action_config - ): - self.api = mir_api - self.action_config = action_config + def __init__(self, context: ActionContext): + self.context = context def is_latch_open(self): ''' @@ -61,14 +57,14 @@ def is_correct_cart(self, cart_id: str): # -------------------------------------------------------------------------- def register_get(self, register: int): - if not self.api.connected: + if not self.context.api.connected: return None try: response = requests.get( - self.api.prefix + f'registers/{register}', - headers=self.api.headers, - timeout=self.api.timeout) - if self.api.debug: + self.context.api.prefix + f'registers/{register}', + headers=self.context.api.headers, + timeout=self.context.api.timeout) + if self.context.api.debug: print(f"Response: {response.headers}") # Response value is string, return integer of value return int(response.json().get('value', 0)) @@ -80,16 +76,16 @@ def register_get(self, register: int): return None def io_module_guid_status_get(self, io_guid: str): - if not self.api.connected: + if not self.context.api.connected: return None if io_guid is None: return None try: response = requests.get( - self.api.prefix + f'io_modules/{io_guid}/status', - headers=self.api.headers, - timeout=self.api.timeout) - if self.api.debug: + self.context.api.prefix + f'io_modules/{io_guid}/status', + headers=self.context.api.headers, + timeout=self.context.api.timeout) + if self.context.api.debug: print(f"Response: {response.headers}") if 'input_state' not in response.json(): return None @@ -102,14 +98,14 @@ def io_module_guid_status_get(self, io_guid: str): return None def io_modules_get(self): - if not self.api.connected: + if not self.context.api.connected: return None try: response = requests.get( - self.api.prefix + f'io_modules', - headers=self.api.headers, - timeout=self.api.timeout) - if self.api.debug: + self.context.api.prefix + f'io_modules', + headers=self.context.api.headers, + timeout=self.context.api.timeout) + if self.context.api.debug: print(f"Response: {response.headers}") # Response value is string, return integer of value return response.json() From 15ee84ab4de80a03411af3939d39cd8f3a471074 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 8 Oct 2024 05:03:54 +0000 Subject: [PATCH 38/50] Clean up details for pickup/dropoff actions and add cancellation behavior in dispatch_delivery task Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir_actions/mir_action.py | 3 +- .../rmf_cart_delivery.py | 168 +++++++++++------- .../rmf_cart_detection.py | 53 +++--- .../dispatch_delivery.py | 31 +++- 4 files changed, 158 insertions(+), 97 deletions(-) diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index caceba1..78b944f 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -50,7 +50,8 @@ def __init__(self, context: ActionContext): ''' This method is called on every update by the robot adapter to monitor the - progress and completion of the action + progress and completion of the action. + Return True if the action is completed, else False. ''' @abstractmethod def update_action(self): diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index f880450..6086bef 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -42,7 +42,7 @@ def __init__(self, queue_id: str, start_time: float): class Pickup: state: PickupState pickup_lots: list[str] # contains list of pickup lots - cart_id: str + cart_id: str | None mission: Mission latching: bool @@ -56,7 +56,7 @@ class ActionFactory(MirActionFactory): def __init__(self, action_config: dict): MirActionFactory.__init__(action_config) # Raise error if config file is invalid - # Note(@xiyuoh) Using if-else to check for valid keys in the action + # NOTE(@xiyuoh) Using if-else to check for valid keys in the action # config is not the most scalable. Consider other ways to # do this. if 'cart_detection_module' not in action_config: @@ -78,22 +78,26 @@ def __init__(self, action_config: dict): # TODO(@xiyuoh) Remove requirement for configuring dock_to_cart and # exit lot as they will be created on the robot by the # fleet adapter, hence will not need to be configured - elif 'dock_to_cart' not in action_config['missions']: + elif ('dock_to_cart' not in action_config['missions'] or + action_config['missions']['dock_to_cart'] is None): raise KeyError( f'CartDelivery MirAction requires the configured MiR mission ' f'name for [dock_to_cart], but it is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') - elif 'pickup' not in action_config['missions']: + elif ('pickup' not in action_config['missions'] or + action_config['missions']['pickup'] is None): raise KeyError( f'CartDelivery MirAction requires the configured MiR mission ' f'name for [pickup], but it is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') - elif 'dropoff' not in action_config['missions']: + elif ('dropoff' not in action_config['missions'] or + action_config['missions']['dropoff'] is None): raise KeyError( f'CartDelivery MirAction requires the configured MiR mission ' f'name for [dropoff], but it is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') - elif 'exit_lot' not in action_config['missions']: + elif ('exit_lot' not in action_config['missions'] or + action_config['missions']['exit_lot'] is None): raise KeyError( f'CartDelivery MirAction requires the configured MiR mission ' f'name for [exit_lot], but it is not provided in the action ' @@ -105,23 +109,28 @@ def perform_action( description: dict, context: ActionContext, ) -> MirAction: + # Import CartDetection module + # TODO(@xiyuoh) Import this only once during ActionFactory instantiation + detection_module = context.action_config['cart_detection_module'] + detection_plugin = importlib.import_module(detection_module) + cart_detection = detection_plugin.CartDetection(context) + match category: case 'delivery_pickup': - return CartPickup(description, context) + return CartPickup(description, context, cart_detection) case 'delivery_dropoff': - return CartDropoff(description, context) + return CartDropoff(description, context, cart_detection) class CartPickup(MirAction): - def __init__(self, description: dict, context: ActionContext): + def __init__( + self, + description: dict, + context: ActionContext, + cart_detection + ): MirAction.__init__(self, context) - # Import CartDetection module - # TODO(@xiyuoh) Import this only once during ActionFactory instantiation - detection_module = context.action_config['cart_detection_module'] - detection_plugin = importlib.import_module(detection_module) - self.cart_detection = detection_plugin.CartDetection(context) - # Mission names to be used during pickup self.dock_to_cart_mission = \ self.context.action_config['missions']['dock_to_cart'] @@ -130,15 +139,15 @@ def __init__(self, description: dict, context: ActionContext): self.exit_mission = \ self.context.action_config['missions']['exit_lot'] + self.cart_detection = cart_detection self.search_timeout = \ self.context.action_config.get('search_timeout', 60) # seconds self.cart_marker_type_guid = \ self.context.api.docking_offsets_guid_get( self.context.action_config['marker_types']['cart']) - # Begin action # Check if the robot's latch is currently open - if self.is_latch_open(): + if self.cart_detection.is_latch_open(): # Latch is open, unable to perform pickup self.context.node.get_logger().info( f'Robot [{self.context.name}] latch is open, unable to ' @@ -146,12 +155,13 @@ def __init__(self, description: dict, context: ActionContext): self.cancel_task() return + # Begin action self.context.node.get_logger().info( f'New pickup requested for robot [{self.context.name}]') self.pickup = Pickup( state=PickupState.AT_PICKUP, pickup_lots=[description.get('pickup_lot')], - cart_id=description.get('cart_id'), + cart_id=description.get('cart_id', None), mission=None, latching=False ) @@ -164,8 +174,7 @@ def update_action(self): return False def update_pickup(self, pickup: Pickup): - # If this is a PerformAction pickup, check that the action is underway - # and valid + # Check that the action is underway and valid if self.context.execution is not None and not self.context.execution.okay(): self.context.node.get_logger().info( f'[delivery_pickup] action is killed/canceled.') @@ -177,7 +186,6 @@ def update_pickup(self, pickup: Pickup): match pickup.state: case PickupState.AT_PICKUP: # Send rmf_dock_to_cart mission - assert self.dock_to_cart_mission is not None current_wp_name = pickup.pickup_lots[0] mir_pos = self.context.api.known_positions.get(current_wp_name) if not mir_pos: @@ -255,10 +263,9 @@ def update_pickup(self, pickup: Pickup): case PickupState.DOCK_COMPLETED: # Check if robot docked under the correct cart - cart_check = self.is_correct_cart(pickup.cart_id) + cart_check = self.cart_detection.is_correct_cart(pickup.cart_id) if cart_check: # If cart is correct, send pickup mission - assert self.pickup_mission is not None mission_queue_id = self.context.api.queue_mission_by_name( self.pickup_mission) if not mission_queue_id: @@ -288,7 +295,8 @@ def update_pickup(self, pickup: Pickup): self.context.node.get_logger().info( f'Robot [{self.context.name}] found the wrong cart, exiting ' f'lot and cancelling task.') - self.exit_lot() + if not self.exit_lot(): + return self.cancel_task() pickup.state = PickupState.TASK_CANCELLED @@ -304,7 +312,8 @@ def update_pickup(self, pickup: Pickup): self.context.node.get_logger().info( f'Robot [{self.context.name}] successfully received cart, ' f'exiting lot with cart and ending mission') - self.exit_lot() + if not self.exit_lot(): + return return True case PickupState.TASK_CANCELLED: @@ -324,29 +333,24 @@ def update_pickup(self, pickup: Pickup): # Clear any errors self.context.api.clear_error() self.context.api.status_put(state_id=MiRStateCode.READY) + # NOTE(@xiyuoh) If this task is cancelled in the middle of a - # cart pickup (latching), the users should implement a - # cancellation behavior in the task JSON to ensure that this - # robot can continue with its next task without having a cart - # latched on. + # cart pickup, the dispatch_delivery task is built with a + # cancellation behavior to ensure that this robot drops the cart + # off and is able to continue with its next task safely return True return False - def is_latch_open(self): - return self.cart_detection.is_latch_open() - - def is_correct_cart(self, cart_id: str): - return self.cart_detection.is_correct_cart(cart_id) - - def is_under_cart(self): - return self.cart_detection.is_under_cart() - def exit_lot(self): - if not self.context.api.connected: - return None - # Set footprint to robot footprint before exiting lot - return self.context.api.queue_mission_by_name(self.exit_mission) + mission_queue_id = self.context.api.queue_mission_by_name(self.exit_mission) + if not mission_queue_id: + self.context.node.get_logger().info( + f'Unable to queue exit lot mission for robot ' + f'[{self.context.name}], retrying in the next ' + f'action loop' + ) + return mission_queue_id def cancel_task(self, label: str = ''): def _cancel_success(): @@ -358,12 +362,35 @@ def _cancel_fail(): class CartDropoff(MirAction): - def __init__(self, description: dict, context: ActionContext): + def __init__( + self, + description: dict, + context: ActionContext, + cart_detection + ): MirAction.__init__(self, context) - self.dropoff: Dropoff = None + self.skip_action = False + self.cart_detection = cart_detection self.dropoff_mission = \ self.context.action_config['missions']['dropoff'] + self.exit_mission = \ + self.context.action_config['missions']['exit_lot'] + + # Check if robot is currently latching onto a cart. If the latch is + # released before triggering a dropoff mission, there is no need to + # perform a full dropoff. The action should end after ensuring that the + # robot is no longer under a cart. + if not self.cart_detection.is_latch_open(): + self.context.node.get_logger().info( + f'Robot [{self.context.name}] latch is not open, dropoff ' + f'mission will not be queued') + self.skip_action = True + + # Begin action + self.context.node.get_logger().info( + f'New dropoff requested for robot [{self.context.name}]') + self.dropoff = Dropoff(mission=None) def update_action(self): if self.update_dropoff(self.dropoff): @@ -373,36 +400,43 @@ def update_action(self): return False def update_dropoff(self, dropoff: Dropoff): - # Check if action is underway or cancelled + # Check that the action is underway and valid. No action if action has + # been killed or cancelled, as we want to ensure that the robot is free + # of carts. if self.context.execution is not None and not self.context.execution.okay(): self.context.node.get_logger().info( - f'[delivery_dropoff] action is killed/canceled') - - # If cart release is in progress, we let it finish first - if dropoff.mission and not self.context.api.mission_completed( - dropoff.mission.queue_id): - return False - - # Task is cancelled and cart is done dropping off/mission is not - # yet queued anyway, we can mark it as completed at this point - self.context.api.clear_error() - self.context.api.status_put(state_id=MiRStateCode.READY) - # Mark dropoff session as completed + f'[delivery_dropoff] action is killed/canceled! Proceeding to ' + f'queue cart dropoff mission to ensure that robot has fully ' + f'released any attached cart and is safe to perform subsequent ' + f'tasks.') + + # If latch is already open, check if the robot is under a cart and queue + # exit lot mission accordingly + if self.skip_action: + if self.cart_detection.is_under_cart(): + self.context.node.get_logger().info( + f'Robot [{self.context.name}] is detected to be under a ' + f'cart, exiting lot...' + ) + if not self.context.api.queue_mission_by_name(self.exit_mission): + self.context.node.get_logger().info( + f'Unable to queue exit lot mission for robot ' + f'[{self.context.name}], retrying in the next ' + f'action loop' + ) + return return True - # No mission queued yet + # No mission queued yet, queue dropoff mission if dropoff.mission is None: - assert self.dropoff_mission is not None mission_queue_id = self.context.api.queue_mission_by_name( self.dropoff_mission) if not mission_queue_id: - error_str = \ - f'Mission {self.dropoff_mission} not supported, ignoring' - self.context.node.get_logger().error(error_str) - return True - self.context.node.get_logger().info( - f'Mission {self.dropoff_mission} added to queue for robot ' - f'[{self.context.name}].') + self.context.node.get_logger().error( + f'Unable to queue mission [{self.dropoff_mission}], ' + f'retrying on next action update' + ) + return now = self.context.node.get_clock().now().nanoseconds / 1e9 dropoff.mission = Mission(mission_queue_id, now) self.context.node.get_logger().info( @@ -414,6 +448,8 @@ def update_dropoff(self, dropoff: Dropoff): if self.context.api.mission_completed(dropoff.mission.queue_id): self.context.node.get_logger().info( f'[{self.context.name}] Dropoff mission completed!') + self.context.api.clear_error() + self.context.api.status_put(state_id=MiRStateCode.READY) return True else: self.context.node.get_logger().info( diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py index c6f5729..bedb6e1 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_detection.py @@ -12,45 +12,44 @@ # See the License for the specific language governing permissions and # limitations under the License. +from abc import ABC, abstractmethod import requests from urllib.error import HTTPError from fleet_adapter_mir.robot_adapter_mir import ActionContext -class CartDetection: +class CartDetection(ABC): def __init__(self, context: ActionContext): self.context = context + ''' + This method checks if the robot's latch is open and carrying a cart. + Return True if latch is open, else False. + ''' + @abstractmethod def is_latch_open(self): - ''' - Checks if the robot's latch is open and carrying a cart - Return True if latch is open, else False - ''' - # ------------------------ - # IMPLEMENT YOUR CODE HERE - # ------------------------ - return False + # To be implemented + ... + ''' + This method checks if the robot is currently docked under a cart. + Return True if robot is under any carts, else False. + ''' + @abstractmethod def is_under_cart(self): - ''' - Checks if the robot is docked under a cart - Return True if robot is under any carts, else False - ''' - # ------------------------ - # IMPLEMENT YOUR CODE HERE - # ------------------------ - return False + # To be implemented + ... - def is_correct_cart(self, cart_id: str): - ''' - Checks if the detected cart identifier matches the target cart_id - Return True if cart is correct, False if cart is wrong, None if no - cart detected - ''' - # ------------------------ - # IMPLEMENT YOUR CODE HERE - # ------------------------ - return None + ''' + This method checks if the detected cart identifier matches the target + cart_id, if any. + Return True if cart is correct, False if cart is wrong, None if no cart is + detected. + ''' + @abstractmethod + def is_correct_cart(self, cart_id: str | None): + # To be implemented + ... # -------------------------------------------------------------------------- # HELPFUL FUNCTIONS FOR INTERACTING WITH MIR REST API diff --git a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py index b1d453e..5a29766 100644 --- a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py +++ b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py @@ -95,7 +95,6 @@ def __init__(self, argv=sys.argv): now.sec = now.sec + self.args.start_time start_time = now.sec * 1000 + round(now.nanosec/10**6) request["unix_millis_earliest_start_time"] = start_time - # todo(YV): Fill priority after schema is added # Define task request category request["category"] = "compose" @@ -125,7 +124,18 @@ def __init__(self, argv=sys.argv): description["phases"].append( {"activity": {"category": "sequence", "description": { - "activities": pickup_action_activity}}}) + "activities": pickup_action_activity}}}, + # Cancellation behavior for pickup phase: dropoff cart if robot has + # completed the pickup cart action. + {"on_cancel": {"category": "sequence", + "description": [{ + "category": "perform_action", + "description": { + "unix_millis_action_duration_estimate": 60000, + "category": "delivery_dropoff", + "description": {} + } + }]}}) # GoToPlace activity go_to_dropoff_activity = [{ "category": "go_to_place", @@ -134,7 +144,22 @@ def __init__(self, argv=sys.argv): description["phases"].append( {"activity": {"category": "sequence", "description": { - "activities": go_to_dropoff_activity}}}) + "activities": go_to_dropoff_activity}}}, + # Cancellation behavior for GoToPlace phase: dropoff cart at the + # current spot if robot is detected to be carrying a cart. + {"on_cancel": {"category": "sequence", + "description": [{ + "category": "perform_action", + "description": { + "unix_millis_action_duration_estimate": 60000, + "category": "delivery_dropoff", + "description": {} + } + }]}}) + # NOTE(@xiyuoh) If the given site is unsafe for the robot to dropoff + # a cart at random locations, integrators may choose to add on to + # this cancellation behavior to ensure that the robot travels to a + # safe waypoint before performing the cancellation dropoff. # Dropoff activity dropoff_action_activity = [{ "category": "perform_action", From 6d8d3e61f477a05c2faf9f6e65ecf7e636edbb74 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 8 Oct 2024 06:34:00 +0000 Subject: [PATCH 39/50] Move execution out of ActionContext and add supports_action Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 1 - .../fleet_adapter_mir/robot_adapter_mir.py | 16 +- .../fleet_adapter_mir_actions/mir_action.py | 81 ++++++---- .../rmf_cart_delivery.py | 141 +++++++++++------- 4 files changed, 143 insertions(+), 96 deletions(-) diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index b0177c9..a7afc8f 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -97,7 +97,6 @@ rmf_fleet: plugins: rmf_cart_delivery: module: "fleet_adapter_mir_actions.rmf_cart_delivery" - class: "CartDelivery" actions: ["rmf_cart_delivery", "delivery_pickup", "delivery_dropoff"] cart_detection_module: "fleet_adapter_mir_actions.rmf_cart_detection" missions: diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index ead2939..bde9c35 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -75,13 +75,13 @@ def __init__(self, mir_api: MirAPI, update_handle, # rmf_fleet_adapter.RobotUpdateHandle fleet_config: rmf_easy.FleetConfiguration, - execution): + action_config: dict): self.node = node self.name = name self.api = mir_api self.update_handle = update_handle self.fleet_config = fleet_config - self.execution = execution + self.action_config = action_config class RobotAdapterMiR: @@ -164,7 +164,11 @@ def __init__( try: module = action_config['module'] plugin = importlib.import_module(module) - action_factory = plugin.ActionFactory(action_config) + action_context = ActionContext( + self.node, self.name, self.api, self.update_handle, + self.fleet_config, action_config + ) + action_factory = plugin.ActionFactory(action_context) self.action_factories[plugin_name] = action_factory except KeyError: self.node.get_logger().info( @@ -786,12 +790,8 @@ def perform_action(self, category, description, execution): for _, action_factory in self.action_factories.items(): if category in action_factory.actions: - action_context = ActionContext( - self.node, self.name, self.api, self.update_handle, - self.fleet_config, execution - ) action_obj = action_factory.perform_action( - category, description, action_context + category, description, execution ) self.current_action = action_obj return diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 78b944f..167b2c7 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -15,39 +15,13 @@ import json from abc import ABC, abstractmethod from typing import Callable -import rclpy -import rclpy.node as Node -import rmf_adapter.easy_full_control as rmf_easy from fleet_adapter_mir.robot_adapter_mir import ActionContext -from fleet_adapter_mir.mir_api import MirAPI class MirAction(ABC): def __init__(self, context: ActionContext): self.context = context - missions_json = context.action_config.get('missions_json') - if missions_json: - with open(missions_json, 'r') as g: - action_missions = json.load(g) - - # Check if these missions are already created on the robot - missions_created = True - for mission_name in action_missions.keys(): - if mission_name not in context.api.known_missions: - missions_created = False - break - if missions_created: - return - - # Create these missions on the robot - context.api.create_missions(action_missions) - # Update mission actions stored in MirAPI - for mission, mission_data in context.api.known_missions.items(): - context.api.mission_actions[mission] = \ - context.api.missions_mission_id_actions_get( - mission_data['guid']) - ''' This method is called on every update by the robot adapter to monitor the progress and completion of the action. @@ -58,6 +32,9 @@ def update_action(self): # To be populated in the plugins ... + ''' + This method may be used to cancel the current ongoing task. + ''' def cancel_current_task(self, cancel_success: Callable[[], None], cancel_fail: Callable[[], None], @@ -81,13 +58,27 @@ def _on_cancel(result: bool): class MirActionFactory(ABC): - def __init__(self, action_config: dict): - if 'actions' not in action_config: + def __init__(self, context: ActionContext): + self.context = context + + if 'actions' not in context.action_config: raise KeyError( f'List of supported actions is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') - self.action_config = action_config - self.actions = action_config['actions'] + self.actions = context.action_config['actions'] + + # Create required MiR missions on the robot + missions_json = context.action_config.get('missions_json') + self.create_missions(missions_json) + + ''' + This method can be used to verify whether this MirActionFactory supports the + configured action. + ''' + @abstractmethod + def supports_action(self, category: str) -> bool: + # To be populated in the plugins + ... ''' This method creates a MirAction object for the robot adapter to begin and @@ -98,7 +89,35 @@ def perform_action( self, category: str, description: dict, - context: ActionContext, + execution, ) -> MirAction: # To be populated in the plugins ... + + ''' + This method may be used to create any MiR missions required for this + MirAction on the robot from a JSON file. + ''' + def create_missions(self, missions_json): + if missions_json is None: + return + + with open(missions_json, 'r') as g: + action_missions = json.load(g) + + # Check if these missions are already created on the robot + missions_created = True + for mission_name in action_missions.keys(): + if mission_name not in self.context.api.known_missions: + missions_created = False + break + if missions_created: + return + + # Create these missions on the robot + self.context.api.create_missions(action_missions) + # Update mission actions stored in MirAPI + for mission, mission_data in self.context.api.known_missions.items(): + self.context.api.mission_actions[mission] = \ + self.context.api.missions_mission_id_actions_get( + mission_data['guid']) diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index 6086bef..8d475e8 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -53,79 +53,96 @@ class Dropoff: class ActionFactory(MirActionFactory): - def __init__(self, action_config: dict): - MirActionFactory.__init__(action_config) + def __init__(self, context: ActionContext): + MirActionFactory.__init__(context) # Raise error if config file is invalid # NOTE(@xiyuoh) Using if-else to check for valid keys in the action - # config is not the most scalable. Consider other ways to - # do this. - if 'cart_detection_module' not in action_config: + # config is not the most scalable. Consider other ways to do this. + if 'cart_detection_module' not in context.action_config: raise KeyError( f'CartDelivery MirAction requires a cart detection module, but ' f'path to [cart_detection_module] is not provided in the ' f'action config! Unable to instantiate an ActionFactory.') - elif 'marker_types' not in action_config: + elif 'marker_types' not in context.action_config: raise KeyError( f'CartDelivery MirAction requires configured marker types, but ' f'[marker_types] not provided in the action config! ' f'Unable to instantiate an ActionFactory.') - elif 'missions' not in action_config: + elif 'missions' not in context.action_config: raise KeyError( f'CartDelivery MirAction requires configured missions, but ' f'[missions] not provided in the action config! ' f'Unable to instantiate an ActionFactory.') # Check if the required mission names have been configured - # TODO(@xiyuoh) Remove requirement for configuring dock_to_cart and - # exit lot as they will be created on the robot by the - # fleet adapter, hence will not need to be configured - elif ('dock_to_cart' not in action_config['missions'] or - action_config['missions']['dock_to_cart'] is None): + elif ('dock_to_cart' not in context.action_config['missions'] or + context.action_config['missions']['dock_to_cart'] is None): raise KeyError( f'CartDelivery MirAction requires the configured MiR mission ' f'name for [dock_to_cart], but it is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') - elif ('pickup' not in action_config['missions'] or - action_config['missions']['pickup'] is None): + elif ('pickup' not in context.action_config['missions'] or + context.action_config['missions']['pickup'] is None): raise KeyError( f'CartDelivery MirAction requires the configured MiR mission ' f'name for [pickup], but it is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') - elif ('dropoff' not in action_config['missions'] or - action_config['missions']['dropoff'] is None): + elif ('dropoff' not in context.action_config['missions'] or + context.action_config['missions']['dropoff'] is None): raise KeyError( f'CartDelivery MirAction requires the configured MiR mission ' f'name for [dropoff], but it is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') - elif ('exit_lot' not in action_config['missions'] or - action_config['missions']['exit_lot'] is None): + elif ('exit_lot' not in context.action_config['missions'] or + context.action_config['missions']['exit_lot'] is None): raise KeyError( f'CartDelivery MirAction requires the configured MiR mission ' f'name for [exit_lot], but it is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') + # Import CartDetection module + detection_module = context.action_config['cart_detection_module'] + detection_plugin = importlib.import_module(detection_module) + self.cart_detection = detection_plugin.CartDetection(self.context) + + # Verify that this ActionFactory supports all the actions specified in + # the action config + for action in self.actions: + if not self.supports_action(action): + raise ValueError( + f'The plugin config provided [{action}] as a performable ' + f'action, but it is not a supported action in the ' + f'rmf_cart_delivery ActionFactory!' + ) + + def supports_action(self, category: str) -> bool: + match category: + case 'delivery_pickup': + return True + case 'delivery_dropoff': + return True + case _: + return False + def perform_action( self, category: str, description: dict, - context: ActionContext, + execution ) -> MirAction: - # Import CartDetection module - # TODO(@xiyuoh) Import this only once during ActionFactory instantiation - detection_module = context.action_config['cart_detection_module'] - detection_plugin = importlib.import_module(detection_module) - cart_detection = detection_plugin.CartDetection(context) - match category: case 'delivery_pickup': - return CartPickup(description, context, cart_detection) + return CartPickup( + description, execution, self.context, self.cart_detection) case 'delivery_dropoff': - return CartDropoff(description, context, cart_detection) + return CartDropoff( + description, execution, self.context, self.cart_detection) class CartPickup(MirAction): def __init__( self, description: dict, + execution, context: ActionContext, cart_detection ): @@ -133,18 +150,19 @@ def __init__( # Mission names to be used during pickup self.dock_to_cart_mission = \ - self.context.action_config['missions']['dock_to_cart'] + context.action_config['missions']['dock_to_cart'] self.pickup_mission = \ - self.context.action_config['missions']['pickup'] + context.action_config['missions']['pickup'] self.exit_mission = \ - self.context.action_config['missions']['exit_lot'] + context.action_config['missions']['exit_lot'] + self.execution = execution self.cart_detection = cart_detection self.search_timeout = \ - self.context.action_config.get('search_timeout', 60) # seconds + context.action_config.get('search_timeout', 60) # seconds self.cart_marker_type_guid = \ - self.context.api.docking_offsets_guid_get( - self.context.action_config['marker_types']['cart']) + context.api.docking_offsets_guid_get( + context.action_config['marker_types']['cart']) # Check if the robot's latch is currently open if self.cart_detection.is_latch_open(): @@ -152,7 +170,9 @@ def __init__( self.context.node.get_logger().info( f'Robot [{self.context.name}] latch is open, unable to ' f'perform pickup, cancelling task...') - self.cancel_task() + self.cancel_task( + label='Robot latch is still open, unable to perform pickup.' + ) return # Begin action @@ -161,21 +181,21 @@ def __init__( self.pickup = Pickup( state=PickupState.AT_PICKUP, pickup_lots=[description.get('pickup_lot')], - cart_id=description.get('cart_id', None), + cart_id=description.get('cart_id'), mission=None, latching=False ) def update_action(self): if self.update_pickup(self.pickup): - if self.context.execution is not None: - self.context.execution.finished() + if self.execution is not None: + self.execution.finished() return True return False def update_pickup(self, pickup: Pickup): # Check that the action is underway and valid - if self.context.execution is not None and not self.context.execution.okay(): + if self.execution is not None and not self.execution.okay(): self.context.node.get_logger().info( f'[delivery_pickup] action is killed/canceled.') pickup.state = PickupState.TASK_CANCELLED @@ -192,7 +212,10 @@ def update_pickup(self, pickup: Pickup): self.context.node.get_logger().info( f'No shelf position [{mir_pos}] found on robot ' f'[{self.context.name}], cancelling task') - self.cancel_task() + self.cancel_task( + label=f'MiR position is not found on the robot, ' + \ + f'unable to pickup.' + ) pickup.state = PickupState.TASK_CANCELLED return cart_marker_guid = mir_pos['guid'] @@ -287,7 +310,9 @@ def update_pickup(self, pickup: Pickup): f'Robot [{self.context.name}] was unable to dock under any ' f'carts, please check that cart is present. ' f'Cancelling task.') - self.cancel_task() + self.cancel_task( + label=f'Dock to cart failed, unable to perform pickup.' + ) pickup.state = PickupState.TASK_CANCELLED else: # If cart is wrong, cancel this task also but after we @@ -297,7 +322,9 @@ def update_pickup(self, pickup: Pickup): f'lot and cancelling task.') if not self.exit_lot(): return - self.cancel_task() + self.cancel_task( + label=f'Wrong cart found, unable to perform pickup.' + ) pickup.state = PickupState.TASK_CANCELLED case PickupState.PICKUP_REQUESTED: @@ -365,27 +392,30 @@ class CartDropoff(MirAction): def __init__( self, description: dict, + execution, context: ActionContext, cart_detection ): MirAction.__init__(self, context) - self.skip_action = False + self.execution = execution self.cart_detection = cart_detection self.dropoff_mission = \ - self.context.action_config['missions']['dropoff'] + context.action_config['missions']['dropoff'] self.exit_mission = \ - self.context.action_config['missions']['exit_lot'] + context.action_config['missions']['exit_lot'] # Check if robot is currently latching onto a cart. If the latch is # released before triggering a dropoff mission, there is no need to - # perform a full dropoff. The action should end after ensuring that the - # robot is no longer under a cart. + # perform a full dropoff. We can skip queueing the dropoff mission in + # the update loop and end action after ensuring that the robot is no + # longer under a cart. + self.skip_dropoff = False if not self.cart_detection.is_latch_open(): self.context.node.get_logger().info( f'Robot [{self.context.name}] latch is not open, dropoff ' f'mission will not be queued') - self.skip_action = True + self.skip_dropoff = True # Begin action self.context.node.get_logger().info( @@ -394,8 +424,8 @@ def __init__( def update_action(self): if self.update_dropoff(self.dropoff): - if self.context.execution is not None: - self.context.execution.finished() + if self.execution is not None: + self.execution.finished() return True return False @@ -403,16 +433,16 @@ def update_dropoff(self, dropoff: Dropoff): # Check that the action is underway and valid. No action if action has # been killed or cancelled, as we want to ensure that the robot is free # of carts. - if self.context.execution is not None and not self.context.execution.okay(): + if self.execution is not None and not self.execution.okay(): self.context.node.get_logger().info( f'[delivery_dropoff] action is killed/canceled! Proceeding to ' f'queue cart dropoff mission to ensure that robot has fully ' f'released any attached cart and is safe to perform subsequent ' f'tasks.') - # If latch is already open, check if the robot is under a cart and queue - # exit lot mission accordingly - if self.skip_action: + # If skip_dropoff flag is raised, check if the robot is under a cart and + # queue exit lot mission accordingly + if self.skip_dropoff: if self.cart_detection.is_under_cart(): self.context.node.get_logger().info( f'Robot [{self.context.name}] is detected to be under a ' @@ -421,8 +451,7 @@ def update_dropoff(self, dropoff: Dropoff): if not self.context.api.queue_mission_by_name(self.exit_mission): self.context.node.get_logger().info( f'Unable to queue exit lot mission for robot ' - f'[{self.context.name}], retrying in the next ' - f'action loop' + f'[{self.context.name}], retrying on the next update' ) return return True @@ -434,7 +463,7 @@ def update_dropoff(self, dropoff: Dropoff): if not mission_queue_id: self.context.node.get_logger().error( f'Unable to queue mission [{self.dropoff_mission}], ' - f'retrying on next action update' + f'retrying on next update' ) return now = self.context.node.get_clock().now().nanoseconds / 1e9 From fd45e287f5f274de51580299345792c7d48c6387 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 8 Oct 2024 06:50:34 +0000 Subject: [PATCH 40/50] Cleanup and codestyle Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 7 +- fleet_adapter_mir/README.md | 1 - .../fleet_adapter_mir_actions/mir_action.py | 10 +- .../rmf_cart_delivery.py | 103 ++++++++++-------- .../dispatch_delivery.py | 47 ++++---- missions/rmf_cart_missions.json | 13 --- 6 files changed, 89 insertions(+), 92 deletions(-) diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index a7afc8f..6beada3 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -104,15 +104,10 @@ plugins: pickup: "rmf_pickup_cart" # Name of MiR mission for robot to latch onto a cart - to be created and filled in by user dropoff: "rmf_dropoff_cart" # Name of MiR mission for robot to unlatch and exit from under a cart - to be created and filled in by user exit_lot: "rmf_exit_lot" # Name of MiR mission for robot to exit from under a cart - to be created by fleet adapter upon launch - update_footprint: "rmf_update_footprint" # Name of MiR mission for robot to update the footprint of a robot - to be created by fleet adapter upon launch - footprints: - robot: "MiR100-200" # Name of robot footprint stored on MiR - cart: "ShelfCart" # Name of cart footprint stored on MiR marker_types: cart: "AsymmetricShelf" # Name of cart marker type missions_json: "/home/some_user/mir_ws/src/fleet_adapter_mir/missions/rmf_cart_missions.json" # Filepath to RMF cart delivery missions - search_timeout: 60 - pickup_dist_threshold: 3.0 # metres + search_timeout: 60 # Dock to cart timeout in seconds # Examples of additional fields for cart detection module io_name: "MiR internal IOs" latch_io: 2 diff --git a/fleet_adapter_mir/README.md b/fleet_adapter_mir/README.md index e9b13c2..3a916f6 100644 --- a/fleet_adapter_mir/README.md +++ b/fleet_adapter_mir/README.md @@ -87,7 +87,6 @@ The `rmf_cart_delivery` plugin allows users to submit pickup and dropoff tasks t Some relevant MiR missions (docking, exit, update footprint) will be automatically created on the MiR on startup. These missions are used to facilitate the pickup and dropoff activities and can be found in the plugin config under `missions`. They are: - `rmf_dock_to_cart`: Docks robot under the cart - `rmf_exit_lot`: Calls the robot to exit from under the cart -- `rmf_update_footprint`: Updates the robot footprint They are defined and stored in the `rmf_cart_missions.json` file and do not require any further configuration. diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 167b2c7..2c00368 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -41,7 +41,8 @@ def cancel_current_task(self, label: str = ''): current_task_id = self.context.update_handle.more().current_task_id() self.context.node.get_logger().info( - f'[{self.context.name}] Cancel task requested for [{current_task_id}]') + f'[{self.context.name}] Cancel task requested for ' + f'[{current_task_id}]') def _on_cancel(result: bool): if result: @@ -51,7 +52,8 @@ def _on_cancel(result: bool): cancel_success() else: self.context.node.get_logger().info( - f'[{self.context.name}] Failed to cancel task [{current_task_id}]') + f'[{self.context.name}] Failed to cancel task ' + f'[{current_task_id}]') cancel_fail() self.context.update_handle.more().cancel_task( current_task_id, [label], lambda result: _on_cancel(result)) @@ -72,8 +74,8 @@ def __init__(self, context: ActionContext): self.create_missions(missions_json) ''' - This method can be used to verify whether this MirActionFactory supports the - configured action. + This method can be used to verify whether this MirActionFactory supports + the configured action. ''' @abstractmethod def supports_action(self, category: str) -> bool: diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index 8d475e8..81c99d6 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -60,13 +60,13 @@ def __init__(self, context: ActionContext): # config is not the most scalable. Consider other ways to do this. if 'cart_detection_module' not in context.action_config: raise KeyError( - f'CartDelivery MirAction requires a cart detection module, but ' - f'path to [cart_detection_module] is not provided in the ' + f'CartDelivery MirAction requires a cart detection module, ' + f'but path to [cart_detection_module] is not provided in the ' f'action config! Unable to instantiate an ActionFactory.') elif 'marker_types' not in context.action_config: raise KeyError( - f'CartDelivery MirAction requires configured marker types, but ' - f'[marker_types] not provided in the action config! ' + f'CartDelivery MirAction requires configured marker types, ' + f'but [marker_types] not provided in the action config! ' f'Unable to instantiate an ActionFactory.') elif 'missions' not in context.action_config: raise KeyError( @@ -78,8 +78,8 @@ def __init__(self, context: ActionContext): context.action_config['missions']['dock_to_cart'] is None): raise KeyError( f'CartDelivery MirAction requires the configured MiR mission ' - f'name for [dock_to_cart], but it is not provided in the action ' - f'config! Unable to instantiate an ActionFactory.') + f'name for [dock_to_cart], but it is not provided in the ' + f'action config! Unable to instantiate an ActionFactory.') elif ('pickup' not in context.action_config['missions'] or context.action_config['missions']['pickup'] is None): raise KeyError( @@ -202,7 +202,8 @@ def update_pickup(self, pickup: Pickup): # Start state machine check now = self.context.node.get_clock().now().nanoseconds / 1e9 - self.context.node.get_logger().debug(f'PickupState: {pickup.state.name}') + self.context.node.get_logger().debug( + f'PickupState: {pickup.state.name}') match pickup.state: case PickupState.AT_PICKUP: # Send rmf_dock_to_cart mission @@ -213,22 +214,24 @@ def update_pickup(self, pickup: Pickup): f'No shelf position [{mir_pos}] found on robot ' f'[{self.context.name}], cancelling task') self.cancel_task( - label=f'MiR position is not found on the robot, ' + \ + label=f'MiR position is not found on the robot, ' f'unable to pickup.' ) pickup.state = PickupState.TASK_CANCELLED return cart_marker_guid = mir_pos['guid'] - cart_marker_param = self.context.api.get_mission_params_with_value( - self.dock_to_cart_mission, - 'docking', - 'cart_marker', - cart_marker_guid) - marker_type_param = self.context.api.get_mission_params_with_value( - self.dock_to_cart_mission, - 'docking', - 'cart_marker_type', - self.cart_marker_type_guid) + cart_marker_param = \ + self.context.api.get_mission_params_with_value( + self.dock_to_cart_mission, + 'docking', + 'cart_marker', + cart_marker_guid) + marker_type_param = \ + self.context.api.get_mission_params_with_value( + self.dock_to_cart_mission, + 'docking', + 'cart_marker_type', + self.cart_marker_type_guid) mission_params = cart_marker_param + marker_type_param mission_queue_id = self.context.api.queue_mission_by_name( self.dock_to_cart_mission, mission_params) @@ -241,17 +244,17 @@ def update_pickup(self, pickup: Pickup): pickup.mission = Mission(mission_queue_id, now) pickup.state = PickupState.DOCK_REQUESTED self.context.node.get_logger().info( - f'[{self.context.name}] Dock to cart mission requested with ' - f'mission queue id {mission_queue_id}') + f'[{self.context.name}] Dock to cart mission requested ' + f'with mission queue id {mission_queue_id}') case PickupState.DOCK_REQUESTED: # Make sure that there is an rmf_dock_to_cart mission issued if not pickup.mission: self.context.node.get_logger().info( - f'Robot [{self.context.name}] is indicated to be at the ' - f'DOCK_REQUESTED state but no mission queue ID stored ' - f'for this docking mission! Returning to AT_PICKUP ' - f'state.') + f'Robot [{self.context.name}] is indicated to be at ' + f'the DOCK_REQUESTED state but no mission queue ID ' + f'stored for this docking mission! Returning to ' + f'AT_PICKUP state.') pickup.state = PickupState.AT_PICKUP return # Mission completed, move onto the next state @@ -273,7 +276,8 @@ def update_pickup(self, pickup: Pickup): # Mission timeout, cart not found if seconds_passed > self.search_timeout: # Delete mission from mir first - self.context.api.mission_queue_id_delete(pickup.mission.queue_id) + self.context.api.mission_queue_id_delete( + pickup.mission.queue_id) # Regardless of whether the robot completed docking # properly, we move to the next state to check self.context.node.get_logger().info( @@ -286,7 +290,8 @@ def update_pickup(self, pickup: Pickup): case PickupState.DOCK_COMPLETED: # Check if robot docked under the correct cart - cart_check = self.cart_detection.is_correct_cart(pickup.cart_id) + cart_check = \ + self.cart_detection.is_correct_cart(pickup.cart_id) if cart_check: # If cart is correct, send pickup mission mission_queue_id = self.context.api.queue_mission_by_name( @@ -300,15 +305,15 @@ def update_pickup(self, pickup: Pickup): pickup.mission = Mission(mission_queue_id, now) pickup.latching = True self.context.node.get_logger().info( - f'Robot [{self.context.name}] found the correct cart, pickup ' - f'mission requested with mission queue id ' + f'Robot [{self.context.name}] found the correct cart, ' + f'pickup mission requested with mission queue id ' f'{mission_queue_id}') pickup.state = PickupState.PICKUP_REQUESTED elif cart_check is None: # If cart is missing, cancel this task self.context.node.get_logger().info( - f'Robot [{self.context.name}] was unable to dock under any ' - f'carts, please check that cart is present. ' + f'Robot [{self.context.name}] was unable to dock ' + f'under any carts, please check that cart is present. ' f'Cancelling task.') self.cancel_task( label=f'Dock to cart failed, unable to perform pickup.' @@ -318,8 +323,8 @@ def update_pickup(self, pickup: Pickup): # If cart is wrong, cancel this task also but after we # exit from the lot self.context.node.get_logger().info( - f'Robot [{self.context.name}] found the wrong cart, exiting ' - f'lot and cancelling task.') + f'Robot [{self.context.name}] found the wrong cart, ' + f'exiting lot and cancelling task.') if not self.exit_lot(): return self.cancel_task( @@ -345,17 +350,20 @@ def update_pickup(self, pickup: Pickup): case PickupState.TASK_CANCELLED: self.context.node.get_logger().info( - f'Robot [{self.context.name}] is in pickup cancelled state.') + f'Robot [{self.context.name}] is in pickup cancelled ' + f'state.') # If some MiR mission is in progress, we abort it unless # it is latching if pickup.mission and not self.context.api.mission_completed( pickup.mission.queue_id): if pickup.latching: self.context.node.get_logger().info( - f'Robot [{self.context.name}] is performing latching, ' - f'cancelling task after this action is complete.') + f'Robot [{self.context.name}] is performing ' + f'latching, cancelling task after this action is ' + f'complete.') return False - self.context.api.mission_queue_id_delete(pickup.mission.queue_id) + self.context.api.mission_queue_id_delete( + pickup.mission.queue_id) pickup.mission = None # Clear any errors self.context.api.clear_error() @@ -363,14 +371,15 @@ def update_pickup(self, pickup: Pickup): # NOTE(@xiyuoh) If this task is cancelled in the middle of a # cart pickup, the dispatch_delivery task is built with a - # cancellation behavior to ensure that this robot drops the cart - # off and is able to continue with its next task safely + # cancellation behavior to ensure that this robot drops the + # cart off and is able to continue with its next task safely return True return False def exit_lot(self): - mission_queue_id = self.context.api.queue_mission_by_name(self.exit_mission) + mission_queue_id = \ + self.context.api.queue_mission_by_name(self.exit_mission) if not mission_queue_id: self.context.node.get_logger().info( f'Unable to queue exit lot mission for robot ' @@ -383,6 +392,7 @@ def cancel_task(self, label: str = ''): def _cancel_success(): if self.pickup: self.pickup.state = PickupState.TASK_CANCELLED + def _cancel_fail(): pass self.cancel_current_task(_cancel_success, _cancel_fail, label) @@ -437,18 +447,19 @@ def update_dropoff(self, dropoff: Dropoff): self.context.node.get_logger().info( f'[delivery_dropoff] action is killed/canceled! Proceeding to ' f'queue cart dropoff mission to ensure that robot has fully ' - f'released any attached cart and is safe to perform subsequent ' - f'tasks.') + f'released any attached cart and is safe to perform ' + f'subsequent tasks.') - # If skip_dropoff flag is raised, check if the robot is under a cart and - # queue exit lot mission accordingly + # If skip_dropoff flag is raised, check if the robot is under a cart + # and queue exit lot mission accordingly if self.skip_dropoff: if self.cart_detection.is_under_cart(): self.context.node.get_logger().info( f'Robot [{self.context.name}] is detected to be under a ' f'cart, exiting lot...' ) - if not self.context.api.queue_mission_by_name(self.exit_mission): + if not self.context.api.queue_mission_by_name( + self.exit_mission): self.context.node.get_logger().info( f'Unable to queue exit lot mission for robot ' f'[{self.context.name}], retrying on the next update' @@ -469,8 +480,8 @@ def update_dropoff(self, dropoff: Dropoff): now = self.context.node.get_clock().now().nanoseconds / 1e9 dropoff.mission = Mission(mission_queue_id, now) self.context.node.get_logger().info( - f'[{self.context.name}] Dropoff mission requested with mission queue ' - f'id {mission_queue_id}') + f'[{self.context.name}] Dropoff mission requested with ' + f'mission queue id {mission_queue_id}') # Mission queued, check completion else: diff --git a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py index 5a29766..233a98a 100644 --- a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py +++ b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_delivery.py @@ -127,15 +127,16 @@ def __init__(self, argv=sys.argv): "activities": pickup_action_activity}}}, # Cancellation behavior for pickup phase: dropoff cart if robot has # completed the pickup cart action. - {"on_cancel": {"category": "sequence", - "description": [{ - "category": "perform_action", - "description": { - "unix_millis_action_duration_estimate": 60000, - "category": "delivery_dropoff", - "description": {} - } - }]}}) + {"on_cancel": { + "category": "sequence", + "description": [{ + "category": "perform_action", + "description": { + "unix_millis_action_duration_estimate": 60000, + "category": "delivery_dropoff", + "description": {} + } + }]}}) # GoToPlace activity go_to_dropoff_activity = [{ "category": "go_to_place", @@ -145,21 +146,23 @@ def __init__(self, argv=sys.argv): {"activity": {"category": "sequence", "description": { "activities": go_to_dropoff_activity}}}, + # NOTE(@xiyuoh) If the given site is unsafe for the robot to + # dropoff a cart at random locations, integrators may choose to add + # on to this cancellation behavior to ensure that the robot travels + # to a safe waypoint before performing the cancellation dropoff. + # Cancellation behavior for GoToPlace phase: dropoff cart at the # current spot if robot is detected to be carrying a cart. - {"on_cancel": {"category": "sequence", - "description": [{ - "category": "perform_action", - "description": { - "unix_millis_action_duration_estimate": 60000, - "category": "delivery_dropoff", - "description": {} - } - }]}}) - # NOTE(@xiyuoh) If the given site is unsafe for the robot to dropoff - # a cart at random locations, integrators may choose to add on to - # this cancellation behavior to ensure that the robot travels to a - # safe waypoint before performing the cancellation dropoff. + {"on_cancel": { + "category": "sequence", + "description": [{ + "category": "perform_action", + "description": { + "unix_millis_action_duration_estimate": 60000, + "category": "delivery_dropoff", + "description": {} + } + }]}}) # Dropoff activity dropoff_action_activity = [{ "category": "perform_action", diff --git a/missions/rmf_cart_missions.json b/missions/rmf_cart_missions.json index d710062..978bf71 100644 --- a/missions/rmf_cart_missions.json +++ b/missions/rmf_cart_missions.json @@ -64,18 +64,5 @@ ], "action_type": "docking" } - ], - "rmf_update_footprint": [ - { - "priority": 1, - "parameters": [ - { - "value": "some_placeholder_footprint_guid", - "input_name": "footprint", - "id": "footprint" - } - ], - "action_type": "set_footprint" - } ] } \ No newline at end of file From 30d58ffb5b0bafd9f2aed7b4b349677aee1e9821 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 8 Oct 2024 09:05:14 +0000 Subject: [PATCH 41/50] Add and refine documentation Signed-off-by: Xiyu Oh --- docs/mir_actions.md | 56 ++++++++++++++++++++++ docs/mir_missions.md | 39 ++++++++++++++++ fleet_adapter_mir/README.md | 82 +++++++++++---------------------- missions/rmf_cart_missions.json | 56 +++++++++++----------- 4 files changed, 149 insertions(+), 84 deletions(-) create mode 100644 docs/mir_actions.md create mode 100644 docs/mir_missions.md diff --git a/docs/mir_actions.md b/docs/mir_actions.md new file mode 100644 index 0000000..b212a23 --- /dev/null +++ b/docs/mir_actions.md @@ -0,0 +1,56 @@ +## MiR Action Plugins + +This section lists and elaborates on the MiR actions provided out-of-the-box in this repo. + +## Available Action Plugins + +* [rmf_cart_delivery](#rmf_cart_delivery) + + +## rmf_cart_delivery + +### Overview + +The `rmf_cart_delivery` plugin allows users to submit pickup and dropoff tasks for MiR from point A to point B via RMF. The intended workflow of a delivery task is as follows: +1. RMF will send the robot to the pickup lot. +2. The robot will attempt to dock under a cart in the pickup lot. If the cart is missing or is not the desired cart, RMF will cancel the task. +3. If the robot successfully docks under the correct cart, it will latch onto the cart. +4. With the cart attached, the robot will move to the designated dropoff point. +5. Upon reaching the dropoff point, the robot will release and exit from under the cart. The task ends once the robot has safely exited. + +### Setup + +Some relevant MiR missions are pre-defined and can be automatically created on the MiR on startup. These missions are used to facilitate the pickup and dropoff activities. They are defined and stored in the `rmf_cart_missions.json` file and do not require any further configuration. + +However, since there are various types of latching methods available for different MiR models, users will need to set up their custom pickup and dropoff missions on the MiR, as well as implement their own `CartDetection` plugin module with the appropriate APIs to detect latching states. + +Before setting up, you may want to refer to the [list of RMF missions](https://github.com/open-rmf/fleet_adapter_mir/blob/main/docs/mir_missions.md#RMF-missions-for-rmf_cart_delivery) required for this MiR Action. + +Steps for setup: + +1. Create 2 missions on the MiR: + - `rmf_pickup_cart`: Triggers the robot's latching module to open + - `rmf_dropoff_cart`: Triggers the robot's latching module to close and release the cart, then exit from under the cart (relative move in the negative X-direction) +2. Fill in the MiR mission names in the plugin config under `missions`. + - This helps the fleet adapter identify and map the action to the missions you have created earlier. + - The recommended mission names are `rmf_pickup_cart` and `rmf_dropoff_cart`, per the instructions in Step 1. However, it is possible to use a different mission name as long as it is indicated accordingly under `missions`. +3. Fill in the appropriate cart marker type in the plugin config under `marker_type`. +4. Create your own `CartDetection` plugin. + - You are encouraged to use the `CartDetection` class in `rmf_cart_detection.py` as a base for your own module implementation. The class methods will be used by the `CartPickup` and `CartDropoff` Mir Actions. Some API calls to check the MiR's PLC registers and IO modules are provided in case you may want to use them. + - In the plugin config, update the `cart_detection_module` field to point to your own written module. +6. Enable RMF cart missions creation. + - If this is your first time setting up the action, and the pre-defined RMF cart missions have not been created on your robot, you will need to provide the filepath to `rmf_cart_missions.json` under `missions_json`. + +You can refer to `mir_config.yaml` under the `configs` folder for an example of a filled-in plugin configuration. + + +### Usage + +To submit a cart delivery task, you may use the `dispatch_delivery` task script found in the `fleet_adapter_mir_tasks` package: +```bash +ros2 run fleet_adapter_mir_tasks dispatch_delivery -g go_to_waypoint -p pickup_lot -d dropoff_lot -c some_cart_id +``` +- `-g`: Takes in an existing waypoint name for the robot to travel to before performing the pickup. The robot will begin docking into the pickup lot from this waypoint. +- `-p`: Name of the pickup lot. This name should be identical to the shelf position configured on the MiR. +- `-d`: Name of the dropoff lot. This name should be identical to the robot or shelf position configured on the MiR. +- `-c`: Optional cart identifier for the fleet adapter to assess whether the cart is correct for pickup. diff --git a/docs/mir_missions.md b/docs/mir_missions.md new file mode 100644 index 0000000..ee14c22 --- /dev/null +++ b/docs/mir_missions.md @@ -0,0 +1,39 @@ +## MiR Missions for RMF + +RMF relies on MiR missions to command MiR robots to move and perform various actions. To standardize the missions used for RMF, users have the option to automatically create these missions required by RMF via the fleet adapter by parsing in the JSON filepath. This section lists and explains what these missions do. + +Missions may be used for default behavior, which mostly involves the robot patrolling across the same or different maps. Additional missions can be optionally created for custom actions, such as pickup and dropoff missions for delivery tasks. + +For more information on how to set up missions, actions and variables, please refer to [official guide documents](https://www.manualslib.com/manual/1941073/Mir-Mir250.html?page=150#manual) (this is for the MiR250). + + +### List of standardized RMF missions + +* [Default RMF missions](#Default-RMF-missions) +* [RMF missions for `rmf_cart_delivery`](#RMF-missions-for-rmf_cart_delivery) + + +### Default RMF missions + +Definition can be found in `rmf_missions.json`. + +| Mission name | Use case | Created by RMF | +| -------------- | ----------- | :--------------: | +| `rmf_dock_and_charge` | Enables the robot to dock into its charger and begin charging | Yes | +| `rmf_localize` | Switches MiR map once the robot reaches another level | Yes | +| `rmf_move` | Commands the robot to move to the MiR coordinates `[X, Y, Orientation]` | Yes | +| `rmf_move_to_position` | Commands the robot to move to the specified MiR Position | Yes | +| `rmf_follow_line` | Commands the robot to move to two specified MiR Positions in sequence | Yes | + + +### RMF missions for `rmf_cart_delivery` + +| Mission name | Use case | Created by RMF | +| -------------- | ----------- | :--------------: | +| `rmf_dock_to_cart` | Enables the robot to dock under a cart | Yes | +| `rmf_exit_lot` | Commands the robot to move backwards by 1 metre, for exiting from under a cart | Yes | +| `rmf_pickup_cart` | Commands the robot open its latch to attach to a cart | No | +| `rmf_dropoff_cart` | Commands the robot unlatch from a cart and move backwards to exit from under the cart | No | + +**Important note:** +In order to ensure that your robot has the correct footprint after pickup/dropoff, do remember to add in the appropriate MiR actions to update robot footprint in your `rmf_pickup_cart` and `rmf_dropoff_cart` missions. diff --git a/fleet_adapter_mir/README.md b/fleet_adapter_mir/README.md index 3a916f6..e92ec49 100644 --- a/fleet_adapter_mir/README.md +++ b/fleet_adapter_mir/README.md @@ -1,27 +1,17 @@ ## Usage -### Pre-defined RMF variable move mission +### Creating MiR missions for RMF -Users are required to define a custom mission with a single `Move` action, which has the variables `x`, `y` and `yaw`, attached to the coordinates `x`, `y` and `yaw` respectively. This is the mission that the fleet adapter will use to send move commands to the robot, while modifying the desired `x`, `y` and `yaw` values. +RMF relies on a number of MiR missions to command the robot to move, perform map switches, and dock into chargers. These missions are pre-defined in `rmf_missions.json` and can be created automatically on the robots at startup. -For more information on how to set up missions, actions and variables, please refer to [official guide documents](https://www.manualslib.com/manual/1941073/Mir-Mir250.html?page=150#manual) (this is for the MiR250). +For more details about the missions needed and created for RMF, refer to the `mir_missions` documentation found [here](https://github.com/open-rmf/fleet_adapter_mir/blob/main/docs/mir_missions.md). It will be helpful to go through the missions mentioned in the section before setting up your building map. -The name of this custom mission, will need to be passed to the fleet adapter through the configuration file, under `rmf_move_mission`, for each robot's `mir_config`. +The next section will demonstrate how to parse this JSON file to the fleet adapter for MiR mission creation. -```yaml -robots: - ROBOT_NAME: - mir_config: - ... - rmf_move_mission: "CUSTOM_MISSION_NAME" - ... -``` ### Example Entry Point -An example usage of the implemented adapter can be found in the `fleet_adapter_mir/fleet_adapter_mir.py` file. It takes in a configuration file and navigation graph, sets everything up, and spins up the fleet adapter. - -Call it like so: +To run the fleet adapter with full capabilities, (it will requirqe an `rmf_traffic_ros2` schedule node to be active, and the MiR REST server to be available): ```bash # Source the workspace @@ -30,28 +20,21 @@ source ~/mir_ws/install/setup.bash # Get help ros2 run fleet_adapter_mir fleet_adapter_mir -h -# Run in dry-run mode. Disables ROS 2 publishing and MiR REST calls +# Spin up the fleet adapter cd ~/mir_ws/src/fleet_adapter_mir/configs -ros2 run fleet_adapter_mir fleet_adapter_mir -c mir_config.yaml -n nav_graph.yaml -d -``` - -Alternatively, if you want to run everything with full capabilities, (though note that it will require an `rmf_traffic_ros2` schedule node to be active, and the MiR REST server to be available) - -```bash ros2 run fleet_adapter_mir fleet_adapter_mir -c mir_config.yaml -n nav_graph.yaml ``` -If you have not configured the necessary RMF missions on the MiR, you may parse the relevant JSON filepaths when launching the fleet adapter node. On startup, the fleet adapter will create these missions via MiR REST API +If you have not configured the necessary RMF missions on the MiR, you may parse in the filepath to `rmf_missions.json` when launching the fleet adapter node. On startup, the fleet adapter will create these missions via MiR REST API. If a mission with the same Mission Group and Name already exists on the robot, the fleet adapter will skip creating it. ```bash ros2 run fleet_adapter_mir fleet_adapter_mir -c mir_config.yaml -n nav_graph.yaml -a ../missions/rmf_missions.json ``` - ### Configuration -An example configuration file, `mir_config.yaml` has been provided. It has been generously commented, and in the cases where it has not, the parameter names are self-explanatory enough. +An example configuration file, `mir_config.yaml` has been provided. It has been generously commented, and in the cases where it has not, the parameter names are meant to be self-explanatory. @@ -63,47 +46,34 @@ To use the `rmf_dock_and_charge` mission for charging, use the traffic-editor to ```json {"description": {"end_waypoint": "charger_name"}, "mission_name": "rmf_dock_and_charge"} ``` -Where you replace `end_waypoint` with the name of your MiR charger. +Where you replace `charger_name` with the name of your MiR charger. -**MiR positions** +**MiR Positions** -Upon launch, the MiR fleet adapter recognizes MiR positions with identical names to RMF waypoints to be the same location. Hence, when a navigation command is submitted for the robot to a specific waypoint, if this waypoint name also exists as a robot position on the MiR, the fleet adapter would send it directly to the MiR position even if the coordinates are different. +Upon launch, the MiR fleet adapter recognizes MiR Positions with identical names to RMF waypoints to be the same location. Hence, when a navigation command is submitted for the robot to a specific waypoint, if this waypoint name also exists as a robot position on the MiR, the fleet adapter would send it directly to the MiR Position even if the coordinates are different. +Integrators should assume responsibility of making sure that the MiR Positions created on the robots and RMF waypoints with identical names are located reasonably close to one another. -### Plugins -The MiR is capable of performing various types of custom missions and tasks. You can now easily set up plugins offered in this repo instead of writing your own perform action for common use cases. These plugins offered are available under the `fleet_adapter_mir_actions` package. +**Guided MiR movement** -For cart deliveries from point A to B: +Robot travel through tight spaces can be especially tricky. Examples include lift entry, lift exit, or any situation where you might want the robot to move in a straight line over a short distance. One of the default MiR missions created with `rmf_missions.json`, `rmf_follow_line`, helps to ensure that the robot moves in a straight line without the need to create multiple RMF waypoints. -**rmf_cart_delivery** +Using lift entry as an example scenario, you can make use of `rmf_follow_line` this way: +1. Create a MiR robot position right outside the lift, facing inwards. Let's call this MiR Position `Outside_Lift`. +2. Create a MiR robot position inside the lift, facing inwards. This position corresponds to the RMF waypoint inside this lift on this level. Let's call this MiR Position `Inside_Lift`. It helps immensely for both MiR Positions to form a straight line with their `Orientation` values facing the same direction as this line. +3. On the traffic-editor, set the `dock_name` property of the lift waypoint to a json description like this: + ```json + {"description": {"start_waypoint": "Outside_Lift", "end_waypoint": "Inside_Lift"}, "mission_name": "rmf_follow_line", "dock_name": "enter_lift"} + ``` +When the robot is called to enter the lift, regardless of where the robot was located before the mission starts, it will first move to `Outside_Lift`, facing directly towards the lift entry position, and then move to `Inside_Lift`. -The `rmf_cart_delivery` plugin allows users to submit pickup and dropoff tasks to MiR integrated with RMF. The workflow of the task is as follows: -1. RMF will send the robot to the pickup lot -2. The robot will attempt to dock under a cart in the pickup lot -3. If the robot successfully docks under the correct cart, it will proceed to deliver it to a dropoff point. If the cart is missing or is not the desired cart, RMF will cancel the task. +With this mission, integrators have a little more control over the robot movement during travel through tight spaces. -Some relevant MiR missions (docking, exit, update footprint) will be automatically created on the MiR on startup. These missions are used to facilitate the pickup and dropoff activities and can be found in the plugin config under `missions`. They are: -- `rmf_dock_to_cart`: Docks robot under the cart -- `rmf_exit_lot`: Calls the robot to exit from under the cart -They are defined and stored in the `rmf_cart_missions.json` file and do not require any further configuration. +### `MirAction` Plugins -However, since there are various types of latching methods available for different MiR models, users will need to set up their custom pickup and dropoff missions on the MiR, as well as implement their own `CartDetection` plugin module with the appropriate APIs to detect latching states. -1. Create 2 missions on the MiR: - - `rmf_pickup_cart`: Triggers the robot's latching module to open - - `rmf_dropoff_cart`: Triggers the robot's latching module to close and release the cart, then exit from under the cart (relative move -1 metre in the X-direction) -2. Fill in the MiR mission names in the plugin config under `missions`. The default mission names are `rmf_pickup_cart` and `rmf_dropoff_cart`. -3. Fill in the footprints and marker types to be used for your specific robot and cart in the plugin config. -4. Create your own `CartDetection` plugin. You may use `rmf_cart_detection.py` as a reference for how your plugin module should be implemented. Fill in the logic to check whether the robot's latching module is open in blanks marked `# IMPLEMENT YOUR CODE HERE`. Some API calls to check the MiR's PLC registers and IO modules are provided in case you may want to use them. -5. In the plugin config, update the `cart_detection_module` field to point to your own written module. +The MiR is capable of performing various types of custom missions and tasks. You can now easily set up plugins offered in this repo instead of writing your own perform action for common use cases. The plugins offered are available under the `fleet_adapter_mir_actions` package. -To submit a cart delivery task, you may use the `dispatch_delivery` task script found in the `fleet_adapter_mir_tasks` package: -```bash -ros2 run fleet_adapter_mir_tasks dispatch_delivery -g go_to_waypoint -p pickup_lot -d dropoff_lot -c some_cart_id -``` -- `-g`: Takes in an existing waypoint name for the robot to travel to before performing the pickup -- `-p`: Name of the pickup lot. This name should be identical to the shelf position configured on the MiR. -- `-d`: Name of the dropoff lot. This name should be identical to the robot or shelf position configured on the MiR. -- `-c`: Optional cart identifier for the fleet adapter to assess whether the cart is correct for pickup. +You may refer to the `mir_actions` documentation found [here](https://github.com/open-rmf/fleet_adapter_mir/blob/main/docs/mir_actions.md) for a list of action plugins offered and how to set them up. diff --git a/missions/rmf_cart_missions.json b/missions/rmf_cart_missions.json index 978bf71..602913d 100644 --- a/missions/rmf_cart_missions.json +++ b/missions/rmf_cart_missions.json @@ -1,4 +1,32 @@ { + "rmf_dock_to_cart": [ + { + "priority": 1, + "parameters": [ + { + "value": null, + "input_name": "cart_marker", + "id": "marker" + }, + { + "value": "some_placeholder_marker_type_guid", + "input_name": "cart_marker_type", + "id": "marker_type" + }, + { + "value": "10", + "input_name": null, + "id": "retries" + }, + { + "value": 0.25, + "input_name": null, + "id": "max_linear_speed" + } + ], + "action_type": "docking" + } + ], "rmf_exit_lot": [ { "priority": 1, @@ -36,33 +64,5 @@ ], "action_type": "relative_move" } - ], - "rmf_dock_to_cart": [ - { - "priority": 1, - "parameters": [ - { - "value": null, - "input_name": "cart_marker", - "id": "marker" - }, - { - "value": "some_placeholder_marker_type_guid", - "input_name": "cart_marker_type", - "id": "marker_type" - }, - { - "value": "10", - "input_name": null, - "id": "retries" - }, - { - "value": 0.25, - "input_name": null, - "id": "max_linear_speed" - } - ], - "action_type": "docking" - } ] } \ No newline at end of file From 9610e9788b5ee3f9caca31d1b69b2b3ef53235a3 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 8 Oct 2024 09:15:37 +0000 Subject: [PATCH 42/50] Accept new action if there's an ongoing action Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index bde9c35..3993ad8 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -782,11 +782,13 @@ def request_localize(self, estimate, mission: MissionHandle): def perform_action(self, category, description, execution): if self.current_action: # Should not reach here, but we log an error anyway - self.node.get_logger().info( - f'Robot [{self.name}] is busy with another perform action! ' - f'Ignoring new action [{category}]') - execution.finished() - return + self.node.get_logger().error( + f'Robot [{self.name}] received a new action while it is busy ' + f'with another perform action! Ending current action and ' + f'accepting incoming action [{category}]') + if self.current_action.context.execution is not None: + self.current_action.context.execution.finished() + self.current_action = None for _, action_factory in self.action_factories.items(): if category in action_factory.actions: From 99491fe57bf1e72df80a65c08ca42b30f91afeae Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 8 Oct 2024 09:46:36 +0000 Subject: [PATCH 43/50] Fix sample config Signed-off-by: Xiyu Oh --- configs/mir_config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml index 6beada3..4bb78b7 100644 --- a/configs/mir_config.yaml +++ b/configs/mir_config.yaml @@ -80,7 +80,7 @@ rmf_fleet: delivery: True clean: False finishing_request: "charge" # [park, charge, nothing] - actions: ["rmf_cart_delivery", "delivery_pickup", "delivery_dropoff"] + actions: ["delivery_pickup", "delivery_dropoff"] robots: mir_1: charger: "Charger_1" @@ -97,7 +97,7 @@ rmf_fleet: plugins: rmf_cart_delivery: module: "fleet_adapter_mir_actions.rmf_cart_delivery" - actions: ["rmf_cart_delivery", "delivery_pickup", "delivery_dropoff"] + actions: ["delivery_pickup", "delivery_dropoff"] cart_detection_module: "fleet_adapter_mir_actions.rmf_cart_detection" missions: dock_to_cart: "rmf_dock_to_cart" # Name of MiR mission for robot to dock under a cart - to be created by fleet adapter upon launch From 2530e297a423d5a74e4f033f8bf8cf7a877c8c56 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 9 Oct 2024 09:25:31 +0000 Subject: [PATCH 44/50] Robot adapter to be responsible for marking execution as finished Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 9 +++-- .../fleet_adapter_mir_actions/mir_action.py | 3 +- .../rmf_cart_delivery.py | 33 ++++++------------- 3 files changed, 18 insertions(+), 27 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 3993ad8..c5a4633 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -251,11 +251,14 @@ def request_update(self): # PerformAction related checks if self.current_action: if self.current_action.update_action(): - # This means that the action has ended, we can clear the - # current action object + # This means that the action has ended, we can mark the + # ActionExecution as finished and clear the current action + # object self.node.get_logger().info( f'Robot [{self.name}] has completed its current ' - f'action.') + f'action, marking execution as finished.') + if self.current_action.execution is not None: + self.current_action.execution.finished() self.current_action = None # Clear error on updates diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 2c00368..411b748 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -19,8 +19,9 @@ class MirAction(ABC): - def __init__(self, context: ActionContext): + def __init__(self, context: ActionContext, execution): self.context = context + self.execution = execution ''' This method is called on every update by the robot adapter to monitor the diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index 81c99d6..cf55b53 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -146,7 +146,7 @@ def __init__( context: ActionContext, cart_detection ): - MirAction.__init__(self, context) + MirAction.__init__(self, context, execution) # Mission names to be used during pickup self.dock_to_cart_mission = \ @@ -156,7 +156,6 @@ def __init__( self.exit_mission = \ context.action_config['missions']['exit_lot'] - self.execution = execution self.cart_detection = cart_detection self.search_timeout = \ context.action_config.get('search_timeout', 60) # seconds @@ -187,17 +186,15 @@ def __init__( ) def update_action(self): - if self.update_pickup(self.pickup): - if self.execution is not None: - self.execution.finished() - return True - return False + return self.update_pickup(self.pickup) def update_pickup(self, pickup: Pickup): - # Check that the action is underway and valid + # If the action is no longer active, perform cleanup by updating + # PickupState to TASK_CANCELLED. if self.execution is not None and not self.execution.okay(): self.context.node.get_logger().info( - f'[delivery_pickup] action is killed/canceled.') + f'[delivery_pickup] action is no longer underway and valid, ' + f'setting PickupState to TASK_CANCELLED.') pickup.state = PickupState.TASK_CANCELLED # Start state machine check @@ -406,9 +403,8 @@ def __init__( context: ActionContext, cart_detection ): - MirAction.__init__(self, context) + MirAction.__init__(self, context, execution) - self.execution = execution self.cart_detection = cart_detection self.dropoff_mission = \ context.action_config['missions']['dropoff'] @@ -433,22 +429,13 @@ def __init__( self.dropoff = Dropoff(mission=None) def update_action(self): - if self.update_dropoff(self.dropoff): - if self.execution is not None: - self.execution.finished() - return True - return False + return self.update_dropoff(self.dropoff) def update_dropoff(self, dropoff: Dropoff): - # Check that the action is underway and valid. No action if action has - # been killed or cancelled, as we want to ensure that the robot is free - # of carts. + # If action is no longer active, log for awareness. No cleanup required if self.execution is not None and not self.execution.okay(): self.context.node.get_logger().info( - f'[delivery_dropoff] action is killed/canceled! Proceeding to ' - f'queue cart dropoff mission to ensure that robot has fully ' - f'released any attached cart and is safe to perform ' - f'subsequent tasks.') + f'[delivery_dropoff] action is no longer underway and valid!') # If skip_dropoff flag is raised, check if the robot is under a cart # and queue exit lot mission accordingly From b87a704f7d62557aa4ee6ff6e2b5ffb74dac0462 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 13 Nov 2024 12:01:15 +0800 Subject: [PATCH 45/50] Protect MirAction task cancellation from race conditions Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir_actions/mir_action.py | 19 ++++++++++--------- .../rmf_cart_delivery.py | 2 +- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 411b748..0c03a89 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -22,6 +22,7 @@ class MirAction(ABC): def __init__(self, context: ActionContext, execution): self.context = context self.execution = execution + self.action_task_id = self.context.update_handle.more().current_task_id() ''' This method is called on every update by the robot adapter to monitor the @@ -36,28 +37,28 @@ def update_action(self): ''' This method may be used to cancel the current ongoing task. ''' - def cancel_current_task(self, - cancel_success: Callable[[], None], - cancel_fail: Callable[[], None], - label: str = ''): - current_task_id = self.context.update_handle.more().current_task_id() + def cancel_task_of_action( + self, + cancel_success: Callable[[], None], + cancel_fail: Callable[[], None], + label: str = ''): self.context.node.get_logger().info( f'[{self.context.name}] Cancel task requested for ' - f'[{current_task_id}]') + f'[{self.action_task_id}]') def _on_cancel(result: bool): if result: self.context.node.get_logger().info( - f'[{self.context.name}] Found task [{current_task_id}], ' + f'[{self.context.name}] Found task [{self.action_task_id}], ' f'cancelling...') cancel_success() else: self.context.node.get_logger().info( f'[{self.context.name}] Failed to cancel task ' - f'[{current_task_id}]') + f'[{self.action_task_id}]') cancel_fail() self.context.update_handle.more().cancel_task( - current_task_id, [label], lambda result: _on_cancel(result)) + self.action_task_id, [label], lambda result: _on_cancel(result)) class MirActionFactory(ABC): diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index cf55b53..9b7eb0f 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -392,7 +392,7 @@ def _cancel_success(): def _cancel_fail(): pass - self.cancel_current_task(_cancel_success, _cancel_fail, label) + self.cancel_task_of_action(_cancel_success, _cancel_fail, label) class CartDropoff(MirAction): From 49724d88bc58dcfa864733867062d30aee7fc8a7 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 13 Nov 2024 12:35:04 +0800 Subject: [PATCH 46/50] Add performable actions for plugins Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/fleet_adapter_mir.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py index 58e4cfb..e6bab57 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py @@ -29,6 +29,7 @@ import rmf_adapter import rmf_adapter.easy_full_control as rmf_easy +import rmf_adapter.fleet_update_handle as rmf_fleet from rmf_adapter import Transformation from .robot_adapter_mir import RobotAdapterMiR @@ -174,6 +175,23 @@ def create_fleet( update_frequency = config_yaml['rmf_fleet']['robot_state_update_frequency'] debug = config_yaml['rmf_fleet']['debug'] plugin_config = config_yaml.get('plugins') + # Add all plugin actions to the fleet + def _consider(description, confirm: rmf_fleet.Confirmation): + confirm.accept() + for plugin_name, plugin_data in plugin_config.items(): + plugin_actions = plugin_data.get('actions') + if not plugin_actions: + cmd_node.get_logger().warn( + f'No action provided for plugin [{plugin_name}]! Fleet ' + f'[{fleet_handle.fleet_name}] will not bid on tasks submitted ' + f'with actions associated with this plugin unless the action ' + f'is registered as a performable action for this fleet by ' + f'the user.' + ) + continue + for action in plugin_actions: + fleet_handle.add_performable_action(action, _consider) + event_loop = asyncio.new_event_loop() From 8239b87f58dbc0edad472705c1b8df7ecff89014 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 13 Nov 2024 13:46:25 +0800 Subject: [PATCH 47/50] Map action name to plugin name Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 28 +++++++++++++++++-- .../fleet_adapter_mir_actions/mir_action.py | 6 ++++ 2 files changed, 31 insertions(+), 3 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index c5a4633..41cc6dd 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -159,7 +159,8 @@ def __init__( self.current_action = None # Import and store plugin actions and action factories - self.action_factories = {} + self.action_to_plugin_name = {} # Maps action name to plugin name + self.action_factories = {} # Maps plugin name to action factory for plugin_name, action_config in plugin_config.items(): try: module = action_config['module'] @@ -169,6 +170,24 @@ def __init__( self.fleet_config, action_config ) action_factory = plugin.ActionFactory(action_context) + for action in action_factory.actions: + # Verify that this action is not duplicated across plugins + target_plugin = self.action_to_plugin_name.get(action) + if (target_plugin is not None and + target_plugin != plugin_name): + raise Exception( + f'Action [{action}] is indicated to be supported ' + f'by multiple plugins: {target_plugin} and ' + f'{plugin_name}. The fleet adapter is unable to ' + f'select the intended plugin to be paired for ' + f'this action. Please ensure that action names ' + f'are not duplicated across supported plugins. ' + f'Unable to create ActionFactory for ' + f'{plugin_name}. Robot [{self.name}] will not be ' + f'able to perform actions associated with this ' + f'plugin.' + ) + self.action_to_plugin_name[action] = plugin_name self.action_factories[plugin_name] = action_factory except KeyError: self.node.get_logger().info( @@ -793,8 +812,11 @@ def perform_action(self, category, description, execution): self.current_action.context.execution.finished() self.current_action = None - for _, action_factory in self.action_factories.items(): - if category in action_factory.actions: + plugin_name = self.action_to_plugin_name.get(category) + if plugin_name: + action_factory = self.action_factories.get(plugin_name) + if action_factory: + # Valid action-plugin pair exists, create MirAction object action_obj = action_factory.perform_action( category, description, execution ) diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 0c03a89..03a6d61 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -69,6 +69,12 @@ def __init__(self, context: ActionContext): raise KeyError( f'List of supported actions is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') + elif not context.action_config['actions']: + raise Exception( + f'An empty list was provided to the action config! Please ' + f'fill in the list with support actions for this plugin.' + f'Unable to instantiate an ActionFactory.' + ) self.actions = context.action_config['actions'] # Create required MiR missions on the robot From a7aac274df3e771111d5dc58ed3dc87a22fc8d58 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 13 Nov 2024 14:02:41 +0800 Subject: [PATCH 48/50] Do supports_action() check right after loading the action factory Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 7 +++++++ .../fleet_adapter_mir_actions/rmf_cart_delivery.py | 10 ---------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 41cc6dd..6237837 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -187,6 +187,13 @@ def __init__( f'able to perform actions associated with this ' f'plugin.' ) + # Verify that this ActionFactory supports this action + if not action_factory.supports_action(action): + raise ValueError( + f'The plugin config provided [{action}] as a ' + f'performable action, but it is not a supported ' + f'action in the {plugin_name} ActionFactory!' + ) self.action_to_plugin_name[action] = plugin_name self.action_factories[plugin_name] = action_factory except KeyError: diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py index 9b7eb0f..1b15a35 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_cart_delivery.py @@ -104,16 +104,6 @@ def __init__(self, context: ActionContext): detection_plugin = importlib.import_module(detection_module) self.cart_detection = detection_plugin.CartDetection(self.context) - # Verify that this ActionFactory supports all the actions specified in - # the action config - for action in self.actions: - if not self.supports_action(action): - raise ValueError( - f'The plugin config provided [{action}] as a performable ' - f'action, but it is not a supported action in the ' - f'rmf_cart_delivery ActionFactory!' - ) - def supports_action(self, category: str) -> bool: match category: case 'delivery_pickup': From 45ac09adba01be6752192c903f739067916aae61 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 13 Nov 2024 15:00:42 +0800 Subject: [PATCH 49/50] Iterate through factories to try matching unconfigured actions Signed-off-by: Xiyu Oh --- .../fleet_adapter_mir/robot_adapter_mir.py | 22 +++++++++++++------ .../fleet_adapter_mir_actions/mir_action.py | 6 ----- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index 6237837..de05a14 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -819,16 +819,24 @@ def perform_action(self, category, description, execution): self.current_action.context.execution.finished() self.current_action = None + action_factory = None plugin_name = self.action_to_plugin_name.get(category) if plugin_name: action_factory = self.action_factories.get(plugin_name) - if action_factory: - # Valid action-plugin pair exists, create MirAction object - action_obj = action_factory.perform_action( - category, description, execution - ) - self.current_action = action_obj - return + else: + for plugin, factory in self.action_factories.items(): + if factory.supports_action(category): + self.action_to_plugin_name[category] = plugin + action_factory = factory + break + + if action_factory: + # Valid action-plugin pair exists, create MirAction object + action_obj = action_factory.perform_action( + category, description, execution + ) + self.current_action = action_obj + return # No relevant perform action found self.node.get_logger().info( diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py index 03a6d61..0c03a89 100644 --- a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py +++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/mir_action.py @@ -69,12 +69,6 @@ def __init__(self, context: ActionContext): raise KeyError( f'List of supported actions is not provided in the action ' f'config! Unable to instantiate an ActionFactory.') - elif not context.action_config['actions']: - raise Exception( - f'An empty list was provided to the action config! Please ' - f'fill in the list with support actions for this plugin.' - f'Unable to instantiate an ActionFactory.' - ) self.actions = context.action_config['actions'] # Create required MiR missions on the robot From 3045bcc78c388de90caf43901ba4c5ea60b9c7f4 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 13 Nov 2024 15:17:53 +0800 Subject: [PATCH 50/50] Append action to ActionFactory Signed-off-by: Xiyu Oh --- fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py | 1 + 1 file changed, 1 insertion(+) diff --git a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py index de05a14..c2cf37e 100644 --- a/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py +++ b/fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py @@ -827,6 +827,7 @@ def perform_action(self, category, description, execution): for plugin, factory in self.action_factories.items(): if factory.supports_action(category): self.action_to_plugin_name[category] = plugin + factory.actions.append(category) action_factory = factory break