diff --git a/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml b/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml
index 952b00ee..41a8786c 100644
--- a/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml
+++ b/rmf_demos/config/airport_terminal/cleanerBotA_config.yaml
@@ -35,8 +35,54 @@ 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:
+ 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 158210c0..5ccf17b2 100644
--- a/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml
+++ b/rmf_demos/config/airport_terminal/cleanerBotE_config.yaml
@@ -35,8 +35,41 @@ 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:
+ 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/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 ee5655c0..312a89ef 100644
--- a/rmf_demos/config/hotel/cleanerBotA_config.yaml
+++ b/rmf_demos/config/hotel/cleanerBotA_config.yaml
@@ -35,8 +35,33 @@ 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:
+ 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/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/launch/airport_terminal.launch.xml b/rmf_demos/launch/airport_terminal.launch.xml
index 5ab88a87..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 d3b59431..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 1dde2f43..f3674a1e 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.action_description = 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}, "
@@ -201,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():
@@ -214,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()
@@ -476,6 +484,62 @@ def _dock():
self._dock_thread = threading.Thread(target=_dock)
self._dock_thread.start()
+ def start_perform_action(self):
+ cmd_id = self.next_cmd_id()
+ self._quit_action_event.clear()
+ self.started_action = True
+
+ 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, task_name, self.map_name)
+ self.node.get_logger().info(
+ 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 robot action once it is completed
+ self.complete_robot_action()
+
+ self._action_thread = threading.Thread(target=_perform_action)
+ self._action_thread.start()
+
def get_position(self):
''' This helper function returns the live position of the robot in the
RMF coordinate frame'''
@@ -543,6 +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.start_perform_action()
self.update_handle.update_off_grid_position(
self.position, self.action_waypoint_index)
# if robot is merging into a waypoint
@@ -667,6 +732,7 @@ def complete_robot_action(self):
self.action_execution.finished()
self.action_execution = None
self.started_action = False
+ 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 5c1bdaa3..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):
@@ -159,7 +155,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 '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):
"""Insert a RobotUpdateHandle."""
@@ -170,15 +168,18 @@ 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
+ 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 dcf58847..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
@@ -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.process_waypoints = {} # Map processes to waypoints
for robot_name, robot_config in self.config["robots"].items():
self.robots[robot_name] = State()
@@ -130,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")
@@ -284,13 +290,25 @@ 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):
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.process_waypoints)):
return response
robot = self.robots[robot_name]
@@ -303,10 +321,25 @@ 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]:
- target_loc = wp
- path_request.path.append(target_loc)
- previous_wp = [wp.x, wp.y, wp.yaw]
+
+ if task.task in self.docks:
+ task_wps = self.docks[task.task]
+ 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
@@ -375,7 +408,7 @@ 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
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]}]
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 f326e6fa..00000000
--- a/rmf_demos_tasks/rmf_demos_tasks/airport_docker_config.yaml
+++ /dev/null
@@ -1,87 +0,0 @@
-# The configuration file for docking actions that robots can perform
-# FleetName: The name of the fleet
-# DockName: The name of the docking waypoint
-# 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
-
-
-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/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
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 9d14e227..00000000
--- a/rmf_demos_tasks/rmf_demos_tasks/hotel_cleaner_config.yaml
+++ /dev/null
@@ -1,41 +0,0 @@
-# The configuration file for docking actions that robots can perform
-# FleetName: The name of the fleet
-# DockName: The name of the docking waypoint
-# 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
-#
-# 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
-#
-# 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 d3ade08e..6dcd1a4e 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',