Merge pull request #58 from MTrab/feature/navmap-camera-feed

MTrab · web-flow · commit 0f237944920b · 2026-02-28T17:24:34.000+01:00
Add Nav map camera stream entity
diff --git a/README.md b/README.md
@@ -17,6 +17,7 @@ After setup, Home Assistant can expose data from Vector, including:
 - Eye color preset control (disabled by default)
 - Quick actions as buttons (sleep, go home, explore, listen for a beat, fetch cube)
 - Vision camera entity (disabled by default)
+- Nav map camera entity (disabled by default)
 - Home Assistant actions/services:
   - `vector.say_text`
   - `vector.set_eye_color`
@@ -77,7 +78,7 @@ To enable them:
 
 1. Open your Vector device in Home Assistant.
 2. Open the entity list.
-3. Enable entities like `Vision`, `Volume`, `Stimulation`, and diagnostic sensors as needed.
+3. Enable entities like `Vision`, `Nav map`, `Volume`, `Stimulation`, and diagnostic sensors as needed.
 
 ## Actions for Automations
 
diff --git a/custom_components/vector/camera.py b/custom_components/vector/camera.py
@@ -19,7 +19,12 @@ async def async_setup_entry(
 ) -> None:
     """Set up Vector camera entities from config entry."""
     coordinator: VectorCoordinator = entry.runtime_data["coordinator"]
-    async_add_entities([VectorVisionCamera(coordinator, entry)])
+    async_add_entities(
+        [
+            VectorVisionCamera(coordinator, entry),
+            VectorNavMapCamera(coordinator, entry),
+        ]
+    )
 
 
 class VectorVisionCamera(VectorEntity, Camera):
@@ -58,3 +63,38 @@ async def async_camera_image(
             return frame
 
         return self._assets.image_bytes(VectorAsset.IMG_UNKNOWN)
+
+
+class VectorNavMapCamera(VectorEntity, Camera):
+    """Vector nav map camera entity using NavMapFeed stream."""
+
+    _attr_has_entity_name = True
+    _attr_translation_key = "nav_map"
+    _attr_entity_registry_enabled_default = False
+    _attr_frame_interval = 0.5
+
+    def __init__(self, coordinator: VectorCoordinator, entry: ConfigEntry) -> None:
+        """Initialize Vector nav map camera entity."""
+        Camera.__init__(self)
+        VectorEntity.__init__(self, coordinator, entry)
+        self._attr_unique_id = f"{entry.entry_id}_nav_map"
+        self._assets = VectorAssetHandler()
+
+    async def async_added_to_hass(self) -> None:
+        """Prepare bundled fallback assets."""
+        await super().async_added_to_hass()
+        await self._assets.async_prepare(self.hass)
+
+    async def async_camera_image(
+        self,
+        width: int | None = None,
+        height: int | None = None,
+    ) -> bytes | None:
+        """Return latest nav map PNG frame bytes."""
+        del width, height
+
+        frame = await self.coordinator.async_get_latest_nav_map_frame(wait_timeout=1.0)
+        if frame is not None:
+            return frame
+
+        return self._assets.image_bytes(VectorAsset.IMG_UNKNOWN)
diff --git a/custom_components/vector/coordinator.py b/custom_components/vector/coordinator.py
@@ -35,6 +35,9 @@
 _INITIAL_REFRESH_MAX_RETRY_DELAY_SECONDS = 60.0
 _CAMERA_STREAM_READ_TIMEOUT_SECONDS = 30.0
 _CAMERA_RECONNECT_DELAY_SECONDS = 2.0
+_NAV_MAP_WAIT_TIMEOUT_SECONDS = 1.0
+_NAV_MAP_RECONNECT_DELAY_SECONDS = 2.0
+_NAV_MAP_FEED_FREQUENCY_HZ = 2.0
 _AUTH_BACKOFF_BASE_DELAY_SECONDS = 15.0
 _AUTH_BACKOFF_MAX_DELAY_SECONDS = 300.0
 _APP_INTENT_RPC_PATH = "/Anki.Vector.external_interface.ExternalInterface/AppIntent"
@@ -94,19 +97,25 @@ def __init__(self, hass: HomeAssistant, entry: ConfigEntry) -> None:
         self.lift_height_mm: float | None = None
         self.camera_frame: bytes | None = None
         self.camera_frame_updated_monotonic: float | None = None
+        self.nav_map_frame: bytes | None = None
+        self.nav_map_frame_updated_monotonic: float | None = None
         self._client: Any | None = None
         self._robot_config: Any | None = None
         self._pyddlvector: Any | None = None
         self._messaging: Any | None = None
+        self._latest_robot_state: Any | None = None
         self._activity_tracker: Any | None = None
         self._telemetry_filter: Any | None = None
         self._event_listener_task: asyncio.Task[None] | None = None
         self._camera_stream_task: asyncio.Task[None] | None = None
+        self._nav_map_stream_task: asyncio.Task[None] | None = None
         self._wake_enable_stream_task: asyncio.Task[None] | None = None
         self._wake_camera_restart_task: asyncio.Task[None] | None = None
         self._camera_stream_lock = asyncio.Lock()
+        self._nav_map_stream_lock = asyncio.Lock()
         self._image_stream_enable_lock = asyncio.Lock()
         self._camera_frame_event = asyncio.Event()
+        self._nav_map_frame_event = asyncio.Event()
         self._settings_lock = asyncio.Lock()
         self._auth_backoff_delay_seconds = _AUTH_BACKOFF_BASE_DELAY_SECONDS
         self._auth_backoff_lock = asyncio.Lock()
@@ -257,6 +266,15 @@ async def async_shutdown(self) -> None:
             finally:
                 self._camera_stream_task = None
 
+        if self._nav_map_stream_task is not None:
+            self._nav_map_stream_task.cancel()
+            try:
+                await self._nav_map_stream_task
+            except asyncio.CancelledError:
+                pass
+            finally:
+                self._nav_map_stream_task = None
+
         if self._wake_enable_stream_task is not None:
             self._wake_enable_stream_task.cancel()
             try:
@@ -385,6 +403,7 @@ async def _async_event_listener_loop(self) -> None:
                     has_changes = False
                     if event_type == "robot_state":
                         robot_state = event.robot_state
+                        self._latest_robot_state = robot_state
                         previous_activity = self.current_activity
                         if self._activity_tracker is not None:
                             next_activity = _normalize_activity_state(
@@ -802,6 +821,95 @@ async def async_get_latest_camera_frame(
 
         return self.camera_frame
 
+    async def async_start_nav_map_stream(self) -> None:
+        """Ensure persistent nav map stream task is running."""
+        async with self._nav_map_stream_lock:
+            if (
+                self._nav_map_stream_task is not None
+                and not self._nav_map_stream_task.done()
+            ):
+                return
+            self._nav_map_stream_task = self.hass.async_create_background_task(
+                self._async_nav_map_stream_loop(),
+                name=f"vector_nav_map_stream_{self.entry.entry_id}",
+            )
+
+    async def async_get_latest_nav_map_frame(
+        self,
+        *,
+        wait_timeout: float = _NAV_MAP_WAIT_TIMEOUT_SECONDS,
+    ) -> bytes | None:
+        """Return latest nav map PNG frame, optionally waiting for first frame."""
+        await self.async_start_nav_map_stream()
+
+        if self.nav_map_frame is not None:
+            return self.nav_map_frame
+
+        try:
+            await asyncio.wait_for(
+                self._nav_map_frame_event.wait(), timeout=wait_timeout
+            )
+        except TimeoutError:
+            return None
+
+        return self.nav_map_frame
+
+    def _nav_map_robot_pose_provider(self) -> Any | None:
+        """Return current robot pose in nav-map coordinates when available."""
+        if self._pyddlvector is None:
+            return None
+        if not hasattr(self._pyddlvector, "nav_map_robot_pose_from_state"):
+            return None
+        robot_state = self._latest_robot_state
+        if robot_state is None:
+            return None
+        return self._pyddlvector.nav_map_robot_pose_from_state(robot_state)
+
+    async def _async_nav_map_stream_loop(self) -> None:
+        """Keep nav-map feed stream and cache latest rendered PNG frame."""
+        while True:
+            try:
+                client, _ = await self._async_get_client()
+                pyddlvector, _ = await self._async_get_modules()
+                if not hasattr(pyddlvector, "iter_nav_map_frames"):
+                    _LOGGER.debug(
+                        "pyddlvector does not provide iter_nav_map_frames; nav map camera disabled"
+                    )
+                    return
+
+                async for frame in pyddlvector.iter_nav_map_frames(
+                    client,
+                    frequency=_NAV_MAP_FEED_FREQUENCY_HZ,
+                    read_timeout=_CAMERA_STREAM_READ_TIMEOUT_SECONDS,
+                    reconnect_delay=_NAV_MAP_RECONNECT_DELAY_SECONDS,
+                    robot_pose_provider=self._nav_map_robot_pose_provider,
+                ):
+                    frame_bytes = bytes(getattr(frame, "data", b""))
+                    if not frame_bytes:
+                        continue
+                    self.nav_map_frame = frame_bytes
+                    self.nav_map_frame_updated_monotonic = time.monotonic()
+                    self._nav_map_frame_event.set()
+            except asyncio.CancelledError:
+                raise
+            except Exception as err:
+                if _is_unauthenticated_error(err):
+                    await self._async_handle_auth_failure("nav map stream", err)
+                    continue
+                details = str(err).strip()
+                if details:
+                    _LOGGER.debug("Vector nav map stream interrupted: %s", details)
+                else:
+                    _LOGGER.debug(
+                        "Vector nav map stream interrupted (%s)",
+                        err.__class__.__name__,
+                        exc_info=True,
+                    )
+                await asyncio.sleep(_NAV_MAP_RECONNECT_DELAY_SECONDS)
+                continue
+
+            await asyncio.sleep(_NAV_MAP_RECONNECT_DELAY_SECONDS)
+
     async def _async_camera_stream_loop(self) -> None:
         """Keep persistent camera feed stream and cache latest frame."""
         while True:
diff --git a/custom_components/vector/manifest.json b/custom_components/vector/manifest.json
@@ -14,7 +14,7 @@
   "iot_class": "local_push",
   "issue_tracker": "https://github.com/MTrab/vector/issues",
   "requirements": [
-    "pyddlvector@git+https://github.com/MTrab/pyddlvector.git@main"
+    "pyddlvector@git+https://github.com/MTrab/pyddlvector.git@feat/navmap-frame-feed"
   ],
   "version": "0.1.0",
   "zeroconf": [
diff --git a/custom_components/vector/strings.json b/custom_components/vector/strings.json
@@ -3,6 +3,9 @@
     "camera": {
       "vision": {
         "name": "Vision"
+      },
+      "nav_map": {
+        "name": "Nav map"
       }
     },
     "button": {
diff --git a/custom_components/vector/translations/en.json b/custom_components/vector/translations/en.json
@@ -3,6 +3,9 @@
     "camera": {
       "vision": {
         "name": "Vision"
+      },
+      "nav_map": {
+        "name": "Nav map"
       }
     },
     "button": {
diff --git a/tests/test_camera.py b/tests/test_camera.py
@@ -5,7 +5,7 @@
 import asyncio
 from types import SimpleNamespace
 
-from custom_components.vector.camera import VectorVisionCamera
+from custom_components.vector.camera import VectorNavMapCamera, VectorVisionCamera
 from custom_components.vector.const import CONF_HOST, CONF_ROBOT_NAME
 
 
@@ -15,7 +15,9 @@ class FakeCoordinator:
     def __init__(self, *, activity: str, frame: bytes | None) -> None:
         self.current_activity = activity
         self._frame = frame
+        self._nav_map_frame = frame
         self.start_calls = 0
+        self.nav_map_start_calls = 0
 
     def async_add_listener(self, update_callback):
         del update_callback
@@ -30,6 +32,15 @@ async def async_get_latest_camera_frame(
         del wait_timeout
         return self._frame
 
+    async def async_start_nav_map_stream(self) -> None:
+        self.nav_map_start_calls += 1
+
+    async def async_get_latest_nav_map_frame(
+        self, *, wait_timeout: float = 1.0
+    ) -> bytes | None:
+        del wait_timeout
+        return self._nav_map_frame
+
 
 def _entry(data: dict[str, str], entry_id: str = "entry-1") -> SimpleNamespace:
     return SimpleNamespace(data=data, entry_id=entry_id)
@@ -69,3 +80,29 @@ def test_camera_returns_live_frame_when_available() -> None:
 
     image = asyncio.run(entity.async_camera_image())
     assert image == b"\xff\xd8\xff"
+
+
+def test_nav_map_camera_entity_disabled_by_default() -> None:
+    coordinator = FakeCoordinator(activity="idle", frame=b"\x89PNG")
+    entry = _entry({CONF_ROBOT_NAME: "Vector-ABCD", CONF_HOST: "192.168.1.10"})
+    entity = VectorNavMapCamera(coordinator, entry)
+    assert entity.entity_registry_enabled_default is False
+
+
+def test_nav_map_camera_returns_unknown_asset_when_no_frame() -> None:
+    coordinator = FakeCoordinator(activity="idle", frame=None)
+    entry = _entry({CONF_ROBOT_NAME: "Vector-ABCD", CONF_HOST: "192.168.1.10"})
+    entity = VectorNavMapCamera(coordinator, entry)
+
+    image = asyncio.run(entity.async_camera_image())
+    assert image is not None
+    assert image.startswith(b"\x89PNG")
+
+
+def test_nav_map_camera_returns_live_frame_when_available() -> None:
+    coordinator = FakeCoordinator(activity="idle", frame=b"\x89PNG\r\n\x1a\nframe")
+    entry = _entry({CONF_ROBOT_NAME: "Vector-ABCD", CONF_HOST: "192.168.1.10"})
+    entity = VectorNavMapCamera(coordinator, entry)
+
+    image = asyncio.run(entity.async_camera_image())
+    assert image == b"\x89PNG\r\n\x1a\nframe"
