Use waypoint navigation over missions (#27)

mir_connector/config/my_fleet.example.yaml

@@ -15,7 +15,7 @@
       scaling: 0.3 # Percentage of the original image size (optional)
       quality: 60 # JPEG quality (0-100) (optional)
   # Configuration specific for the connector that will connect this robot
-  connector_type: mir100
+  connector_type: MiR100
   connector_config:
     mir_host_address: localhost
     mir_host_port: 80
@@ -25,6 +25,7 @@
     mir_username: username
     mir_password: password
     mir_api_version: v2.0
+    mir_firmware_version: v2
     # Toggle InOrbit Mission Tracking features. https://developer.inorbit.ai/tutorials#mission-tracking-tutorial
     # Mission Tracking features are not available on every InOrbit edition.
     enable_mission_tracking: false

mir_connector/inorbit_mir_connector/config/mir100_model.py

@@ -15,7 +15,7 @@
     "location_tz": "America/Los_Angeles",
     "log_level": "INFO",
     "cameras": [],
-    "connector_type": "mir100",
+    "connector_type": "MiR100",
     "user_scripts_dir": "path/to/user/scripts",
     "env_vars": {"ENV_VAR_NAME": "env_var_value"},
     "maps": {},
@@ -27,16 +27,19 @@
         "mir_enable_ws": True,
         "mir_username": "",
         "mir_password": "",
+        "mir_firmware_version": "v2",
         "enable_mission_tracking": True,
         "mir_api_version": "v2.0",
     },
 }
 
 # Expected values
-CONNECTOR_TYPE = "mir100"
+CONNECTOR_TYPES = ["MiR100", "MiR250"]
+FIRMWARE_VERSIONS = ["v2", "v3"]
 MIR_API_VERSION = "v2.0"
 
 
+# TODO(b-Tomas): Rename all MiR100* to MiR* to make more generic
 class MiR100ConfigModel(BaseModel):
     """
     Specific configuration for MiR100 connector.
@@ -50,6 +53,7 @@ class MiR100ConfigModel(BaseModel):
     mir_username: str
     mir_password: str
     mir_api_version: str
+    mir_firmware_version: str
     enable_mission_tracking: bool
 
     @field_validator("mir_api_version")
@@ -60,6 +64,15 @@ def api_version_validation(cls, mir_api_version):
             )
         return mir_api_version
 
+    @field_validator("mir_firmware_version")
+    def firmware_version_validation(cls, mir_firmware_version):
+        if mir_firmware_version not in FIRMWARE_VERSIONS:
+            raise ValueError(
+                f"Unexpected MiR firmware version '{mir_firmware_version}'. "
+                f"Expected one of '{FIRMWARE_VERSIONS}'"
+            )
+        return mir_firmware_version
+
 
 class MiR100Config(InorbitConnectorConfig):
     """
@@ -70,9 +83,9 @@ class MiR100Config(InorbitConnectorConfig):
 
     @field_validator("connector_type")
     def connector_type_validation(cls, connector_type):
-        if connector_type != CONNECTOR_TYPE:
+        if connector_type not in CONNECTOR_TYPES:
             raise ValueError(
-                f"Unexpected connector type '{connector_type}'. Expected '{CONNECTOR_TYPE}'"
+                f"Unexpected connector type '{connector_type}'. Expected one of '{CONNECTOR_TYPES}'"
             )
         return connector_type
 

mir_connector/inorbit_mir_connector/src/connector.py

@@ -2,8 +2,11 @@
 #
 # SPDX-License-Identifier: MIT
 
+from time import sleep
 import pytz
 import math
+import uuid
+from threading import Thread
 from inorbit_connector.connector import Connector
 from inorbit_edge.robot import COMMAND_CUSTOM_COMMAND
 from inorbit_edge.robot import COMMAND_MESSAGE
@@ -14,13 +17,22 @@
 from ..config.mir100_model import MiR100Config
 
 
-# Publish updates every 1s
-CONNECTOR_UPDATE_FREQ = 1
-
 # Available MiR states to select via actions
 MIR_STATE = {3: "READY", 4: "PAUSE", 11: "MANUALCONTROL"}
 
+# Connector missions group name
+# If a group with this name exists it will be used, otherwise it will be created
+# At shutdown, the group will be deleted
+MIR_INORBIT_MISSIONS_GROUP_NAME = "InOrbit Temporary Missions Group"
+# Distance threshold for MiR move missions in meters
+# Used in waypoints sent via missions when the WS interface is not enabled
+MIR_MOVE_DISTANCE_THRESHOLD = 0.1
+
+# Remove missions created in the temporary missions group every 12 hours
+MISSIONS_GARBAGE_COLLECTION_INTERVAL_SECS = 12 * 60 * 60
 
+
+# TODO(b-Tomas): Rename all MiR100* to MiR* to make more generic
 class Mir100Connector(Connector):
     """MiR100 connector.
 
@@ -40,6 +52,7 @@ def __init__(self, robot_id: str, config: MiR100Config) -> None:
             register_user_scripts=True,
             create_user_scripts_dir=True,
         )
+        self.config = config
 
         # Configure the connection to the robot
         self.mir_api = MirApiV2(
@@ -79,6 +92,9 @@ def __init__(self, robot_id: str, config: MiR100Config) -> None:
             enable_io_mission_tracking=config.connector_config.enable_mission_tracking,
         )
 
+        # Get or create the required missions and mission groups
+        self.setup_connector_missions()
+
     def _inorbit_command_handler(self, command_name, args, options):
         """Callback method for command messages.
 
@@ -173,7 +189,7 @@ def _inorbit_command_handler(self, command_name, args, options):
         elif command_name == COMMAND_NAV_GOAL:
             self._logger.info(f"Received '{command_name}'!. {args}")
             pose = args[0]
-            self.mir_api.send_waypoint(pose)
+            self.send_waypoint_over_missions(pose)
         elif command_name == COMMAND_MESSAGE:
             msg = args[0]
             if msg == "inorbit_pause":
@@ -187,11 +203,16 @@ def _inorbit_command_handler(self, command_name, args, options):
     def _connect(self) -> None:
         """Connect to the robot services and to InOrbit"""
         super()._connect()
+        # If enabled, initiate the websockets client
         if self.ws_enabled:
             self.mir_ws.connect()
+        # Start garbage collection for missions
+        # Running with daemon=True will kill the thread when the main thread is done executing
+        Thread(target=self._missions_garbage_collector, daemon=True).start()
 
     def _disconnect(self):
         """Disconnect from any external services"""
+        self.cleanup_connector_missions()
         super()._disconnect()
         if self.ws_enabled:
             self.mir_ws.disconnect()
@@ -271,3 +292,117 @@ def _execution_loop(self):
             self.mission_tracking.report_mission(self.status, self.metrics)
         except Exception:
             self._logger.exception("Error reporting mission")
+
+    def send_waypoint_over_missions(self, pose):
+        """Use the connector's mission group to create a move mission to a designated pose."""
+        mission_id = str(uuid.uuid4())
+        connector_type = self.config.connector_type
+        firmware_version = self.config.connector_config.mir_firmware_version
+
+        self.mir_api.create_mission(
+            group_id=self.tmp_missions_group_id,
+            name="Move to waypoint",
+            guid=mission_id,
+            description="Mission created by InOrbit",
+        )
+        param_values = {
+            "x": float(pose["x"]),
+            "y": float(pose["y"]),
+            "orientation": math.degrees(float(pose["theta"])),
+            "distance_threshold": MIR_MOVE_DISTANCE_THRESHOLD,
+        }
+        if connector_type == "MiR100" and firmware_version == "v2":
+            param_values["retries"] = 5
+        elif connector_type == "MiR250" and firmware_version == "v3":
+            param_values["blocked_path_timeout"] = 60.0
+        else:
+            self._logger.warning(
+                f"Not supported connector type and firmware version combination for waypoint "
+                f"navigation: {connector_type} {firmware_version}. Will attempt to send waypoint "
+                "based on firmware version."
+            )
+            if firmware_version == "v2":
+                param_values["retries"] = 5
+            else:
+                param_values["blocked_path_timeout"] = 60.0
+
+        action_parameters = [
+            {"value": v, "input_name": None, "guid": str(uuid.uuid4()), "id": k}
+            for k, v in param_values.items()
+        ]
+        self.mir_api.add_action_to_mission(
+            action_type="move_to_position",
+            mission_id=mission_id,
+            parameters=action_parameters,
+            priority=1,
+        )
+        self.mir_api.queue_mission(mission_id)
+
+    def setup_connector_missions(self):
+        """Find and store the required missions and mission groups, or create them if they don't
+        exist."""
+        self._logger.info("Setting up connector missions")
+        # Find or create the missions group
+        mission_groups: list[dict] = self.mir_api.get_mission_groups()
+        group = next(
+            (x for x in mission_groups if x["name"] == MIR_INORBIT_MISSIONS_GROUP_NAME), None
+        )
+        self.tmp_missions_group_id = group["guid"] if group is not None else str(uuid.uuid4())
+        if group is None:
+            self._logger.info(f"Creating mission group '{MIR_INORBIT_MISSIONS_GROUP_NAME}'")
+            group = self.mir_api.create_mission_group(
+                feature=".",
+                icon=".",
+                name=MIR_INORBIT_MISSIONS_GROUP_NAME,
+                priority=0,
+                guid=self.tmp_missions_group_id,
+            )
+            self._logger.info(f"Mission group created with guid '{self.tmp_missions_group_id}'")
+        else:
+            self._logger.info(
+                f"Found mission group '{MIR_INORBIT_MISSIONS_GROUP_NAME}' with "
+                f"guid '{self.tmp_missions_group_id}'"
+            )
+
+    def cleanup_connector_missions(self):
+        """Delete the missions group created at startup"""
+        self._logger.info("Cleaning up connector missions")
+        self._logger.info(f"Deleting missions group {self.tmp_missions_group_id}")
+        self.mir_api.delete_mission_group(self.tmp_missions_group_id)
+
+    def _delete_unused_missions(self):
+        """Delete all missions definitions in the temporary group that are not associated to
+        pending or executing missions"""
+        try:
+            mission_defs = self.mir_api.get_mission_group_missions(self.tmp_missions_group_id)
+            missions_queue = self.mir_api.get_missions_queue()
+            # Do not delete definitions of missions that are pending or executing
+            protected_mission_defs = [
+                self.mir_api.get_mission(mission["id"])["mission_id"]
+                for mission in missions_queue
+                if mission["state"].lower() in ["pending", "executing"]
+            ]
+            # Delete the missions definitions in the temporary group that are not
+            # associated to pending or executing missions
+            missions_to_delete = [
+                mission["guid"]
+                for mission in mission_defs
+                if mission["guid"] not in protected_mission_defs
+            ]
+        except Exception as ex:
+            self._logger.error(f"Failed to get missions for garbage collection: {ex}")
+            self.start_missions_garbage_collector()
+            return
+
+        for mission_id in missions_to_delete:
+            try:
+                self._logger.info(f"Deleting mission {mission_id}")
+                self.mir_api.delete_mission_definition(mission_id)
+            except Exception as ex:
+                self._logger.error(f"Failed to delete mission {mission_id}: {ex}")
+
+    def _missions_garbage_collector(self):
+        """Delete unused missions preiodically"""
+        while True:
+            sleep(MISSIONS_GARBAGE_COLLECTION_INTERVAL_SECS)
+            self._delete_unused_missions()

mir_connector/inorbit_mir_connector/src/mir_api/mir_api_v2.py

@@ -19,6 +19,7 @@
 # Endpoints
 METRICS_ENDPOINT_V2 = "metrics"
 MISSION_QUEUE_ENDPOINT_V2 = "mission_queue"
+MISSION_GROUPS_ENDPOINT_V2 = "mission_groups"
 MISSIONS_ENDPOINT_V2 = "missions"
 STATUS_ENDPOINT_V2 = "status"
 
@@ -80,6 +81,80 @@ def get_metrics(self):
                 samples[sample.name] = sample.value
         return samples
 
+    def get_mission_groups(self):
+        """Get available mission groups"""
+        mission_groups_api_url = f"{self.mir_api_base_url}/{MISSION_GROUPS_ENDPOINT_V2}"
+        groups = self._get(mission_groups_api_url, self.api_session).json()
+        return groups
+
+    def get_mission_group_missions(self, mission_group_id: str):
+        """Get available missions for a mission group"""
+        mission_group_api_url = (
+            f"{self.mir_api_base_url}/{MISSION_GROUPS_ENDPOINT_V2}/{mission_group_id}/missions"
+        )
+        missions = self._get(mission_group_api_url, self.api_session).json()
+        return missions
+
+    def create_mission_group(self, feature, icon, name, priority, **kwargs):
+        """Create a new mission group"""
+        mission_groups_api_url = f"{self.mir_api_base_url}/{MISSION_GROUPS_ENDPOINT_V2}"
+        group = {"feature": feature, "icon": icon, "name": name, "priority": priority, **kwargs}
+        response = self._post(
+            mission_groups_api_url,
+            self.api_session,
+            headers={"Content-Type": "application/json"},
+            json=group,
+        )
+        return response.json()
+
+    def delete_mission_group(self, group_id):
+        """Delete a mission group"""
+        mission_group_api_url = f"{self.mir_api_base_url}/{MISSION_GROUPS_ENDPOINT_V2}/{group_id}"
+        self._delete(
+            mission_group_api_url,
+            self.api_session,
+            headers={"Content-Type": "application/json"},
+        )
+
+    def delete_mission_definition(self, mission_id):
+        """Delete a mission definition"""
+        mission_api_url = f"{self.mir_api_base_url}/{MISSIONS_ENDPOINT_V2}/{mission_id}"
+        self._delete(
+            mission_api_url,
+            self.api_session,
+            headers={"Content-Type": "application/json"},
+        )
+
+    def create_mission(self, group_id, name, **kwargs):
+        """Create a mission"""
+        mission_api_url = f"{self.mir_api_base_url}/{MISSIONS_ENDPOINT_V2}"
+        mission = {"group_id": group_id, "name": name, **kwargs}
+        response = self._post(
+            mission_api_url,
+            self.api_session,
+            headers={"Content-Type": "application/json"},
+            json=mission,
+        )
+        return response.json()
+
+    def add_action_to_mission(self, action_type, mission_id, parameters, priority, **kwargs):
+        """Add an action to an existing mission"""
+        action_api_url = f"{self.mir_api_base_url}/{MISSIONS_ENDPOINT_V2}/{mission_id}/actions"
+        action = {
+            "mission_id": mission_id,
+            "action_type": action_type,
+            "parameters": parameters,
+            "priority": priority,
+            **kwargs,
+        }
+        response = self._post(
+            action_api_url,
+            self.api_session,
+            headers={"Content-Type": "application/json"},
+            json=action,
+        )
+        return response.json()
+
     def get_mission(self, mission_queue_id):
         """Queries a mission using the mission_queue/{mission_id} endpoint"""
         mission_api_url = f"{self.mir_api_base_url}/{MISSION_QUEUE_ENDPOINT_V2}/{mission_queue_id}"
@@ -111,6 +186,12 @@ def get_mission_actions(self, mission_id):
         actions = response.json()
         return actions
 
+    def get_missions_queue(self):
+        """Returns all missions in the missions queue"""
+        missions_api_url = f"{self.mir_api_base_url}/{MISSION_QUEUE_ENDPOINT_V2}"
+        response = self._get(missions_api_url, self.api_session)
+        return response.json()
+
     def get_executing_mission_id(self):
         """Returns the id of the mission being currently executed by the robot"""
         # Note(mike) This could be optimized fetching only some elements, but the API is pretty
@@ -179,6 +260,7 @@ def clear_error(self):
 
     def send_waypoint(self, pose):
         """Receives a pose and sends a request to command the robot to navigate to the waypoint"""
+        # Note: This method is deprecated. Prefer creating one-off missions with move actions.
         self.logger.info("Sending waypoint")
         orientation_degs = math.degrees(float(pose["theta"]))
         parameters = {