Adds publish_map to the RobotSession

inorbit_edge/robot.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
-
+import io
 from dataclasses import dataclass, field
 import json
 from typing import Tuple
@@ -10,6 +10,7 @@
 import os
 import logging
 import paho.mqtt.client as mqtt
+from PIL import Image
 from urllib.parse import urlsplit
 import socks
 import ssl
@@ -31,6 +32,7 @@
     CustomCommandRosMessage,
     CameraMessage,
     SystemStatsMessage,
+    MapMessage,
 )
 from inorbit_edge.video import CameraStreamer, Camera
 from inorbit_edge.missions import MissionsModule
@@ -66,6 +68,7 @@
 MQTT_NAV_GOAL_GOAL = "ros/loc/nav_goal"
 MQTT_NAV_GOAL_MULTI = "ros/loc/goal_path"
 MQTT_INITIAL_POSE = "ros/loc/set_pose"
+MQTT_MAP_TOPIC = "ros/loc/map2"
 MQTT_CUSTOM_COMMAND = "custom_command/script/command"
 MQTT_CUSTOM_COMMAND_MESSAGE = "custom_command/ros"
 MQTT_SCRIPT_OUTPUT_TOPIC = "custom_command/script/status"
@@ -226,6 +229,10 @@ def __init__(self, robot_id, robot_name, api_key=None, **kwargs) -> None:
                 "last_ts": 0,
                 "min_time_between_calls": 1,  # seconds
             },
+            "publish_map": {
+                "last_ts": 0,
+                "min_time_between_calls": 1,  # seconds
+            },
         }
 
     def _get_robot_subtopic(self, subtopic):
@@ -487,6 +494,63 @@ def _stop_cameras_streaming(self):
             for s in self.camera_streamers.values():
                 s.stop()
 
+    def publish_map(
+        self,
+        file,
+        map_id="map",
+        frame_id="map",
+        x=0,
+        y=0,
+        resolution=0.05,
+        ts=None,
+        is_update=False,
+    ):
+        """
+        Sends the information about a map and optionally its contents. The map message
+        data is passed as a separate variable to indicate that the map data needs to
+        be actually sent. An is_update flag can be provided to inform in the message
+        that this is an update message, aimed to correct a previous one, currently
+        used for frame_id/map_id updates.
+        """
+        try:
+            # Open the image file
+            img = Image.open(file)
+            # Check if image file is a png image
+            if img.format != "PNG":
+                self.logger.error(f"{file} is not a PNG image")
+                return None
+            # Verify opens and reads the entire image file
+            img.verify()
+        except IOError:
+            self.logger.error(f"{file} is not accessible.")
+            return None
+
+        # img.verify() closes the file, reload it now that its validated
+        img = Image.open(file)
+
+        # Create a BytesIO object
+        img_byte_arr = io.BytesIO()
+        img.save(img_byte_arr, format="PNG")
+        img_byte_arr = img_byte_arr.getvalue()
+
+        # Build the protobuf message
+        data = MapMessage()
+        data.width = img.width
+        data.height = img.height
+        data.data_hash = hash(tuple(img_byte_arr))
+        data.pixels = img_byte_arr
+        data.label = map_id
+        data.map_id = map_id
+        data.frame_id = frame_id
+        data.x = x
+        data.y = y
+        data.resolution = resolution
+        data.ts = ts if ts else int(time.time() * 1000)
+        data.is_update = is_update
+
+        # Publish the map
+        self.publish_protobuf(MQTT_MAP_TOPIC, data, qos=1, retain=True)
+
     def publish_camera_frame(self, camera_id, image, width, height, ts):
         """Publishes a camera frame"""
         msg = CameraMessage()

inorbit_edge/tests/demo/example.py

@@ -6,7 +6,6 @@
 from random import randint, uniform, random
 from math import pi
 import os
-import requests
 import sys
 from math import inf
 
@@ -32,24 +31,6 @@
 NUM_LASERS = 3
 
 
-# TODO: integrate this into the Edge SDK ``RobotSession`` class
-def publish_robot_map(inorbit_api_url, inorbit_api_key, robot_id, map_file):
-    url = inorbit_api_url + "/robots/" + robot_id + "/maps"
-
-    payload = {
-        "metadata": '{"mapId":"map", "label": "map", "resolution": 0.1, "x": -15, "y": -15}'  # noqa: E501
-    }
-    files = [
-        (
-            "image",
-            ("map.png", open(map_file, "rb"), "image/png"),
-        )
-    ]
-    headers = {"x-auth-inorbit-app-key": inorbit_api_key}
-
-    requests.request("POST", url, headers=headers, data=payload, files=files)
-
-
 class FakeRobot:
     """Class that simulates robot data and generates random data"""
 
@@ -188,14 +169,8 @@ def my_command_handler(robot_id, command_name, args, options):
             robot_id=robot_id, robot_name=robot_id
         )
         fake_robot_pool[robot_id] = FakeRobot(robot_id=robot_id, robot_name=robot_id)
-        publish_robot_map(
-            inorbit_api_url=inorbit_api_url,
-            inorbit_api_key=inorbit_api_key,
-            robot_id=robot_id,
-            map_file=os.path.join(
-                os.path.dirname(os.path.abspath(__file__)), "map.png"
-            ),
-        )
+        img = os.path.join(os.path.dirname(os.path.abspath(__file__)), "map.png")
+        robot_session.publish_map(img, "map", "map", -1.5, -1.5, 0.05)
         if video_url is not None:
             robot_session.register_camera("0", OpenCVCamera(video_url))