Allow custom docking offset update during runtime

fleet_adapter_mir/fleet_adapter_mir/fleet_adapter_mir.py

@@ -180,19 +180,20 @@ def _consider(description: dict):
         confirm = rmf_fleet.Confirmation()
         confirm.accept()
         return confirm
-    for plugin_name, plugin_data in plugin_config.items():
-        plugin_actions = plugin_data.get('actions')
-        if not plugin_actions:
-            cmd_node.get_logger().warn(
-                f'No action provided for plugin [{plugin_name}]! Fleet '
-                f'[{fleet_handle.fleet_name}] will not bid on tasks submitted '
-                f'with actions associated with this plugin unless the action '
-                f'is registered as a performable action for this fleet by '
-                f'the user.'
-            )
-            continue
-        for action in plugin_actions:
-            fleet_handle.more().add_performable_action(action, _consider)
+    if plugin_config:
+        for plugin_name, plugin_data in plugin_config.items():
+            plugin_actions = plugin_data.get('actions')
+            if not plugin_actions:
+                cmd_node.get_logger().warn(
+                    f'No action provided for plugin [{plugin_name}]! Fleet '
+                    f'[{fleet_handle.fleet_name}] will not bid on tasks '
+                    f'submitted with actions associated with this plugin '
+                    f'unless the action is registered as a performable action '
+                    f'for this fleet by the user.'
+                )
+                continue
+            for action in plugin_actions:
+                fleet_handle.more().add_performable_action(action, _consider)
 
 
     event_loop = asyncio.new_event_loop()

fleet_adapter_mir/fleet_adapter_mir/mir_api.py

@@ -40,6 +40,7 @@ class MiRStateCode(enum.IntEnum):
 class MiRPositionTypes(enum.IntEnum):
     ROBOT = 0
     SHELF = 5
+    VL_MARKER = 11
     CHARGING_STATION = 20
     CHARGING_STATION_ENTRY = 21
     CART = 22
@@ -193,6 +194,7 @@ def update_known_positions(self):
                 self.positions_guid_delete(pos['guid'])
             elif pos['type_id'] == MiRPositionTypes.ROBOT or \
                     pos['type_id'] == MiRPositionTypes.SHELF or \
+                    pos['type_id'] == MiRPositionTypes.VL_MARKER or \
                     pos['type_id'] == MiRPositionTypes.CHARGING_STATION or \
                     pos['type_id'] == MiRPositionTypes.LIFT or \
                     pos['type_id'] == MiRPositionTypes.LIFT_ENTRY:
@@ -312,7 +314,13 @@ def go_to_known_position(self, position_name):
             return None
         return self.dock(self.go_to, None, position_name)
 
-    def dock(self, mission_name, start_waypoint, end_waypoint):
+    def dock(
+        self,
+        mission_name,
+        start_waypoint,
+        end_waypoint,
+        offsets=None
+    ):
         mission_params = None
 
         # Get parameters for start and end waypoints
@@ -352,6 +360,28 @@ def dock(self, mission_name, start_waypoint, end_waypoint):
             mission_params = end_param + dock_param + marker_param
         else:
             mission_params = end_param
+
+        # Remove redundant params
+        # TODO(@xiyuoh) Double check this against other docking missions
+        remove_items = []
+        for i in mission_params:
+            if i.get('input_name') is None:
+                remove_items.append(i)
+        for rm in remove_items:
+            mission_params.remove(rm)
+
+        # Before queueing the mission, update docking offsets if any
+        if offsets:
+            for data in offsets:
+                docking_offset_guid = data['docking_offset_guid']
+                self.docking_offsets_values_put(
+                    docking_offset_guid,
+                    'string',
+                    data['offset'][0],
+                    data['offset'][1],
+                    data['offset'][2]
+                )
+
         return self.queue_mission_by_name(mission_name, mission_params)
 
     def localize(self, map, estimate, index, position_name=None):
@@ -798,6 +828,22 @@ def positions_guid_get(self, guid):
         except Exception as err:
             print(f"Other error: {err}")
 
+    def position_types_get(self):
+        if not self.connected:
+            return
+        try:
+            response = requests.get(
+                self.prefix + 'position_types/',
+                headers=self.headers,
+                timeout=self.timeout)
+            if self.debug:
+                print(f"Response: {response.json()}")
+            return response.json()
+        except HTTPError as http_err:
+            print(f"HTTP error: {http_err}")
+        except Exception as err:
+            print(f"Other error: {err}")
+
     def status_put(self, state_id):
         if not self.connected:
             return
@@ -930,6 +976,60 @@ def docking_offsets_guid_get(self, offset_name: str):
                 return offs['guid']
         return None
 
+    def docking_offsets_values_get(self, guid: str):
+        if not self.connected:
+            return
+        try:
+            response = requests.get(
+                self.prefix + f'docking_offsets/{guid}',
+                headers=self.headers,
+                timeout=self.timeout)
+            if self.debug:
+                print(f"Response: {response.json()}")
+            return response.json()
+        except HTTPError as http_err:
+            print(f"HTTP error: {http_err}")
+        except Exception as err:
+            print(f"Other error: {err}")
+
+    def docking_offsets_values_put(self, guid, name, x, y, orientation):
+        data = {
+            'name': name,
+            'x_offset': x,
+            'y_offset': y,
+            'orientation_offset': orientation,
+        }
+        try:
+            response = requests.put(
+                self.prefix + f'docking_offsets/{guid}',
+                headers=self.headers,
+                data=json.dumps(data),
+                timeout=self.timeout
+            )
+            if self.debug:
+                print(f"Response: {response.json()}")
+            return response.json()
+        except HTTPError as http_err:
+            print(f"HTTP error: {http_err}")
+        except Exception as err:
+            print(f"Other error: {err}")
+
+    def positions_docking_offsets_get(self, guid: str):
+        if not self.connected:
+            return
+        try:
+            response = requests.get(
+                self.prefix + f'positions/{guid}/docking_offsets',
+                headers=self.headers,
+                timeout=self.timeout)
+            if self.debug:
+                print(f"Response: {response.json()}")
+            return response.json()
+        except HTTPError as http_err:
+            print(f"HTTP error: {http_err}")
+        except Exception as err:
+            print(f"Other error: {err}")
+
     def footprints_get(self):
         if not self.connected:
             return

fleet_adapter_mir/fleet_adapter_mir/robot_adapter_mir.py

@@ -162,12 +162,42 @@ def __init__(
             self.node.get_logger().error(error_message)
             raise RuntimeError(error_message)
 
+        # Store custom docking offset config
+        self.docking_offsets = {}
+        docking_offset_config = conversions.get('docking_offsets')
+        if docking_offset_config is not None:
+            for pos_name, offsets in docking_offset_config.items():
+                pos_data = self.api.known_positions.get(pos_name)
+                if pos_data is None:
+                    continue
+                pos_guid = pos_data.get('guid')
+                if pos_guid is None:
+                    continue
+                # Get docking offset guid for this position
+                docking_offset_data = \
+                    self.api.positions_docking_offsets_get(pos_guid)
+                if docking_offset_data is None:
+                    continue
+                # Assume only one docking offset guid for each position
+                # TODO(@xiyuoh) Check this assumption
+                docking_offset_guid = docking_offset_data[0]['guid']
+                for wp, offset in offsets.items():
+                    data = {}
+                    data['position_name'] = pos_name
+                    data['position_guid'] = pos_guid
+                    data['docking_offset_guid'] = docking_offset_guid
+                    data['offset'] = offset
+                    self.docking_offsets[wp] = data
+
         # Track the current ongoing action
         self.current_action = None
 
         # Import and store plugin actions and action factories
         self.action_to_plugin_name = {}  # Maps action name to plugin name
         self.action_factories = {}  # Maps plugin name to action factory
+
+        if plugin_config is None:
+            return
         for plugin_name, action_config in plugin_config.items():
             try:
                 module = action_config['module']
@@ -685,8 +715,20 @@ def request_dock(
                     start_waypoint = None
                 mission_name = docking_points['mission_name']
 
+                # Apply custom docking offset if any
+                offsets = []
+                if start_waypoint is not None and \
+                        start_waypoint in self.docking_offsets:
+                    start_wp_offset = self.docking_offsets[start_waypoint]
+                    offsets.append(start_wp_offset)
+                if end_waypoint in self.docking_offsets:
+                    end_wp_offset = self.docking_offsets[end_waypoint]
+                    offsets.append(end_wp_offset)
+                if len(offsets) == 0:
+                    offsets = None
+
                 mission_queue_id = self.api.dock(
-                    mission_name, start_waypoint, end_waypoint)
+                    mission_name, start_waypoint, end_waypoint, offsets)
                 if mission_queue_id is None:
                     self.node.get_logger().info(
                         f'[{self.name}] Dock mission cannot be queued '