MiR: Localize command (#28)

mir_connector/cac_examples/actions.yaml

@@ -0,0 +1,41 @@
+# Action Definition examples for robots connected via the MiR <> InOrbit Connector.
+# It is recommended to fill the `scope` field with a tag that groups all robots using the connector
+# (e.g. `tag/<ACCOUNT_ID>/<TAG_ID>`).
+
+# Actions can make use of specific behavior defined in the MiR <> InOrbit Connector, by setting the
+# `filename` argument to the name of the command to execute.
+
+# The "localize" command sets the robot's position and current map.
+# The expected arguments are "x" and "y" in meters and "orientation" in degrees, as the target pose
+# in MiR Fleet, and "map_id" as the target map in MiR Fleet, which matches the "frame_id" uploaded 
+# to InOrbit
+kind: ActionDefinition
+metadata:
+  # REQUIRED, see note at the beginning of the file.
+  # scope: <CONFIG_SCOPE>
+  id: localize-map-1
+apiVersion: v0.1
+spec:
+  arguments:
+  - name: filename
+    type: string
+    value: localize
+  - name: --x
+    type: number
+    value: 1.0
+  - name: --y
+    type: number
+    value: 1.0
+  - name: --orientation
+    type: number
+    value: 90
+  - name: --map_id
+    type: string
+    value: "<MiR_fleet_map_id>"
+  confirmation:
+    required: true
+  description: ''
+  group: Maps
+  label: Map 1
+  lock: false
+  type: RunScript

mir_connector/inorbit_mir_connector/src/connector.py

@@ -137,6 +137,32 @@ def _inorbit_command_handler(self, command_name, args, options):
             elif script_name == "set_waiting_for" and script_args[0] == "--text":
                 self._logger.info(f"Setting 'waiting for' value to {script_args[1]}")
                 self.mission_tracking.waiting_for_text = script_args[1]
+            elif script_name == "localize":
+                # The localize command sets the robot's position and current map
+                # The expected arguments are "x" and "y" in meters and "orientation" in degrees, as
+                # in MiR Fleet, and "map_id" as the target map in MiR Fleet, which matches the
+                # uploaded "frame_id" in InOrbit
+                if (
+                    len(script_args) == 8
+                    and script_args[0] == "--x"
+                    and script_args[2] == "--y"
+                    and script_args[4] == "--orientation"
+                    and script_args[6] == "--map_id"
+                ):
+                    status = {
+                        "position": {
+                            "x": float(script_args[1]),
+                            "y": float(script_args[3]),
+                            "orientation": float(script_args[5]),
+                        },
+                        "map_id": script_args[7],
+                    }
+                    self._logger.info(f"Changing map to {script_args[7]}")
+                    self.mir_api.set_status(status)
+                else:
+                    self._logger.error("Invalid arguments for 'localize' command")
+                    options["result_function"]("1", execution_status_details="Invalid arguments")
+                    return
             else:
                 # Other kind if custom commands may be handled by the edge-sdk (e.g. user_scripts)
                 # and not by the connector code itself

mir_connector/inorbit_mir_connector/src/mir_api/mir_api_base.py

@@ -44,10 +44,10 @@ def _delete(self, url: str, session: Session, **kwargs) -> Response:
         self._handle_status(res, kwargs)
         return res
 
-    def _put(self, url: str, session: Session, *kwargs) -> Response:
+    def _put(self, url: str, session: Session, **kwargs) -> Response:
         """Perform a PUT request."""
         self.logger.debug(f"PUTing {url}: {kwargs}")
-        res = session.delete(url, **kwargs)
+        res = session.put(url, **kwargs)
         self.logger.debug(f"Response: {res}")
         self._handle_status(res, kwargs)
         return res

mir_connector/inorbit_mir_connector/src/mir_api/mir_api_v2.py

@@ -165,8 +165,8 @@ def set_status(self, data):
         response = self._put(
             status_api_url,
             self.api_session,
-            data=data,
             headers={"Content-Type": "application/json"},
+            json=data,
         )
         return response.json()
 

mir_connector/inorbit_mir_connector/tests/test_connector.py

@@ -211,6 +211,42 @@ def test_command_callback_inorbit_messages(connector, callback_kwargs):
         assert connector.mir_api.set_state.call_args == call(code)
 
 
+def test_command_callback_change_map(connector, callback_kwargs):
+    callback_kwargs["command_name"] = "customCommand"
+    # test invalid args
+    callback_kwargs["args"] = ["localize", ["--map_id", "map_id"]]
+    connector._inorbit_command_handler(**callback_kwargs)
+    connector.mir_api.change_map.assert_not_called()
+    callback_kwargs["options"]["result_function"].assert_called_with(
+        "1", execution_status_details="Invalid arguments"
+    )
+    # test valid args
+    callback_kwargs["args"] = [
+        "localize",
+        [
+            "--x",
+            1.0,
+            "--y",
+            2.0,
+            "--orientation",
+            90.0,
+            "--map_id",
+            "map_id",
+        ],
+    ]
+    connector._inorbit_command_handler(**callback_kwargs)
+    connector.mir_api.set_status.assert_called_with(
+        {
+            "position": {
+                "x": 1.0,
+                "y": 2.0,
+                "orientation": 90.0,
+            },
+            "map_id": "map_id",
+        }
+    )
+
+
 def test_connector_loop(connector, monkeypatch):
     connector.mission_tracking.report_mission = Mock()