From d17c57a5f3e52fcb4d75cb8490f24c026c3f6cca Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Wed, 21 Sep 2022 15:04:25 +0800 Subject: [PATCH 01/14] Fix teleop action Signed-off-by: Xi Yu Oh --- .../rmf_demos_fleet_adapter/fleet_manager.py | 14 +++++++++++--- rmf_demos_tasks/rmf_demos_tasks/teleop_robot.py | 3 ++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py index c3c2a4ed..77be0a86 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py @@ -21,6 +21,7 @@ import time import copy import argparse +import uuid import rclpy from rclpy.node import Node @@ -323,8 +324,15 @@ def robot_state_cb(self, msg): if (msg.name in self.robots): robot = self.robots[msg.name] if not robot.is_expected_task_id(msg.task_id): - # This message is out of date, so disregard it. - if robot.last_path_request is not None: + # This message is out of date. Check if task_id is a valid + # uuid, as the robot may be carrying out a teleop action. + try: + action_id = uuid.UUID(msg.task_id) + except ValueError: + return + if robot.last_path_request is not None and \ + (robot.last_completed_request != + int(robot.last_path_request.task_id)): # Resend the latest task request for this robot, in case # the message was dropped. if self.debug: @@ -334,7 +342,7 @@ def robot_state_cb(self, msg): f'because it is currently following {msg.task_id}' ) self.path_pub.publish(robot.last_path_request) - return + return robot.state = msg # Check if robot has reached destination diff --git a/rmf_demos_tasks/rmf_demos_tasks/teleop_robot.py b/rmf_demos_tasks/rmf_demos_tasks/teleop_robot.py index a265a447..cfac0ca2 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/teleop_robot.py +++ b/rmf_demos_tasks/rmf_demos_tasks/teleop_robot.py @@ -48,7 +48,8 @@ def __init__(self, argv=sys.argv): self.args = parser.parse_args(argv[1:]) self.pub = self.create_publisher( - PathRequest, 'robot_path_requests', 1) + PathRequest, 'robot_path_requests', + qos_profile=qos_profile_system_default) msg = PathRequest() msg.fleet_name = self.args.fleet From 7483dd5aae7df7137aea5f86c112caed6d165e5f Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Thu, 22 Sep 2022 16:46:52 +0800 Subject: [PATCH 02/14] Add toggle action endpoint Signed-off-by: Xi Yu Oh --- .../rmf_demos_fleet_adapter/RobotClientAPI.py | 18 +++++++++++ .../RobotCommandHandle.py | 6 ++++ .../rmf_demos_fleet_adapter/fleet_manager.py | 32 +++++++++++-------- 3 files changed, 43 insertions(+), 13 deletions(-) diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py index d8e27505..ae0e21a9 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py @@ -187,6 +187,24 @@ def requires_replan(self, robot_name: str): return response['data'].get('replan', False) return False + def toggle_action(self, robot_name: str, toggle: bool): + '''Request to toggle the robot's mode_teleop parameter. + Return True if the toggle request is successful''' + url = self.prefix +\ + f"/open-rmf/rmf_demos_fm/toggle_action?robot_name={robot_name}" + data = {'toggle': toggle} + try: + response = requests.post(url, timeout=self.timeout, json=data) + response.raise_for_status() + if self.debug: + print(f'Response: {response.json()}') + return response.json()['success'] + except HTTPError as http_err: + print(f'HTTP error: {http_err}') + except Exception as err: + print(f'Other error: {err}') + return False + def data(self, robot_name=None): if robot_name is None: url = self.prefix + f'/open-rmf/rmf_demos_fm/status/' diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py index feb70ea0..1dde2f43 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py @@ -119,6 +119,7 @@ def __init__(self, # The graph index of the waypoint the robot starts or ends an action self.action_waypoint_index = None self.current_cmd_id = 0 + self.started_action = False # Threading variables self._lock = threading.Lock() @@ -539,6 +540,9 @@ def update_state(self): self.position, self.dock_waypoint_index) # if robot is performing an action elif (self.action_execution is not None): + if not self.started_action: + self.started_action = True + self.api.toggle_action(self.name, self.started_action) self.update_handle.update_off_grid_position( self.position, self.action_waypoint_index) # if robot is merging into a waypoint @@ -662,6 +666,8 @@ def complete_robot_action(self): return self.action_execution.finished() self.action_execution = None + self.started_action = False + self.api.toggle_action(self.name, self.started_action) self.node.get_logger().info(f"Robot {self.name} has completed the" f" action it was performing") diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py index 77be0a86..dcf58847 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py @@ -21,7 +21,6 @@ import time import copy import argparse -import uuid import rclpy from rclpy.node import Node @@ -54,11 +53,12 @@ class Request(BaseModel): - map_name: str + map_name: Optional[str] = None task: Optional[str] = None destination: Optional[dict] = None data: Optional[dict] = None speed_limit: Optional[float] = None + toggle: Optional[bool] = None class Response(BaseModel): @@ -76,6 +76,7 @@ def __init__(self, state: RobotState = None, destination: Location = None): self.destination = destination self.last_path_request = None self.last_completed_request = None + self.mode_teleop = False self.svy_transformer = Transformer.from_crs('EPSG:4326', 'EPSG:3414') self.gps_pos = [0, 0] @@ -320,19 +321,24 @@ async def start_process(robot_name: str, cmd_id: int, task: Request): response['success'] = True return response + @app.post('/open-rmf/rmf_demos_fm/toggle_action/', + response_model=Response) + async def toggle_teleop(robot_name: str, mode: Request): + response = {'success': False, 'msg': ''} + if (robot_name not in self.robots): + return response + # Toggle action mode + self.robots[robot_name].mode_teleop = mode.toggle + response['success'] = True + return response + def robot_state_cb(self, msg): if (msg.name in self.robots): robot = self.robots[msg.name] - if not robot.is_expected_task_id(msg.task_id): - # This message is out of date. Check if task_id is a valid - # uuid, as the robot may be carrying out a teleop action. - try: - action_id = uuid.UUID(msg.task_id) - except ValueError: - return - if robot.last_path_request is not None and \ - (robot.last_completed_request != - int(robot.last_path_request.task_id)): + if not robot.is_expected_task_id(msg.task_id) and \ + not robot.mode_teleop: + # This message is out of date, so disregard it. + if robot.last_path_request is not None: # Resend the latest task request for this robot, in case # the message was dropped. if self.debug: @@ -342,7 +348,7 @@ def robot_state_cb(self, msg): f'because it is currently following {msg.task_id}' ) self.path_pub.publish(robot.last_path_request) - return + return robot.state = msg # Check if robot has reached destination From 50d7b287128f9ed1089d24232cc972a97af9ab61 Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Fri, 23 Sep 2022 15:52:04 +0800 Subject: [PATCH 03/14] Add clean action support in fleet adapter Signed-off-by: Xi Yu Oh --- .../RobotCommandHandle.py | 28 +++++++++++++++++++ .../rmf_demos_fleet_adapter/fleet_adapter.py | 14 ++++++++-- 2 files changed, 39 insertions(+), 3 deletions(-) diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py index 1dde2f43..5b0c2ba2 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py @@ -120,6 +120,7 @@ def __init__(self, self.action_waypoint_index = None self.current_cmd_id = 0 self.started_action = False + self.clean_zone = None # Threading variables self._lock = threading.Lock() @@ -129,6 +130,8 @@ def __init__(self, self._quit_dock_event = threading.Event() self._stopping_thread = None self._quit_stopping_event = threading.Event() + self._action_thread = None + self._quit_action_event = threading.Event() self.node.get_logger().info( f"The robot is starting at: [{self.position[0]:.2f}, " @@ -476,6 +479,26 @@ def _dock(): self._dock_thread = threading.Thread(target=_dock) self._dock_thread.start() + def perform_clean_action(self): + cmd_id = self.next_cmd_id() + self.started_action = True + + def _perform_clean(): + self.api.start_process( + self.name, cmd_id, self.clean_zone, self.map_name) + + while not self.api.process_completed(self.name, cmd_id): + if self.action_execution is None: + break + + if self._quit_action_event.wait(0.1): + self.node.get_logger().info("Aborting action") + return + + self._action_thread = threading.Thread(target=_perform_clean) + self._action_thread.start() + return + def get_position(self): ''' This helper function returns the live position of the robot in the RMF coordinate frame''' @@ -543,6 +566,8 @@ def update_state(self): if not self.started_action: self.started_action = True self.api.toggle_action(self.name, self.started_action) + if self.clean_zone is not None: + self.perform_clean_action() self.update_handle.update_off_grid_position( self.position, self.action_waypoint_index) # if robot is merging into a waypoint @@ -668,6 +693,9 @@ def complete_robot_action(self): self.action_execution = None self.started_action = False self.api.toggle_action(self.name, self.started_action) + if self._action_thread is not None: + if self._action_thread.is_alive(): + self._action_thread.join() self.node.get_logger().info(f"Robot {self.name} has completed the" f" action it was performing") diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py index 5c1bdaa3..37a89124 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py @@ -159,7 +159,9 @@ def _consider(description: dict): return confirm # Configure this fleet to perform any kind of teleop action - fleet_handle.add_performable_action("teleop", _consider) + if 'action' in fleet_config['task_capabilities']: + for action in fleet_config['task_capabilities']['action']: + fleet_handle.add_performable_action(action, _consider) def _updater_inserter(cmd_handle, update_handle): """Insert a RobotUpdateHandle.""" @@ -170,15 +172,21 @@ def _action_executor(category: str, execution: adpt.robot_update_handle.ActionExecution): with cmd_handle._lock: - if len(description) > 0 and\ + # Check if an action waypoint is provided in the description + # If yes, it will be a string instead of a dict + if type(description) is str and len(description) > 0 and\ description in cmd_handle.graph.keys: cmd_handle.action_waypoint_index = \ - cmd_handle.find_waypoint(description).index + cmd_handle.graph.find_waypoint(description).index else: cmd_handle.action_waypoint_index = \ cmd_handle.last_known_waypoint_index cmd_handle.on_waypoint = None cmd_handle.on_lane = None + # Set the clean zone if this is a cleaning task + if category == 'clean' and \ + type(description) is dict and 'zone' in description: + cmd_handle.clean_zone = description['zone'] cmd_handle.action_execution = execution # Set the action_executioner for the robot cmd_handle.update_handle.set_action_executor(_action_executor) From 9fe068b8807ad6f598217b4f5a3bd2cc8b9e97e1 Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Fri, 23 Sep 2022 15:54:26 +0800 Subject: [PATCH 04/14] Add action to cleanerbot config yamls Signed-off-by: Xi Yu Oh --- rmf_demos/config/airport_terminal/cleanerBotA_config.yaml | 1 + rmf_demos/config/airport_terminal/cleanerBotE_config.yaml | 1 + rmf_demos/config/hotel/cleanerBotA_config.yaml | 1 + 3 files changed, 3 insertions(+) diff --git a/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml b/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml index 952b00ee..8de067e6 100644 --- a/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml +++ b/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml @@ -37,6 +37,7 @@ rmf_fleet: delivery: False clean: True finishing_request: "park" # [park, charge, nothing] + action: ["clean"] # CleanerBotA CONFIG ================================================================= diff --git a/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml b/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml index 158210c0..c4d43d21 100644 --- a/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml +++ b/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml @@ -37,6 +37,7 @@ rmf_fleet: delivery: False clean: True finishing_request: "park" # [park, charge, nothing] + action: ["clean"] # CleanerBotE CONFIG ================================================================= diff --git a/rmf_demos/config/hotel/cleanerBotA_config.yaml b/rmf_demos/config/hotel/cleanerBotA_config.yaml index ee5655c0..9f4b4531 100644 --- a/rmf_demos/config/hotel/cleanerBotA_config.yaml +++ b/rmf_demos/config/hotel/cleanerBotA_config.yaml @@ -37,6 +37,7 @@ rmf_fleet: delivery: False clean: True finishing_request: "park" # [park, charge, nothing] + action: ["clean"] # CleanerBotA CONFIG ================================================================= From e726efc744c64f2ef2931e20505a41720a8d73fd Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Fri, 23 Sep 2022 16:05:08 +0800 Subject: [PATCH 05/14] Support multiple action_desc Signed-off-by: Xi Yu Oh --- .../rmf_demos_tasks/dispatch_action.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py index 93368658..addc1664 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py @@ -50,7 +50,7 @@ def __init__(self, argv=sys.argv): parser.add_argument('-a', '--action', required=True, type=str, help='action name') parser.add_argument('-ad', '--action_desc', required=False, - default='{}', + default='{}', nargs='+', type=str, help='action description in json str') parser.add_argument('-F', '--fleet', type=str, help='Fleet name, should define tgt with robot') @@ -110,7 +110,11 @@ def __init__(self, argv=sys.argv): description["phases"] = [] activities = [] - def _add_action(): + def _add_action(i=0): + try: + action_desc = json.loads(self.args.action_desc[i]) + except ValueError as e: + action_desc = self.args.action_desc[i] activities.append( { "category": "perform_action", @@ -118,7 +122,7 @@ def _add_action(): { "unix_millis_action_duration_estimate": 60000, "category": self.args.action, - "description": json.loads(self.args.action_desc), + "description": action_desc, "use_tool_sink": self.args.use_tool_sink } }) @@ -127,12 +131,12 @@ def _add_action(): _add_action() else: # Add action activities - for start in self.args.starts: + for i in range(len(self.args.starts)): activities.append({ "category": "go_to_place", - "description": start + "description": self.args.starts[i] }) - _add_action() + _add_action(i) # Add activities to phases description["phases"].append({ From f749c7d01903b8cfb594015967fcb2e5b2fccd49 Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Fri, 23 Sep 2022 16:08:42 +0800 Subject: [PATCH 06/14] Remove dock_name from hotel and airport building yamls Signed-off-by: Xi Yu Oh --- .../maps/airport_terminal/airport_terminal.building.yaml | 8 ++++---- rmf_demos_maps/maps/hotel/hotel.building.yaml | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rmf_demos_maps/maps/airport_terminal/airport_terminal.building.yaml b/rmf_demos_maps/maps/airport_terminal/airport_terminal.building.yaml index 8551dde4..1ec5d498 100644 --- a/rmf_demos_maps/maps/airport_terminal/airport_terminal.building.yaml +++ b/rmf_demos_maps/maps/airport_terminal/airport_terminal.building.yaml @@ -1978,22 +1978,22 @@ levels: - [1570.707, 393.481, 0, ""] - [1534.155, 328.233, 0, ""] - [1477.21, 325.884, 0, ""] - - [1477.21, 344.67, 0, zone_1, {dock_name: [1, zone_1], is_cleaning_zone: [4, true]}] + - [1477.21, 344.67, 0, zone_1, {is_cleaning_zone: [4, true]}] - [1863.099, 387.726, 0, ""] - [1880.72, 359.38, 0, ""] - - [1880.59, 335.992, 0, zone_3, {dock_name: [1, zone_3], is_cleaning_zone: [4, true]}] + - [1880.59, 335.992, 0, zone_3, {is_cleaning_zone: [4, true]}] - [1792.451, 389.216, 0, ""] - [1818.61, 634.908, 0, ""] - [1850.262, 635.345, 0, ""] - [1850.315, 677.033, 0, ""] - [1813.811, 355.792, 0, ""] - - [1813.842, 330.119, 0, zone_2, {dock_name: [1, zone_2], is_cleaning_zone: [4, true]}] + - [1813.842, 330.119, 0, zone_2, {is_cleaning_zone: [4, true]}] - [1914.686, 675.55, 0, ""] - [1944.653, 676.5700000000001, 0, charger_cleanerBotE_0, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, cleanerBotE_0], spawn_robot_type: [1, CleanerBotE]}] - [1944.648, 645.399, 0, ""] - [1990.269, 645.721, 0, ""] - [1990.009, 677.358, 0, charger_cleanerBotE_1, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, cleanerBotE_1], spawn_robot_type: [1, CleanerBotE]}] - - [1850.375, 734.322, 0, zone_4, {dock_name: [1, zone_4], is_cleaning_zone: [4, true]}] + - [1850.375, 734.322, 0, zone_4, {is_cleaning_zone: [4, true]}] - [1429.736, 602.524, 0, charger_cleanerBotA_0, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, cleanerBotA_0], spawn_robot_type: [1, CleanerBotA]}] - [2068.856, 362.446, 0, ""] - [2107.685, 321.598, 0, ""] diff --git a/rmf_demos_maps/maps/hotel/hotel.building.yaml b/rmf_demos_maps/maps/hotel/hotel.building.yaml index a0b3689c..421f94a9 100644 --- a/rmf_demos_maps/maps/hotel/hotel.building.yaml +++ b/rmf_demos_maps/maps/hotel/hotel.building.yaml @@ -233,16 +233,16 @@ levels: - [269.855, 244.427, 0, ""] - [505.917, 580.269, 0, ""] - [449.7, 584.276, 0, ""] - - [428.186, 637.526, 0, clean_lobby, {dock_name: [1, clean_lobby], is_cleaning_zone: [4, true]}] + - [428.186, 637.526, 0, clean_lobby, {is_cleaning_zone: [4, true]}] - [247.245, 598.705, 0, ""] - [216.974, 446.636, 0, ""] - [205.957, 391.294, 0, ""] - - [153.759, 417.897, 0, clean_waiting_area, {dock_name: [1, clean_waiting_area], is_cleaning_zone: [4, true]}] + - [153.759, 417.897, 0, clean_waiting_area, {is_cleaning_zone: [4, true]}] - [525.131, 496.822, 0, cleanerbot_charger2, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, cleanerBotA_2], spawn_robot_type: [1, CleanerBotA]}] - [473.507, 498.428, 0, cleanerbot_charger1, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, cleanerBotA_1], spawn_robot_type: [1, CleanerBotA]}] - [205.148, 321.043, 0, ""] - [254.959, 203.925, 0, ""] - - [230.964, 204.483, 0, clean_restaurant, {dock_name: [1, clean_restaurant], is_cleaning_zone: [4, true]}] + - [230.964, 204.483, 0, clean_restaurant, {is_cleaning_zone: [4, true]}] - [422.801, 492.463, 0, tinybot_charger, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, tinyBot_1], spawn_robot_type: [1, TinyRobot]}] - [255.891, 653.729, 0, ""] - [261.443, 700.003, 0, deliverybot_charger, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, deliveryBot_1], spawn_robot_type: [1, DeliveryRobot]}] From 60ab522105958c35ccded534170c2e8266a8a08c Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Tue, 27 Sep 2022 00:37:46 +0800 Subject: [PATCH 07/14] Enable auto-end for cleaning tasks Signed-off-by: Xi Yu Oh --- .../rmf_demos_fleet_adapter/RobotCommandHandle.py | 4 ++++ .../rmf_demos_fleet_adapter/fleet_adapter.py | 7 +++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py index 5b0c2ba2..78a90c0a 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py @@ -121,6 +121,7 @@ def __init__(self, self.current_cmd_id = 0 self.started_action = False self.clean_zone = None + self.auto_end = False # Threading variables self._lock = threading.Lock() @@ -494,6 +495,8 @@ def _perform_clean(): if self._quit_action_event.wait(0.1): self.node.get_logger().info("Aborting action") return + if self.auto_end: + self.complete_robot_action() self._action_thread = threading.Thread(target=_perform_clean) self._action_thread.start() @@ -692,6 +695,7 @@ def complete_robot_action(self): self.action_execution.finished() self.action_execution = None self.started_action = False + self.auto_end = False self.api.toggle_action(self.name, self.started_action) if self._action_thread is not None: if self._action_thread.is_alive(): diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py index 37a89124..63ff18f5 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py @@ -185,8 +185,11 @@ def _action_executor(category: str, cmd_handle.on_lane = None # Set the clean zone if this is a cleaning task if category == 'clean' and \ - type(description) is dict and 'zone' in description: - cmd_handle.clean_zone = description['zone'] + type(description) is dict: + if 'zone' in description: + cmd_handle.clean_zone = description['zone'] + if 'auto_end' in description: + cmd_handle.auto_end = description['auto_end'] cmd_handle.action_execution = execution # Set the action_executioner for the robot cmd_handle.update_handle.set_action_executor(_action_executor) From 3a2770ab69f1ea8c678ca1ac77cca2bc117ce40d Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Tue, 27 Sep 2022 15:46:48 +0800 Subject: [PATCH 08/14] Default auto end for cleaning and cleanup threading items Signed-off-by: Xi Yu Oh --- .../RobotCommandHandle.py | 21 ++++++++++++------- .../rmf_demos_fleet_adapter/fleet_adapter.py | 9 +++----- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py index 78a90c0a..46bdbd7e 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py @@ -121,7 +121,6 @@ def __init__(self, self.current_cmd_id = 0 self.started_action = False self.clean_zone = None - self.auto_end = False # Threading variables self._lock = threading.Lock() @@ -205,6 +204,7 @@ def interrupt(self): self._quit_dock_event.set() self._quit_path_event.set() self._quit_stopping_event.set() + self._quit_action_event.set() if self._follow_path_thread is not None: if self._follow_path_thread.is_alive(): @@ -218,6 +218,10 @@ def interrupt(self): if self._stopping_thread.is_alive(): self._stopping_thread.join() + if self._action_thread is not None: + if self._action_thread.is_alive(): + self._action_thread.join() + def stop(self): if self.debug: plan_id = self.update_handle.unstable_current_plan_id() @@ -482,11 +486,15 @@ def _dock(): def perform_clean_action(self): cmd_id = self.next_cmd_id() + self._quit_action_event.clear() self.started_action = True def _perform_clean(): self.api.start_process( self.name, cmd_id, self.clean_zone, self.map_name) + self.node.get_logger().info( + f'Robot [{self.name}] is starting a new cleaning ' + f'task: {self.clean_zone}') while not self.api.process_completed(self.name, cmd_id): if self.action_execution is None: @@ -495,8 +503,9 @@ def _perform_clean(): if self._quit_action_event.wait(0.1): self.node.get_logger().info("Aborting action") return - if self.auto_end: - self.complete_robot_action() + + # End the clean action once it is completed + self.complete_robot_action() self._action_thread = threading.Thread(target=_perform_clean) self._action_thread.start() @@ -695,11 +704,9 @@ def complete_robot_action(self): self.action_execution.finished() self.action_execution = None self.started_action = False - self.auto_end = False + if self.clean_zone is not None: + self.clean_zone = None self.api.toggle_action(self.name, self.started_action) - if self._action_thread is not None: - if self._action_thread.is_alive(): - self._action_thread.join() self.node.get_logger().info(f"Robot {self.name} has completed the" f" action it was performing") diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py index 63ff18f5..9e3745b7 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py @@ -184,12 +184,9 @@ def _action_executor(category: str, cmd_handle.on_waypoint = None cmd_handle.on_lane = None # Set the clean zone if this is a cleaning task - if category == 'clean' and \ - type(description) is dict: - if 'zone' in description: - cmd_handle.clean_zone = description['zone'] - if 'auto_end' in description: - cmd_handle.auto_end = description['auto_end'] + if category == 'clean' and type(description) is dict and \ + 'clean_task_name' in description: + cmd_handle.clean_zone = description['clean_task_name'] cmd_handle.action_execution = execution # Set the action_executioner for the robot cmd_handle.update_handle.set_action_executor(_action_executor) From 02ee236995492efbad05ac599a6d377a0645362b Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Tue, 27 Sep 2022 16:01:58 +0800 Subject: [PATCH 09/14] Change dispatch_clean to use perform action Signed-off-by: Xi Yu Oh --- .../rmf_demos_tasks/dispatch_clean.py | 34 +++++++++++++++++-- 1 file changed, 31 insertions(+), 3 deletions(-) diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py index d9f1d534..21aaf7dc 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py @@ -93,11 +93,39 @@ def __init__(self, argv=sys.argv): request["unix_millis_earliest_start_time"] = start_time # Define task request category - request["category"] = "clean" + request["category"] = "compose" # Define task request description with cleaning zone - description = {} # task_description_Clean.json - description["zone"] = self.args.clean_start + description = {} # task_description_Compose.json + description["category"] = "clean" + description["phases"] = [] + activities = [] + + # Send robot to start waypoint first + activities.append({ + "category": "go_to_place", + "description": self.args.clean_start + }) + # Send a perform action request for robot to clean area + activities.append({ + "category": "perform_action", + "description": + { + "unix_millis_action_duration_estimate": 60000, + "category": "clean", + "description": + { + "clean_task_name": self.args.clean_start + }, + "use_tool_sink": True + } + }) + + description["phases"].append({ + "activity": { + "category": "sequence", + "description": {"activities": activities}} + }) request["description"] = description payload["request"] = request From 75b5067b14576602060e57031429fe9764a80e82 Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Wed, 28 Sep 2022 19:35:14 +0800 Subject: [PATCH 10/14] Switch from mock docker to cleaning server Signed-off-by: Xi Yu Oh --- rmf_demos/launch/airport_terminal.launch.xml | 6 +- rmf_demos/launch/hotel.launch.xml | 6 +- .../rmf_demos_fleet_adapter/fleet_manager.py | 30 +++++- .../airport_docker_config.yaml | 8 +- .../rmf_demos_tasks/cleaning_server.py | 93 +++++++++++++++++++ .../rmf_demos_tasks/hotel_cleaner_config.yaml | 10 +- rmf_demos_tasks/setup.py | 1 + 7 files changed, 135 insertions(+), 19 deletions(-) create mode 100644 rmf_demos_tasks/rmf_demos_tasks/cleaning_server.py diff --git a/rmf_demos/launch/airport_terminal.launch.xml b/rmf_demos/launch/airport_terminal.launch.xml index 5ab88a87..ef14d5b3 100644 --- a/rmf_demos/launch/airport_terminal.launch.xml +++ b/rmf_demos/launch/airport_terminal.launch.xml @@ -63,10 +63,10 @@ - + - - + + diff --git a/rmf_demos/launch/hotel.launch.xml b/rmf_demos/launch/hotel.launch.xml index d3b59431..086ab400 100644 --- a/rmf_demos/launch/hotel.launch.xml +++ b/rmf_demos/launch/hotel.launch.xml @@ -52,10 +52,10 @@ - + - - + + diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py index dcf58847..92271e9a 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py @@ -32,7 +32,7 @@ from rclpy.qos import QoSReliabilityPolicy as Reliability from rmf_fleet_msgs.msg import RobotState, Location, PathRequest, \ - DockSummary, RobotMode + DockSummary, RobotMode, CleanTaskSummary import rmf_adapter as adpt import rmf_adapter.vehicletraits as traits @@ -112,6 +112,7 @@ def __init__(self, config, nav_path): self.robots = {} # Map robot name to state self.docks = {} # Map dock name to waypoints + self.clean_paths = {} # Map clean tasks to waypoints for robot_name, robot_config in self.config["robots"].items(): self.robots[robot_name] = State() @@ -170,6 +171,12 @@ def message(data): self.dock_summary_cb, qos_profile=transient_qos) + self.create_subscription( + CleanTaskSummary, + 'clean_task_summary', + self.clean_task_summary_cb, + qos_profile=transient_qos) + self.path_pub = self.create_publisher( PathRequest, 'robot_path_requests', @@ -290,7 +297,8 @@ async def start_process(robot_name: str, cmd_id: int, task: Request): response = {'success': False, 'msg': ''} if (robot_name not in self.robots or len(task.task) < 1 or - task.task not in self.docks): + (task.task not in self.docks and + task.task not in self.clean_paths)): return response robot = self.robots[robot_name] @@ -303,7 +311,15 @@ async def start_process(robot_name: str, cmd_id: int, task: Request): previous_wp = [cur_x, cur_y, cur_yaw] target_loc = Location() path_request.path.append(cur_loc) - for wp in self.docks[task.task]: + + if task.task in self.docks: + task_wps = self.docks[task.task] + elif task.task in self.clean_paths: + task_wps = self.clean_paths[task.task] + else: + return response + + for wp in task_wps: target_loc = wp path_request.path.append(target_loc) previous_wp = [wp.x, wp.y, wp.yaw] @@ -375,10 +391,16 @@ def robot_state_cb(self, msg): def dock_summary_cb(self, msg): for fleet in msg.docks: - if(fleet.fleet_name == self.fleet_name): + if (fleet.fleet_name == self.fleet_name): for dock in fleet.params: self.docks[dock.start] = dock.path + def clean_task_summary_cb(self, msg): + for fleet in msg.clean_tasks: + if (fleet.fleet_name == self.fleet_name): + for clean_task in fleet.params: + self.clean_paths[clean_task.name] = clean_task.path + def get_robot_state(self, robot: State, robot_name): data = {} if self.gps: diff --git a/rmf_demos_tasks/rmf_demos_tasks/airport_docker_config.yaml b/rmf_demos_tasks/rmf_demos_tasks/airport_docker_config.yaml index f326e6fa..8e5f199b 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/airport_docker_config.yaml +++ b/rmf_demos_tasks/rmf_demos_tasks/airport_docker_config.yaml @@ -1,9 +1,9 @@ -# The configuration file for docking actions that robots can perform +# The configuration file for cleaning actions that robots can perform # FleetName: The name of the fleet -# DockName: The name of the docking waypoint +# Clean task name: The name of the cleaning task # level_name: The level name -# path: The waypoints that the robot will pass through during the docking process) -# finish_waypoint: The name of the waypoint where the robot will end up at the end of the docking process +# path: The waypoints that the robot will pass through during the cleaning process) +# finish_waypoint: The name of the waypoint where the robot will end up at the end of the cleaning process cleanerBotA: diff --git a/rmf_demos_tasks/rmf_demos_tasks/cleaning_server.py b/rmf_demos_tasks/rmf_demos_tasks/cleaning_server.py new file mode 100644 index 00000000..6f778a7d --- /dev/null +++ b/rmf_demos_tasks/rmf_demos_tasks/cleaning_server.py @@ -0,0 +1,93 @@ +# 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 rclpy +import argparse +import yaml +import time +from rclpy.node import Node + +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_fleet_msgs.msg import CleanTaskSummary, CleanTask, \ + CleanTaskParameter, Location + + +class CleaningServer(Node): + """ + The CleaningServer provides the coordinates for each clean zone + in the respective demo worlds by publishing a CleanTaskSummary + when launched. + """ + + def __init__(self, config_yaml): + super().__init__('cleaning_server') + self.get_logger().info(f'Starting cleaning server...') + self.config_yaml = config_yaml + + transient_qos = QoSProfile( + history=History.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + depth=1, + reliability=Reliability.RMW_QOS_POLICY_RELIABILITY_RELIABLE, + durability=Durability.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) + self.cleaning_server_publisher = self.create_publisher( + CleanTaskSummary, 'clean_task_summary', qos_profile=transient_qos) + + # Populate the cleaning service msg + clean_task_summary = CleanTaskSummary() + for fleet_name, cleaning_info in self.config_yaml.items(): + clean_task = CleanTask() + clean_task.fleet_name = fleet_name + for clean_task_name, clean_waypoints in cleaning_info.items(): + param = CleanTaskParameter() + param.name = clean_task_name + for point in clean_waypoints['path']: + location = Location() + location.x = point[0] + location.y = point[1] + location.yaw = point[2] + location.level_name = clean_waypoints['level_name'] + param.path.append(location) + clean_task.params.append(param) + clean_task_summary.clean_tasks.append(clean_task) + time.sleep(2) + self.cleaning_server_publisher.publish(clean_task_summary) + + +def main(argv=sys.argv): + rclpy.init(args=argv) + args_without_ros = rclpy.utilities.remove_ros_args(argv) + parser = argparse.ArgumentParser( + prog='cleaning_server', + description='Start cleaning service') + parser.add_argument("-c", "--config", type=str, required=True, + help="Path to config file") + args = parser.parse_args(args_without_ros[1:]) + + config = args.config + + with open(config, 'r') as f: + config_yaml = yaml.safe_load(f) + + cleaning_server = CleaningServer(config_yaml) + rclpy.spin(cleaning_server) + rclpy.shutdown() + + +if __name__ == '__main__': + main(sys.argv) diff --git a/rmf_demos_tasks/rmf_demos_tasks/hotel_cleaner_config.yaml b/rmf_demos_tasks/rmf_demos_tasks/hotel_cleaner_config.yaml index 9d14e227..c1fd0de9 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/hotel_cleaner_config.yaml +++ b/rmf_demos_tasks/rmf_demos_tasks/hotel_cleaner_config.yaml @@ -1,14 +1,14 @@ -# The configuration file for docking actions that robots can perform +# The configuration file for cleaning actions that robots can perform # FleetName: The name of the fleet -# DockName: The name of the docking waypoint +# Clean task name: The name of the cleaning task # level_name: The level name -# path: The waypoints that the robot will pass through during the docking process) -# finish_waypoint: The name of the waypoint where the robot will end up at the end of the docking process +# path: The waypoints that the robot will pass through during the cleaning process) +# finish_waypoint: The name of the waypoint where the robot will end up at the end of the cleaning process # # Note: These waypoints are in (x meters, y meters, yaw radian) form # # If loaded correctly, you will be able to echo the msg via: -# ros2 topic echo /dock_summary --qos-durability transient_local --qos-reliability reliable +# ros2 topic echo /clean_task_summary --qos-durability transient_local --qos-reliability reliable # # Configuring Cleaning path: # The full cleaning path which the robot will travel along. When configuring, the first diff --git a/rmf_demos_tasks/setup.py b/rmf_demos_tasks/setup.py index e2014912..784124c6 100644 --- a/rmf_demos_tasks/setup.py +++ b/rmf_demos_tasks/setup.py @@ -27,6 +27,7 @@ 'request_loop = rmf_demos_tasks.request_loop:main', 'request_lift = rmf_demos_tasks.request_lift:main', 'cancel_task = rmf_demos_tasks.cancel_task:main', + 'cleaning_server = rmf_demos_tasks.cleaning_server:main', 'dispatch_loop = rmf_demos_tasks.dispatch_loop:main', 'dispatch_action = rmf_demos_tasks.dispatch_action:main', 'dispatch_patrol = rmf_demos_tasks.dispatch_patrol:main', From a7dbc4f97c06ac1527b46bab4500bc04dd793895 Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Fri, 30 Sep 2022 17:59:20 +0800 Subject: [PATCH 11/14] Remove cleaning server Signed-off-by: Xi Yu Oh --- .../airport_terminal/cleanerBotA_config.yaml | 46 +++++++++ .../airport_terminal/cleanerBotE_config.yaml | 33 +++++++ .../config/hotel/cleanerBotA_config.yaml | 25 +++++ rmf_demos/launch/airport_terminal.launch.xml | 8 -- rmf_demos/launch/hotel.launch.xml | 7 -- .../rmf_demos_fleet_adapter/RobotClientAPI.py | 19 ++++ .../RobotCommandHandle.py | 52 ++++++++--- .../rmf_demos_fleet_adapter/fleet_adapter.py | 5 +- .../rmf_demos_fleet_adapter/fleet_manager.py | 59 +++++++----- .../airport_docker_config.yaml | 87 ----------------- .../rmf_demos_tasks/cleaning_server.py | 93 ------------------- .../rmf_demos_tasks/hotel_cleaner_config.yaml | 41 -------- rmf_demos_tasks/setup.py | 3 - 13 files changed, 199 insertions(+), 279 deletions(-) delete mode 100644 rmf_demos_tasks/rmf_demos_tasks/airport_docker_config.yaml delete mode 100644 rmf_demos_tasks/rmf_demos_tasks/cleaning_server.py delete mode 100644 rmf_demos_tasks/rmf_demos_tasks/hotel_cleaner_config.yaml diff --git a/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml b/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml index 8de067e6..5a294cef 100644 --- a/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml +++ b/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml @@ -38,6 +38,52 @@ rmf_fleet: clean: True finishing_request: "park" # [park, charge, nothing] action: ["clean"] + process_waypoints: + zone_1: + level_name: "L1" + path: [ [121.3, -26.76, 0.0], + [121.1, -17.44, 1.57], + [116.2, -17.44, 0.0], + [116.2, -26.60, 1.57], + [110.6, -26.60, 3.14], + [110.6, -17.44, 1.57], + [105.9, -17.44, 0.0], + [105.9, -32.1, 0.0], + [121.5, -30.35, -1.57], + [121.3, -28.3, 0.0] ] + finish_waypoint: "zone_1" + zone_2: + level_name: "L1" + path: [ [149.00, -29.22, 0.0], + [129.4, -29.67, -1.57], + [129.4, -24.76, -1.57], + [149.0, -24.76, 3.14], + [149.0, -27.11, 0.0] ] + finish_waypoint: "zone_2" + zone_3: + level_name: "L1" + path: [ [154.4, -29.51, -1.57], + [169.9, -29.79, -0.8], + [173.1, -26.41, 1.57], + [173.1, -23.26, 3.14], + [154.4, -23.26, -1.57], + [154.4, -25.00, -1.57], + [168.5, -25.00, 3.14], + [168.5, -27.60, 1.57], + [154.4, -27.60, 0.0]] + finish_waypoint: "zone_3" + zone_4: + level_name: "L1" + path: [ [152.0, -55.6, 0.0], + [123.1, -55.6, 0.0], + [123.1, -57.14, 0.0], + [148.5, -57.14, 0.0], + [148.5, -59.34, 0.0], + [148.5, -59.34, 0.0], + [123.0, -59.06, 0.0], + [123.0, -60.8, 0.0], + [152.0, -60.3, 0.0] ] + finish_waypoint: "zone_4" # CleanerBotA CONFIG ================================================================= diff --git a/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml b/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml index c4d43d21..f83af046 100644 --- a/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml +++ b/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml @@ -38,6 +38,39 @@ rmf_fleet: clean: True finishing_request: "park" # [park, charge, nothing] action: ["clean"] + process_waypoints: + zone_2: + level_name: "L1" + path: [ [149.00, -29.22, 0.0], + [129.4, -29.67, -1.57], + [129.4, -24.76, -1.57], + [149.0, -24.76, 3.14], + [149.0, -27.11, 0.0] ] + finish_waypoint: "zone_2" + zone_3: + level_name: "L1" + path: [ [154.4, -29.51, -1.57], + [169.9, -29.79, -0.8], + [173.1, -26.41, 1.57], + [173.1, -23.26, 3.14], + [154.4, -23.26, -1.57], + [154.4, -25.00, -1.57], + [168.5, -25.00, 3.14], + [168.5, -27.60, 1.57], + [154.4, -27.60, 0.0]] + finish_waypoint: "zone_3" + zone_4: + level_name: "L1" + path: [ [152.0, -55.6, 0.0], + [123.1, -55.6, 0.0], + [123.1, -57.14, 0.0], + [148.5, -57.14, 0.0], + [148.5, -59.34, 0.0], + [148.5, -59.34, 0.0], + [123.0, -59.06, 0.0], + [123.0, -60.8, 0.0], + [152.0, -60.3, 0.0] ] + finish_waypoint: "zone_4" # CleanerBotE CONFIG ================================================================= diff --git a/rmf_demos/config/hotel/cleanerBotA_config.yaml b/rmf_demos/config/hotel/cleanerBotA_config.yaml index 9f4b4531..51da03dd 100644 --- a/rmf_demos/config/hotel/cleanerBotA_config.yaml +++ b/rmf_demos/config/hotel/cleanerBotA_config.yaml @@ -38,6 +38,31 @@ rmf_fleet: clean: True finishing_request: "park" # [park, charge, nothing] action: ["clean"] + process_waypoints: + clean_lobby: + level_name: "L1" + path: [ [24.59, -35.49, 0.0], + [24.24, -38.77, 1.57], + [18.3, -38.62, 0.0], + [18.3, -35.3, 0.0], + [23.8, -35.45, 0.0] ] + finish_waypoint: "clean_lobby" + clean_waiting_area: + level_name: "L1" + path: [ [8.54, -21.93, 1.57], + [14.0, -23.25, 0.0], + [14.0, -24.7, 0.0], + [8.6, -24.7, 1.57], + [8.56, -23.27, 0.0] ] + finish_waypoint: "clean_waiting_area" + clean_restaurant: + level_name: "L1" + path: [ [14.2, -11.35, 1.57], + [16.5, -11.35, 1.57], + [17.8, -13.9, 0.0], + [12.8, -14.1, 0.0], + [12.86, -11.4, 0.0] ] + finish_waypoint: "clean_restaurant" # CleanerBotA CONFIG ================================================================= diff --git a/rmf_demos/launch/airport_terminal.launch.xml b/rmf_demos/launch/airport_terminal.launch.xml index ef14d5b3..4a2ccb9c 100644 --- a/rmf_demos/launch/airport_terminal.launch.xml +++ b/rmf_demos/launch/airport_terminal.launch.xml @@ -63,12 +63,4 @@ - - - - - - - - diff --git a/rmf_demos/launch/hotel.launch.xml b/rmf_demos/launch/hotel.launch.xml index 086ab400..783fb29d 100644 --- a/rmf_demos/launch/hotel.launch.xml +++ b/rmf_demos/launch/hotel.launch.xml @@ -52,11 +52,4 @@ - - - - - - - diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py index ae0e21a9..b85206ae 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py @@ -92,6 +92,25 @@ def navigate(self, print(f'Other error: {err}') return False + def retrieve_process_waypoints(self, + process: str): + ''' Returns the list of waypoints for a given process.''' + url = self.prefix +\ + f'/open-rmf/rmf_demos_fm/process_waypoints?process={process}' + try: + response = requests.get(url, timeout=self.timeout) + response.raise_for_status() + if self.debug: + print(f'Response: {response.json()}') + if not response.json()['success']: + return None + return response.json()['data']['path'] + except HTTPError as http_err: + print(f'HTTP error: {http_err}') + except Exception as err: + print(f'Other error: {err}') + return None + def start_process(self, robot_name: str, cmd_id: int, diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py index 46bdbd7e..6e5d703e 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py @@ -120,7 +120,7 @@ def __init__(self, self.action_waypoint_index = None self.current_cmd_id = 0 self.started_action = False - self.clean_zone = None + self.action_description = None # Threading variables self._lock = threading.Lock() @@ -484,30 +484,60 @@ def _dock(): self._dock_thread = threading.Thread(target=_dock) self._dock_thread.start() - def perform_clean_action(self): + def check_perform_action(self): cmd_id = self.next_cmd_id() self._quit_action_event.clear() self.started_action = True - def _perform_clean(): + task_name = None + for desc_key in self.action_description.keys(): + if '_task_name' in desc_key: + task_name = self.action_description[desc_key] + break + if task_name is None: + # If there is no task name, it is possible that this action is + # a manual process where RMF releases control, e.g. teleop + # An /action_execution_notice should be published manually to + # end the action + return + + def _perform_action(): + wps = self.api.retrieve_process_waypoints(task_name) + if wps is None: + # If no waypoints are found for this task, end robot action + self.node.get_logger().info( + f'No waypoints found for task [{task_name}], ' + f'ending robot action') + self.complete_robot_action() + return + self.api.start_process( - self.name, cmd_id, self.clean_zone, self.map_name) + self.name, cmd_id, task_name, self.map_name) self.node.get_logger().info( - f'Robot [{self.name}] is starting a new cleaning ' - f'task: {self.clean_zone}') + f'Robot [{self.name}] is executing perform action {task_name}') while not self.api.process_completed(self.name, cmd_id): if self.action_execution is None: break + traj = schedule.make_trajectory( + self.vehicle_traits, + self.adapter.now(), + wps) + itinerary = schedule.Route(self.map_name, traj) + if self.update_handle is not None: + participant = \ + self.update_handle.get_unstable_participant() + participant.set_itinerary([itinerary]) + if self._quit_action_event.wait(0.1): self.node.get_logger().info("Aborting action") return - # End the clean action once it is completed + # End the robot action once it is completed self.complete_robot_action() - self._action_thread = threading.Thread(target=_perform_clean) + self._action_thread = threading.Thread(target=_perform_action) self._action_thread.start() return @@ -578,8 +608,7 @@ def update_state(self): if not self.started_action: self.started_action = True self.api.toggle_action(self.name, self.started_action) - if self.clean_zone is not None: - self.perform_clean_action() + self.check_perform_action() self.update_handle.update_off_grid_position( self.position, self.action_waypoint_index) # if robot is merging into a waypoint @@ -704,8 +733,7 @@ def complete_robot_action(self): self.action_execution.finished() self.action_execution = None self.started_action = False - if self.clean_zone is not None: - self.clean_zone = None + self.action_description = None self.api.toggle_action(self.name, self.started_action) self.node.get_logger().info(f"Robot {self.name} has completed the" f" action it was performing") diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py index 9e3745b7..ac666fc4 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py @@ -183,10 +183,7 @@ def _action_executor(category: str, cmd_handle.last_known_waypoint_index cmd_handle.on_waypoint = None cmd_handle.on_lane = None - # Set the clean zone if this is a cleaning task - if category == 'clean' and type(description) is dict and \ - 'clean_task_name' in description: - cmd_handle.clean_zone = description['clean_task_name'] + cmd_handle.action_description = description cmd_handle.action_execution = execution # Set the action_executioner for the robot cmd_handle.update_handle.set_action_executor(_action_executor) diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py index 92271e9a..caadeb88 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py @@ -32,7 +32,7 @@ from rclpy.qos import QoSReliabilityPolicy as Reliability from rmf_fleet_msgs.msg import RobotState, Location, PathRequest, \ - DockSummary, RobotMode, CleanTaskSummary + DockSummary, RobotMode import rmf_adapter as adpt import rmf_adapter.vehicletraits as traits @@ -112,7 +112,7 @@ def __init__(self, config, nav_path): self.robots = {} # Map robot name to state self.docks = {} # Map dock name to waypoints - self.clean_paths = {} # Map clean tasks to waypoints + self.process_waypoints = {} # Map processes to waypoints for robot_name, robot_config in self.config["robots"].items(): self.robots[robot_name] = State() @@ -131,6 +131,11 @@ def __init__(self, config, nav_path): self.vehicle_traits.differential.reversible =\ self.config['rmf_fleet']['reversible'] + if 'process_waypoints' in self.config['rmf_fleet']: + process_waypoints = self.config['rmf_fleet']['process_waypoints'] + for task_name, task_waypoints in process_waypoints.items(): + self.process_waypoints[task_name] = task_waypoints + self.sio = socketio.Client() @self.sio.on("/gps") @@ -171,12 +176,6 @@ def message(data): self.dock_summary_cb, qos_profile=transient_qos) - self.create_subscription( - CleanTaskSummary, - 'clean_task_summary', - self.clean_task_summary_cb, - qos_profile=transient_qos) - self.path_pub = self.create_publisher( PathRequest, 'robot_path_requests', @@ -291,6 +290,17 @@ async def stop(robot_name: str, cmd_id: int): response['success'] = True return response + @app.get('/open-rmf/rmf_demos_fm/process_waypoints/', + response_model=Response) + async def process_waypoints(process: str): + response = {'success': False, 'msg': ''} + if (process not in self.process_waypoints): + return response + + response['data'] = self.process_waypoints[process] + response['success'] = True + return response + @app.post('/open-rmf/rmf_demos_fm/start_task/', response_model=Response) async def start_process(robot_name: str, cmd_id: int, task: Request): @@ -298,7 +308,7 @@ async def start_process(robot_name: str, cmd_id: int, task: Request): if (robot_name not in self.robots or len(task.task) < 1 or (task.task not in self.docks and - task.task not in self.clean_paths)): + task.task not in self.process_waypoints)): return response robot = self.robots[robot_name] @@ -314,15 +324,22 @@ async def start_process(robot_name: str, cmd_id: int, task: Request): if task.task in self.docks: task_wps = self.docks[task.task] - elif task.task in self.clean_paths: - task_wps = self.clean_paths[task.task] - else: - return response - - for wp in task_wps: - target_loc = wp - path_request.path.append(target_loc) - previous_wp = [wp.x, wp.y, wp.yaw] + for wp in task_wps: + target_loc = wp + path_request.path.append(target_loc) + previous_wp = [wp.x, wp.y, wp.yaw] + elif task.task in self.process_waypoints: + task_wps = self.process_waypoints[task.task]['path'] + task_level_name = \ + self.process_waypoints[task.task]['level_name'] + for wp in task_wps: + target_loc = Location() + target_loc.x = wp[0] + target_loc.y = wp[1] + target_loc.yaw = wp[2] + target_loc.level_name = task_level_name + path_request.path.append(target_loc) + previous_wp = wp path_request.fleet_name = self.fleet_name path_request.robot_name = robot_name @@ -395,12 +412,6 @@ def dock_summary_cb(self, msg): for dock in fleet.params: self.docks[dock.start] = dock.path - def clean_task_summary_cb(self, msg): - for fleet in msg.clean_tasks: - if (fleet.fleet_name == self.fleet_name): - for clean_task in fleet.params: - self.clean_paths[clean_task.name] = clean_task.path - def get_robot_state(self, robot: State, robot_name): data = {} if self.gps: diff --git a/rmf_demos_tasks/rmf_demos_tasks/airport_docker_config.yaml b/rmf_demos_tasks/rmf_demos_tasks/airport_docker_config.yaml deleted file mode 100644 index 8e5f199b..00000000 --- a/rmf_demos_tasks/rmf_demos_tasks/airport_docker_config.yaml +++ /dev/null @@ -1,87 +0,0 @@ -# The configuration file for cleaning actions that robots can perform -# FleetName: The name of the fleet -# Clean task name: The name of the cleaning task -# level_name: The level name -# path: The waypoints that the robot will pass through during the cleaning process) -# finish_waypoint: The name of the waypoint where the robot will end up at the end of the cleaning process - - -cleanerBotA: - zone_1: - level_name: "L1" - path: [ [121.3, -26.76, 0.0], - [121.1, -17.44, 1.57], - [116.2, -17.44, 0.0], - [116.2, -26.60, 1.57], - [110.6, -26.60, 3.14], - [110.6, -17.44, 1.57], - [105.9, -17.44, 0.0], - [105.9, -32.1, 0.0], - [121.5, -30.35, -1.57], - [121.3, -28.3, 0.0] ] - finish_waypoint: "zone_1" - zone_2: - level_name: "L1" - path: [ [149.00, -29.22, 0.0], - [129.4, -29.67, -1.57], - [129.4, -24.76, -1.57], - [149.0, -24.76, 3.14], - [149.0, -27.11, 0.0] ] - finish_waypoint: "zone_2" - zone_3: - level_name: "L1" - path: [ [154.4, -29.51, -1.57], - [169.9, -29.79, -0.8], - [173.1, -26.41, 1.57], - [173.1, -23.26, 3.14], - [154.4, -23.26, -1.57], - [154.4, -25.00, -1.57], - [168.5, -25.00, 3.14], - [168.5, -27.60, 1.57], - [154.4, -27.60, 0.0]] - finish_waypoint: "zone_3" - zone_4: - level_name: "L1" - path: [ [152.0, -55.6, 0.0], - [123.1, -55.6, 0.0], - [123.1, -57.14, 0.0], - [148.5, -57.14, 0.0], - [148.5, -59.34, 0.0], - [148.5, -59.34, 0.0], - [123.0, -59.06, 0.0], - [123.0, -60.8, 0.0], - [152.0, -60.3, 0.0] ] - finish_waypoint: "zone_4" -cleanerBotE: - zone_2: - level_name: "L1" - path: [ [149.00, -29.22, 0.0], - [129.4, -29.67, -1.57], - [129.4, -24.76, -1.57], - [149.0, -24.76, 3.14], - [149.0, -27.11, 0.0] ] - finish_waypoint: "zone_2" - zone_3: - level_name: "L1" - path: [ [154.4, -29.51, -1.57], - [169.9, -29.79, -0.8], - [173.1, -26.41, 1.57], - [173.1, -23.26, 3.14], - [154.4, -23.26, -1.57], - [154.4, -25.00, -1.57], - [168.5, -25.00, 3.14], - [168.5, -27.60, 1.57], - [154.4, -27.60, 0.0]] - finish_waypoint: "zone_3" - zone_4: - level_name: "L1" - path: [ [152.0, -55.6, 0.0], - [123.1, -55.6, 0.0], - [123.1, -57.14, 0.0], - [148.5, -57.14, 0.0], - [148.5, -59.34, 0.0], - [148.5, -59.34, 0.0], - [123.0, -59.06, 0.0], - [123.0, -60.8, 0.0], - [152.0, -60.3, 0.0] ] - finish_waypoint: "zone_4" diff --git a/rmf_demos_tasks/rmf_demos_tasks/cleaning_server.py b/rmf_demos_tasks/rmf_demos_tasks/cleaning_server.py deleted file mode 100644 index 6f778a7d..00000000 --- a/rmf_demos_tasks/rmf_demos_tasks/cleaning_server.py +++ /dev/null @@ -1,93 +0,0 @@ -# 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 rclpy -import argparse -import yaml -import time -from rclpy.node import Node - -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_fleet_msgs.msg import CleanTaskSummary, CleanTask, \ - CleanTaskParameter, Location - - -class CleaningServer(Node): - """ - The CleaningServer provides the coordinates for each clean zone - in the respective demo worlds by publishing a CleanTaskSummary - when launched. - """ - - def __init__(self, config_yaml): - super().__init__('cleaning_server') - self.get_logger().info(f'Starting cleaning server...') - self.config_yaml = config_yaml - - transient_qos = QoSProfile( - history=History.RMW_QOS_POLICY_HISTORY_KEEP_LAST, - depth=1, - reliability=Reliability.RMW_QOS_POLICY_RELIABILITY_RELIABLE, - durability=Durability.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) - self.cleaning_server_publisher = self.create_publisher( - CleanTaskSummary, 'clean_task_summary', qos_profile=transient_qos) - - # Populate the cleaning service msg - clean_task_summary = CleanTaskSummary() - for fleet_name, cleaning_info in self.config_yaml.items(): - clean_task = CleanTask() - clean_task.fleet_name = fleet_name - for clean_task_name, clean_waypoints in cleaning_info.items(): - param = CleanTaskParameter() - param.name = clean_task_name - for point in clean_waypoints['path']: - location = Location() - location.x = point[0] - location.y = point[1] - location.yaw = point[2] - location.level_name = clean_waypoints['level_name'] - param.path.append(location) - clean_task.params.append(param) - clean_task_summary.clean_tasks.append(clean_task) - time.sleep(2) - self.cleaning_server_publisher.publish(clean_task_summary) - - -def main(argv=sys.argv): - rclpy.init(args=argv) - args_without_ros = rclpy.utilities.remove_ros_args(argv) - parser = argparse.ArgumentParser( - prog='cleaning_server', - description='Start cleaning service') - parser.add_argument("-c", "--config", type=str, required=True, - help="Path to config file") - args = parser.parse_args(args_without_ros[1:]) - - config = args.config - - with open(config, 'r') as f: - config_yaml = yaml.safe_load(f) - - cleaning_server = CleaningServer(config_yaml) - rclpy.spin(cleaning_server) - rclpy.shutdown() - - -if __name__ == '__main__': - main(sys.argv) diff --git a/rmf_demos_tasks/rmf_demos_tasks/hotel_cleaner_config.yaml b/rmf_demos_tasks/rmf_demos_tasks/hotel_cleaner_config.yaml deleted file mode 100644 index c1fd0de9..00000000 --- a/rmf_demos_tasks/rmf_demos_tasks/hotel_cleaner_config.yaml +++ /dev/null @@ -1,41 +0,0 @@ -# The configuration file for cleaning actions that robots can perform -# FleetName: The name of the fleet -# Clean task name: The name of the cleaning task -# level_name: The level name -# path: The waypoints that the robot will pass through during the cleaning process) -# finish_waypoint: The name of the waypoint where the robot will end up at the end of the cleaning process -# -# Note: These waypoints are in (x meters, y meters, yaw radian) form -# -# If loaded correctly, you will be able to echo the msg via: -# ros2 topic echo /clean_task_summary --qos-durability transient_local --qos-reliability reliable -# -# Configuring Cleaning path: -# The full cleaning path which the robot will travel along. When configuring, the first -# waypoint is required to be the current robot's waypoint. - -cleanerBotA: - clean_lobby: - level_name: "L1" - path: [ [24.59, -35.49, 0.0], - [24.24, -38.77, 1.57], - [18.3, -38.62, 0.0], - [18.3, -35.3, 0.0], - [23.8, -35.45, 0.0] ] - finish_waypoint: "clean_lobby" - clean_waiting_area: - level_name: "L1" - path: [ [8.54, -21.93, 1.57], - [14.0, -23.25, 0.0], - [14.0, -24.7, 0.0], - [8.6, -24.7, 1.57], - [8.56, -23.27, 0.0] ] - finish_waypoint: "clean_waiting_area" - clean_restaurant: - level_name: "L1" - path: [ [14.2, -11.35, 1.57], - [16.5, -11.35, 1.57], - [17.8, -13.9, 0.0], - [12.8, -14.1, 0.0], - [12.86, -11.4, 0.0] ] - finish_waypoint: "clean_restaurant" diff --git a/rmf_demos_tasks/setup.py b/rmf_demos_tasks/setup.py index 784124c6..8ea6da62 100644 --- a/rmf_demos_tasks/setup.py +++ b/rmf_demos_tasks/setup.py @@ -10,8 +10,6 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - ('share/'+package_name, [package_name+'/airport_docker_config.yaml']), - ('share/'+package_name, [package_name+'/hotel_cleaner_config.yaml']), ], install_requires=['setuptools'], author='Grey', @@ -27,7 +25,6 @@ 'request_loop = rmf_demos_tasks.request_loop:main', 'request_lift = rmf_demos_tasks.request_lift:main', 'cancel_task = rmf_demos_tasks.cancel_task:main', - 'cleaning_server = rmf_demos_tasks.cleaning_server:main', 'dispatch_loop = rmf_demos_tasks.dispatch_loop:main', 'dispatch_action = rmf_demos_tasks.dispatch_action:main', 'dispatch_patrol = rmf_demos_tasks.dispatch_patrol:main', From 55ea8e308e136b647552cfc8e55449175cf36d21 Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Fri, 30 Sep 2022 19:11:10 +0800 Subject: [PATCH 12/14] Revert changes on dispatch_action Signed-off-by: Xi Yu Oh --- .../rmf_demos_tasks/dispatch_action.py | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py index addc1664..93368658 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py @@ -50,7 +50,7 @@ def __init__(self, argv=sys.argv): parser.add_argument('-a', '--action', required=True, type=str, help='action name') parser.add_argument('-ad', '--action_desc', required=False, - default='{}', nargs='+', + default='{}', type=str, help='action description in json str') parser.add_argument('-F', '--fleet', type=str, help='Fleet name, should define tgt with robot') @@ -110,11 +110,7 @@ def __init__(self, argv=sys.argv): description["phases"] = [] activities = [] - def _add_action(i=0): - try: - action_desc = json.loads(self.args.action_desc[i]) - except ValueError as e: - action_desc = self.args.action_desc[i] + def _add_action(): activities.append( { "category": "perform_action", @@ -122,7 +118,7 @@ def _add_action(i=0): { "unix_millis_action_duration_estimate": 60000, "category": self.args.action, - "description": action_desc, + "description": json.loads(self.args.action_desc), "use_tool_sink": self.args.use_tool_sink } }) @@ -131,12 +127,12 @@ def _add_action(i=0): _add_action() else: # Add action activities - for i in range(len(self.args.starts)): + for start in self.args.starts: activities.append({ "category": "go_to_place", - "description": self.args.starts[i] + "description": start }) - _add_action(i) + _add_action() # Add activities to phases description["phases"].append({ From 96fff51c82d311c36375002ac89b94360aed1eb5 Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Wed, 22 Mar 2023 06:19:36 +0000 Subject: [PATCH 13/14] Minor updates Signed-off-by: Xi Yu Oh --- rmf_demos/config/airport_terminal/cleanerBotA_config.yaml | 2 +- rmf_demos/config/airport_terminal/cleanerBotE_config.yaml | 2 +- rmf_demos/config/hotel/cleanerBotA_config.yaml | 2 +- .../rmf_demos_fleet_adapter/RobotCommandHandle.py | 5 ++--- .../rmf_demos_fleet_adapter/fleet_adapter.py | 4 ++-- 5 files changed, 7 insertions(+), 8 deletions(-) diff --git a/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml b/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml index 5a294cef..bac9f462 100644 --- a/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml +++ b/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml @@ -37,7 +37,7 @@ rmf_fleet: delivery: False clean: True finishing_request: "park" # [park, charge, nothing] - action: ["clean"] + actions: ["clean"] process_waypoints: zone_1: level_name: "L1" diff --git a/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml b/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml index f83af046..ae2078b5 100644 --- a/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml +++ b/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml @@ -37,7 +37,7 @@ rmf_fleet: delivery: False clean: True finishing_request: "park" # [park, charge, nothing] - action: ["clean"] + actions: ["clean"] process_waypoints: zone_2: level_name: "L1" diff --git a/rmf_demos/config/hotel/cleanerBotA_config.yaml b/rmf_demos/config/hotel/cleanerBotA_config.yaml index 51da03dd..ea81d026 100644 --- a/rmf_demos/config/hotel/cleanerBotA_config.yaml +++ b/rmf_demos/config/hotel/cleanerBotA_config.yaml @@ -37,7 +37,7 @@ rmf_fleet: delivery: False clean: True finishing_request: "park" # [park, charge, nothing] - action: ["clean"] + actions: ["clean"] process_waypoints: clean_lobby: level_name: "L1" diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py index 6e5d703e..f3674a1e 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotCommandHandle.py @@ -484,7 +484,7 @@ def _dock(): self._dock_thread = threading.Thread(target=_dock) self._dock_thread.start() - def check_perform_action(self): + def start_perform_action(self): cmd_id = self.next_cmd_id() self._quit_action_event.clear() self.started_action = True @@ -539,7 +539,6 @@ def _perform_action(): self._action_thread = threading.Thread(target=_perform_action) self._action_thread.start() - return def get_position(self): ''' This helper function returns the live position of the robot in the @@ -608,7 +607,7 @@ def update_state(self): if not self.started_action: self.started_action = True self.api.toggle_action(self.name, self.started_action) - self.check_perform_action() + self.start_perform_action() self.update_handle.update_off_grid_position( self.position, self.action_waypoint_index) # if robot is merging into a waypoint diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py index ac666fc4..3409efa8 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py @@ -159,8 +159,8 @@ def _consider(description: dict): return confirm # Configure this fleet to perform any kind of teleop action - if 'action' in fleet_config['task_capabilities']: - for action in fleet_config['task_capabilities']['action']: + if 'actions' in fleet_config['task_capabilities']: + for action in fleet_config['task_capabilities']['actions']: fleet_handle.add_performable_action(action, _consider) def _updater_inserter(cmd_handle, update_handle): From fdb2d282dabe362430a91239494be581c98aa0ad Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Wed, 22 Mar 2023 07:33:30 +0000 Subject: [PATCH 14/14] Remove clean task capability Signed-off-by: Xi Yu Oh --- rmf_demos/config/airport_terminal/cleanerBotA_config.yaml | 1 - rmf_demos/config/airport_terminal/cleanerBotE_config.yaml | 1 - rmf_demos/config/airport_terminal/deliveryRobot_config.yaml | 1 - rmf_demos/config/airport_terminal/tinyRobot_config.yaml | 1 - rmf_demos/config/battle_royale/tinyRobot_config.yaml | 1 - rmf_demos/config/clinic/deliveryRobot_config.yaml | 1 - rmf_demos/config/clinic/tinyRobot_config.yaml | 1 - rmf_demos/config/hotel/cleanerBotA_config.yaml | 1 - rmf_demos/config/hotel/deliveryRobot_config.yaml | 1 - rmf_demos/config/hotel/tinyRobot_config.yaml | 1 - rmf_demos/config/office/tinyRobot_config.yaml | 1 - .../rmf_demos_fleet_adapter/fleet_adapter.py | 4 ---- 12 files changed, 15 deletions(-) diff --git a/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml b/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml index bac9f462..41a8786c 100644 --- a/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml +++ b/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml @@ -35,7 +35,6 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: False delivery: False - clean: True finishing_request: "park" # [park, charge, nothing] actions: ["clean"] process_waypoints: diff --git a/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml b/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml index ae2078b5..5ccf17b2 100644 --- a/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml +++ b/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml @@ -35,7 +35,6 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: False delivery: False - clean: True finishing_request: "park" # [park, charge, nothing] actions: ["clean"] process_waypoints: diff --git a/rmf_demos/config/airport_terminal/deliveryRobot_config.yaml b/rmf_demos/config/airport_terminal/deliveryRobot_config.yaml index bcaf8325..ae7f1b22 100644 --- a/rmf_demos/config/airport_terminal/deliveryRobot_config.yaml +++ b/rmf_demos/config/airport_terminal/deliveryRobot_config.yaml @@ -35,7 +35,6 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: True delivery: True - clean: False finishing_request: "park" # [park, charge, nothing] # DeliveryRobot CONFIG ================================================================= diff --git a/rmf_demos/config/airport_terminal/tinyRobot_config.yaml b/rmf_demos/config/airport_terminal/tinyRobot_config.yaml index 911a98ca..119f6eb3 100644 --- a/rmf_demos/config/airport_terminal/tinyRobot_config.yaml +++ b/rmf_demos/config/airport_terminal/tinyRobot_config.yaml @@ -34,7 +34,6 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: True delivery: True - clean: False finishing_request: "park" # [park, charge, nothing] # TinyRobot CONFIG ================================================================= diff --git a/rmf_demos/config/battle_royale/tinyRobot_config.yaml b/rmf_demos/config/battle_royale/tinyRobot_config.yaml index 29f80eb2..fb5a5323 100755 --- a/rmf_demos/config/battle_royale/tinyRobot_config.yaml +++ b/rmf_demos/config/battle_royale/tinyRobot_config.yaml @@ -35,7 +35,6 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: True delivery: True - clean: False finishing_request: "nothing" # [park, charge, nothing] # TinyRobot CONFIG ================================================================= diff --git a/rmf_demos/config/clinic/deliveryRobot_config.yaml b/rmf_demos/config/clinic/deliveryRobot_config.yaml index 81367db8..ee37b292 100644 --- a/rmf_demos/config/clinic/deliveryRobot_config.yaml +++ b/rmf_demos/config/clinic/deliveryRobot_config.yaml @@ -35,7 +35,6 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: True delivery: True - clean: False finishing_request: "park" # [park, charge, nothing] # DeliveryRobot CONFIG ================================================================= diff --git a/rmf_demos/config/clinic/tinyRobot_config.yaml b/rmf_demos/config/clinic/tinyRobot_config.yaml index 93795745..3d840c20 100644 --- a/rmf_demos/config/clinic/tinyRobot_config.yaml +++ b/rmf_demos/config/clinic/tinyRobot_config.yaml @@ -34,7 +34,6 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: True delivery: True - clean: False finishing_request: "park" # [park, charge, nothing] # TinyRobot CONFIG ================================================================= diff --git a/rmf_demos/config/hotel/cleanerBotA_config.yaml b/rmf_demos/config/hotel/cleanerBotA_config.yaml index ea81d026..312a89ef 100644 --- a/rmf_demos/config/hotel/cleanerBotA_config.yaml +++ b/rmf_demos/config/hotel/cleanerBotA_config.yaml @@ -35,7 +35,6 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: False delivery: False - clean: True finishing_request: "park" # [park, charge, nothing] actions: ["clean"] process_waypoints: diff --git a/rmf_demos/config/hotel/deliveryRobot_config.yaml b/rmf_demos/config/hotel/deliveryRobot_config.yaml index 16f100b1..1c7c0165 100644 --- a/rmf_demos/config/hotel/deliveryRobot_config.yaml +++ b/rmf_demos/config/hotel/deliveryRobot_config.yaml @@ -35,7 +35,6 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: True delivery: True - clean: False finishing_request: "park" # [park, charge, nothing] # DeliveryRobot CONFIG ================================================================= diff --git a/rmf_demos/config/hotel/tinyRobot_config.yaml b/rmf_demos/config/hotel/tinyRobot_config.yaml index da706f70..894698d6 100644 --- a/rmf_demos/config/hotel/tinyRobot_config.yaml +++ b/rmf_demos/config/hotel/tinyRobot_config.yaml @@ -34,7 +34,6 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: True delivery: True - clean: False finishing_request: "park" # [park, charge, nothing] # TinyRobot CONFIG ================================================================= diff --git a/rmf_demos/config/office/tinyRobot_config.yaml b/rmf_demos/config/office/tinyRobot_config.yaml index 914ec809..2024399c 100644 --- a/rmf_demos/config/office/tinyRobot_config.yaml +++ b/rmf_demos/config/office/tinyRobot_config.yaml @@ -34,7 +34,6 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: True delivery: True - clean: False finishing_request: "park" # [park, charge, nothing] # TinyRobot CONFIG ================================================================= diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py index 3409efa8..d7013263 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py @@ -138,10 +138,6 @@ def initialize_fleet(config_yaml, nav_graph_path, node, use_sim_time): node.get_logger().info( f"Fleet [{fleet_name}] is configured to perform Delivery tasks") task_capabilities.append(TaskType.TYPE_DELIVERY) - if fleet_config['task_capabilities']['clean']: - node.get_logger().info( - f"Fleet [{fleet_name}] is configured to perform Clean tasks") - task_capabilities.append(TaskType.TYPE_CLEAN) # Callable for validating requests that this fleet can accommodate def _task_request_check(task_capabilities, msg: TaskProfile):