Use perform_action instead of dock() for cleaning tasks

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 =================================================================
 

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 =================================================================
 

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 =================================================================

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 =================================================================

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 =================================================================

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 =================================================================

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 =================================================================

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 =================================================================
 

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 =================================================================

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 =================================================================

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 =================================================================

rmf_demos/launch/airport_terminal.launch.xml

@@ -63,12 +63,4 @@
     </include>
   </group>
 
-  <!-- Mock Docker Node, to provide Fleet Adapter fix cleaning task paths -->
-  <group>
-    <let name="docking_config_file" value="$(find-pkg-share rmf_demos_tasks)/airport_docker_config.yaml"/>
-    <node pkg="rmf_demos_tasks" exec="mock_docker" args="-c $(var docking_config_file)">
-      <param name="use_sim_time" value="$(var use_sim_time)"/>
-    </node>
-  </group>
-
 </launch>

rmf_demos/launch/hotel.launch.xml

@@ -52,11 +52,4 @@
     </include>
   </group>
 
-  <!-- Mock Docker Node, to provide CleanerBotA Fleet Adapter fix cleaning task paths -->
-  <group>
-    <let name="docking_config_file" value="$(find-pkg-share rmf_demos_tasks)/hotel_cleaner_config.yaml"/>
-    <node pkg="rmf_demos_tasks" exec="mock_docker" args="-c $(var docking_config_file)">
-      <param name="use_sim_time" value="$(var use_sim_time)"/>
-    </node>
-  </group>
 </launch>

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,

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")