Use waypoint navigation over missions

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

@@ -4,6 +4,7 @@
 
 import pytz
 import math
+import uuid
 from inorbit_connector.connector import Connector
 from inorbit_edge.robot import COMMAND_CUSTOM_COMMAND
 from inorbit_edge.robot import COMMAND_MESSAGE
@@ -14,13 +15,19 @@
 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
+
 
+# TODO(b-Tomas): Rename all MiR100* to MiR* to make more generic
 class Mir100Connector(Connector):
     """MiR100 connector.
 
@@ -40,6 +47,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 +87,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.
 
@@ -147,7 +158,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":
@@ -166,6 +177,7 @@ def _connect(self) -> None:
 
     def _disconnect(self):
         """Disconnect from any external services"""
+        self.cleanup_connector_missions()
         super()._disconnect()
         if self.ws_enabled:
             self.mir_ws.disconnect()
@@ -245,3 +257,80 @@ 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.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.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.missions_group_id,
+            )
+            self._logger.info(f"Mission group created with guid '{self.missions_group_id}'")
+        else:
+            self._logger.info(
+                f"Found mission group '{MIR_INORBIT_MISSIONS_GROUP_NAME}' with "
+                f"guid '{self.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.missions_group_id}")
+        self.mir_api.delete_mission_group(self.missions_group_id)

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,63 @@ 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 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 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}"
@@ -179,6 +237,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 = {

mir_connector/inorbit_mir_connector/tests/test_config_validation.py

@@ -13,7 +13,7 @@ def example_configuration_dict():
     return {
         "inorbit_robot_key": "1234567890",
         "location_tz": "America/Los_Angeles",
-        "connector_type": "mir100",
+        "connector_type": "MiR100",
         "log_level": "INFO",
         "user_scripts": {"path": "/path/to/scripts", "env_vars": {"name": "value"}},
     }
@@ -31,6 +31,7 @@ def example_mir100_configuration_dict(example_configuration_dict):
             "mir_username": "admin",
             "mir_password": "admin",
             "mir_api_version": "v2.0",
+            "mir_firmware_version": "v2",
             "enable_mission_tracking": True,
         },
     }