feat: add safe Q10 goto lifecycle

hmmbob · hmmbob · commit 01d1fc5d0f55 · 2026-07-31T17:07:04.000+02:00
diff --git a/roborock/devices/traits/b01/q10/__init__.py b/roborock/devices/traits/b01/q10/__init__.py
@@ -89,7 +89,6 @@ def __init__(self, channel: B01Q10Channel) -> None:
         """Initialize the B01Props API."""
         self._channel = channel
         self.command = CommandTrait(channel)
-        self.vacuum = VacuumTrait(self.command)
         self.remote = RemoteTrait(self.command)
         self.status = StatusTrait()
         self.volume = SoundVolumeTrait(self.command)
@@ -101,6 +100,7 @@ def __init__(self, channel: B01Q10Channel) -> None:
         self.consumable = ConsumableTrait()
         self._map_dps = MapDpsTrait()
         self.map = MapContentTrait(self._map_dps)
+        self.vacuum = VacuumTrait(self.command, self.status, self.map)
         self.clean_history = CleanHistoryTrait(self.command)
         # Read-model traits updated from the device's DPS push stream.
         self._updatable_traits = [
@@ -122,6 +122,7 @@ async def start(self) -> None:
 
     async def close(self) -> None:
         """Close any resources held by the trait."""
+        await self.vacuum.close()
         if self._subscribe_task is not None:
             self._subscribe_task.cancel()
             try:
diff --git a/roborock/devices/traits/b01/q10/map.py b/roborock/devices/traits/b01/q10/map.py
@@ -122,6 +122,11 @@ def roborock_position(self) -> Q10Point | None:
             y=trace_to_roborock_coordinate(position.y),
         )
 
+    @property
+    def trace_sequence(self) -> int | None:
+        """Current cleaning-session sequence from the trace stream."""
+        return self._trace_packet.sequence if self._trace_packet else None
+
     @property
     def robot_heading(self) -> int | None:
         """Current heading for orienting a robot marker on a caller-rendered map."""
diff --git a/roborock/devices/traits/b01/q10/vacuum.py b/roborock/devices/traits/b01/q10/vacuum.py
@@ -1,20 +1,33 @@
 """Traits for Q10 B01 devices."""
 
+import asyncio
+import logging
 from base64 import b64encode
+from math import hypot
 from struct import error as StructError
 from struct import pack
 
 from roborock.data.b01_q10.b01_q10_code_mappings import (
     B01_Q10_DP,
     YXCleanType,
     YXDeviceCleanTask,
+    YXDeviceState,
     YXFanLevel,
 )
+from roborock.exceptions import RoborockException
 
 from .command import CommandTrait
 from .coordinates import roborock_to_vector_coordinate
+from .map import MapContentTrait
+from .status import StatusTrait
 
 _ZONE_NAME_FIELD_LENGTH = 19
+_GOTO_HALF_ZONE_SIZE = 200
+_GOTO_TOLERANCE = 200
+_GOTO_TIMEOUT = 300
+_GOTO_RETRY_INTERVAL = 1
+
+_LOGGER = logging.getLogger(__name__)
 
 
 def _encode_zone(x1: int, y1: int, x2: int, y2: int, clean_count: int) -> str:
@@ -56,9 +69,128 @@ class VacuumTrait:
     commands to Q10 devices.
     """
 
-    def __init__(self, command: CommandTrait) -> None:
+    def __init__(
+        self,
+        command: CommandTrait,
+        status: StatusTrait,
+        map_content: MapContentTrait,
+    ) -> None:
         """Initialize the VacuumTrait."""
         self._command = command
+        self._status = status
+        self._map = map_content
+        self._goto_monitor_task: asyncio.Task[None] | None = None
+        self._goto_trace_sequence: int | None = None
+
+    async def close(self) -> None:
+        """Cancel background work owned by the trait."""
+        if (task := self._goto_monitor_task) is None:
+            return
+        task.cancel()
+        try:
+            await task
+        except asyncio.CancelledError:
+            pass
+        self._goto_monitor_task = None
+        self._goto_trace_sequence = None
+
+    def cancel_goto(self) -> None:
+        """Cancel monitoring for an emulated goto replaced by another command."""
+        if self._goto_monitor_task is not None:
+            self._goto_monitor_task.cancel()
+            self._goto_monitor_task = None
+            self._goto_trace_sequence = None
+
+    async def _async_monitor_goto_target(
+        self,
+        x: int,
+        y: int,
+        previous_trace_sequence: int | None,
+    ) -> None:
+        """Pause the owned mini-zone task after it reaches the target."""
+        current_task = asyncio.current_task()
+        owned_trace_sequence: int | None = None
+        owned_task_seen = False
+        update_event = asyncio.Event()
+        remove_map_listener = self._map.add_update_listener(update_event.set)
+        remove_status_listener = self._status.add_update_listener(update_event.set)
+        try:
+            async with asyncio.timeout(_GOTO_TIMEOUT):
+                while True:
+                    trace_sequence = self._map.trace_sequence
+                    if owned_trace_sequence is None:
+                        if trace_sequence is not None and trace_sequence != previous_trace_sequence:
+                            owned_trace_sequence = trace_sequence
+                            self._goto_trace_sequence = trace_sequence
+                    elif trace_sequence != owned_trace_sequence:
+                        _LOGGER.debug("Q10 goto task was replaced by another cleaning session")
+                        return
+
+                    if (
+                        owned_trace_sequence is not None
+                        and self._status.clean_task_type is YXDeviceCleanTask.DIVIDE_AREAS
+                        and self._status.status
+                        not in {
+                            YXDeviceState.IDLE,
+                            YXDeviceState.PAUSED,
+                            YXDeviceState.RETURNING_HOME,
+                            YXDeviceState.CHARGING,
+                        }
+                    ):
+                        owned_task_seen = True
+
+                    if owned_task_seen and self._status.clean_task_type is not YXDeviceCleanTask.DIVIDE_AREAS:
+                        _LOGGER.debug("Q10 goto task was replaced by another task type")
+                        return
+
+                    if owned_task_seen and self._status.status in {
+                        YXDeviceState.IDLE,
+                        YXDeviceState.PAUSED,
+                        YXDeviceState.RETURNING_HOME,
+                        YXDeviceState.CHARGING,
+                    }:
+                        return
+
+                    if (
+                        owned_trace_sequence is not None
+                        and (position := self._map.roborock_position) is not None
+                        and hypot(position.x - x, position.y - y) <= _GOTO_TOLERANCE
+                    ):
+                        try:
+                            await self._command.send(command=B01_Q10_DP.PAUSE, params=0)
+                        except RoborockException as err:
+                            _LOGGER.warning("Failed to pause completed Q10 goto task; retrying: %s", err)
+                        else:
+                            return
+
+                    update_event.clear()
+                    try:
+                        async with asyncio.timeout(_GOTO_RETRY_INTERVAL):
+                            await update_event.wait()
+                    except TimeoutError:
+                        pass
+        except TimeoutError:
+            if (
+                owned_trace_sequence is not None
+                and self._map.trace_sequence == owned_trace_sequence
+                and self._status.clean_task_type is YXDeviceCleanTask.DIVIDE_AREAS
+            ):
+                _LOGGER.warning(
+                    "Q10 vacuum did not reach goto target (%s, %s) within %s seconds; stopping zone task",
+                    x,
+                    y,
+                    _GOTO_TIMEOUT,
+                )
+                try:
+                    await self._command.send(command=B01_Q10_DP.STOP, params=0)
+                except RoborockException as err:
+                    _LOGGER.warning("Failed to stop timed-out Q10 goto task: %s", err)
+        finally:
+            remove_map_listener()
+            remove_status_listener()
+            if self._goto_monitor_task is current_task:
+                self._goto_monitor_task = None
+                self._goto_trace_sequence = None
 
     async def start_clean(self) -> None:
         """Start a whole-home clean.
@@ -73,6 +205,7 @@ async def start_clean(self) -> None:
         whole-home clean (clean_task_type -> 1).
         """
         await self._command.send(command=B01_Q10_DP.START_CLEAN, params=1)
+        self.cancel_goto()
 
     async def clean_segments(self, segment_ids: list[int]) -> None:
         """Start a room / segment clean for the given segment (room) ids.
@@ -94,6 +227,7 @@ async def clean_segments(self, segment_ids: list[int]) -> None:
             # "parameters" -- the firmware only accepts that exact key.
             params={"cmd": YXDeviceCleanTask.ELECTORAL.code, "clean_paramters": segment_ids},
         )
+        self.cancel_goto()
 
     async def clean_zone(
         self,
@@ -105,33 +239,75 @@ async def clean_zone(
         clean_count: int = 1,
     ) -> None:
         """Clean one rectangular zone in the common Roborock coordinate space."""
+        encoded_zone = _encode_zone(x1, y1, x2, y2, clean_count)
         await self._command.send(
             command=B01_Q10_DP.START_CLEAN,
             params={
                 "cmd": YXDeviceCleanTask.DIVIDE_AREAS.code,
                 # "clean_paramters" is the spelling required by the firmware.
-                "clean_paramters": _encode_zone(x1, y1, x2, y2, clean_count),
+                "clean_paramters": encoded_zone,
+            },
+        )
+        self.cancel_goto()
+
+    async def goto_position(self, x: int, y: int) -> None:
+        """Move to a coordinate using an owned 40 cm zone-clean task."""
+        if (position := self._map.roborock_position) is not None and hypot(
+            position.x - x, position.y - y
+        ) <= _GOTO_TOLERANCE:
+            if (
+                self._goto_monitor_task is not None
+                and self._goto_trace_sequence is not None
+                and self._map.trace_sequence == self._goto_trace_sequence
+                and self._status.clean_task_type is YXDeviceCleanTask.DIVIDE_AREAS
+            ):
+                await self._command.send(command=B01_Q10_DP.PAUSE, params=0)
+                self.cancel_goto()
+            return
+
+        previous_trace_sequence = self._map.trace_sequence
+        encoded_zone = _encode_zone(
+            x - _GOTO_HALF_ZONE_SIZE,
+            y - _GOTO_HALF_ZONE_SIZE,
+            x + _GOTO_HALF_ZONE_SIZE,
+            y + _GOTO_HALF_ZONE_SIZE,
+            1,
+        )
+        await self._command.send(
+            command=B01_Q10_DP.START_CLEAN,
+            params={
+                "cmd": YXDeviceCleanTask.DIVIDE_AREAS.code,
+                "clean_paramters": encoded_zone,
             },
         )
+        self.cancel_goto()
+        self._goto_monitor_task = asyncio.create_task(
+            self._async_monitor_goto_target(x, y, previous_trace_sequence),
+            name="roborock_q10_goto",
+        )
 
     async def spot_clean(self) -> None:
         """Start a spot / part clean around the robot's current position.
 
         Verified live: ``{"dps": {"201": 5}}`` (clean_task_type -> 5).
         """
         await self._command.send(command=B01_Q10_DP.START_CLEAN, params=5)
+        self.cancel_goto()
 
     async def pause_clean(self) -> None:
         """Pause the current task. Verified live: ``{"dps": {"204": 0}}``."""
         await self._command.send(command=B01_Q10_DP.PAUSE, params=0)
+        self.cancel_goto()
 
     async def resume_clean(self) -> None:
         """Resume a paused task. Verified live: ``{"dps": {"205": 0}}``."""
         await self._command.send(command=B01_Q10_DP.RESUME, params=0)
+        self.cancel_goto()
 
     async def stop_clean(self) -> None:
         """Stop / cancel the current task. Verified live: ``{"dps": {"206": 0}}``."""
         await self._command.send(command=B01_Q10_DP.STOP, params=0)
+        self.cancel_goto()
 
     async def return_to_dock(self) -> None:
         """Send the robot back to the dock to charge.
@@ -142,6 +318,7 @@ async def return_to_dock(self) -> None:
         wash mop en route and ``4`` = collect dust en route.)
         """
         await self._command.send(command=B01_Q10_DP.START_BACK, params=5)
+        self.cancel_goto()
 
     async def empty_dustbin(self) -> None:
         """Empty the dustbin at the dock.
diff --git a/tests/devices/traits/b01/q10/test_map.py b/tests/devices/traits/b01/q10/test_map.py
@@ -81,6 +81,7 @@ def test_update_from_trace_packet_populates_path_and_position() -> None:
     assert (trait.robot_position.x, trait.robot_position.y) == (276, -1)
     assert trait.roborock_position is not None
     assert (trait.roborock_position.x, trait.roborock_position.y) == (26190, 25498)
+    assert trait.trace_sequence == trace.sequence
     assert trait.robot_heading == -34
     assert len(updates) == 1
 
diff --git a/tests/devices/traits/b01/q10/test_vacuum.py b/tests/devices/traits/b01/q10/test_vacuum.py
