Skip to content

Commit 42e117a

Browse files
committed
feat: expose Q7 robot position
Add `MapContentTrait.robot_position` (common Roborock coordinates) for Q7 devices, kept up to date from two sources: - the current pose in live map frames (`currentPose`), ignoring the `(1100, 1100)` and `(0, 0)` placeholders the firmware reports when the pose is unknown or outside the map; - `cur_path` samples in unsolicited `prop.post` device pushes, which the device streams while a task is running. To receive those pushes, `B01Q7Channel` now implements a `Q7PushChannel` protocol that decodes `*.post` messages into `Q7DevicePush`, and `Q7PropertiesApi.start()` subscribes to them. Add `MapContentTrait.refresh_live()`, which requests the live map by type (`service.upload_by_maptype`). Unlike the saved map it carries the current pose and makes the device stream `cur_path` positions.
1 parent 63c9290 commit 42e117a

9 files changed

Lines changed: 325 additions & 17 deletions

File tree

‎roborock/devices/device_manager.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -282,6 +282,7 @@ def device_creator(home_data: HomeData, device: HomeDataDevice, product: HomeDat
282282
device,
283283
rpc_channel=b01_q7_channel,
284284
map_rpc_channel=b01_q7_channel,
285+
push_channel=b01_q7_channel,
285286
)
286287
else:
287288
raise UnsupportedDeviceError(f"Device {device.name} has unsupported B01 model: {product.model}")

‎roborock/devices/rpc/b01_q7_channel.py‎

Lines changed: 20 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,10 @@
1616
CommandType,
1717
MapKey,
1818
ParamsType,
19+
Q7DevicePush,
1920
Q7RequestMessage,
2021
create_map_key,
22+
decode_device_push,
2123
decode_map_payload,
2224
decode_rpc_response,
2325
encode_mqtt_payload,
@@ -58,6 +60,14 @@ async def subscribe_map_pushes(self, callback: Callable[[bytes], None]) -> Calla
5860
...
5961

6062

63+
class Q7PushChannel(Protocol):
64+
"""Protocol for channels delivering unsolicited Q7 device pushes."""
65+
66+
async def subscribe_device_pushes(self, callback: Callable[[Q7DevicePush], None]) -> Callable[[], None]:
67+
"""Subscribe to ``*.post`` messages the device publishes on its own."""
68+
...
69+
70+
6171
def _matches_map_response(response_message: RoborockMessage, *, version: bytes | None) -> bytes | None:
6272
"""Return raw map payload bytes for matching MAP_RESPONSE messages."""
6373
if (
@@ -166,7 +176,7 @@ def find_response(response_message: RoborockMessage) -> DecodedB01Response | Non
166176
raise
167177

168178

169-
class B01Q7Channel(Channel, Q7RpcChannel, Q7MapRpcChannel):
179+
class B01Q7Channel(Channel, Q7RpcChannel, Q7MapRpcChannel, Q7PushChannel):
170180
"""Unified B01 Q7 channel wrapping MQTT transport."""
171181

172182
def __init__(self, mqtt_channel: MqttChannel, map_key: MapKey) -> None:
@@ -231,6 +241,15 @@ def on_message(message: RoborockMessage) -> None:
231241

232242
return await self._mqtt_channel.subscribe(on_message)
233243

244+
async def subscribe_device_pushes(self, callback: Callable[[Q7DevicePush], None]) -> Callable[[], None]:
245+
"""Subscribe to unsolicited ``*.post`` messages (property changes, live path, events)."""
246+
247+
def on_message(message: RoborockMessage) -> None:
248+
if (push := decode_device_push(message)) is not None:
249+
callback(push)
250+
251+
return await self._mqtt_channel.subscribe(on_message)
252+
234253

235254
def create_b01_q7_channel(
236255
device: HomeDataDevice,

‎roborock/devices/traits/b01/q7/__init__.py‎

Lines changed: 20 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -17,10 +17,10 @@
1717
SCWindMapping,
1818
WaterLevelMapping,
1919
)
20-
from roborock.devices.rpc.b01_q7_channel import Q7MapRpcChannel, Q7RpcChannel
20+
from roborock.devices.rpc.b01_q7_channel import Q7MapRpcChannel, Q7PushChannel, Q7RpcChannel
2121
from roborock.devices.traits import Trait
2222
from roborock.exceptions import RoborockException
23-
from roborock.protocols.b01_q7_protocol import CommandType, ParamsType
23+
from roborock.protocols.b01_q7_protocol import CommandType, ParamsType, Q7DevicePush
2424
from roborock.roborock_message import RoborockB01Props
2525
from roborock.roborock_typing import RoborockB01Q7Methods
2626

@@ -63,12 +63,14 @@ def __init__(
6363
self,
6464
rpc_channel: Q7RpcChannel,
6565
map_rpc_channel: Q7MapRpcChannel,
66+
push_channel: Q7PushChannel,
6667
device: HomeDataDevice,
6768
product: HomeDataProduct,
6869
) -> None:
6970
"""Initialize the Q7 API."""
7071
self._rpc_channel = rpc_channel
7172
self._map_rpc_channel = map_rpc_channel
73+
self._push_channel = push_channel
7274
self._device = device
7375
self._product = product
7476

@@ -83,18 +85,27 @@ def __init__(
8385
self.map,
8486
)
8587
self._unsub_map_pushes: Callable[[], None] | None = None
88+
self._unsub_device_pushes: Callable[[], None] | None = None
8689

8790
async def start(self) -> None:
88-
"""Start listening for unsolicited map pushes from the device."""
89-
if self._unsub_map_pushes is not None:
90-
return
91-
self._unsub_map_pushes = await self._map_rpc_channel.subscribe_map_pushes(self.map_content.update_from_push)
91+
"""Start listening for unsolicited map frames and device pushes."""
92+
if self._unsub_map_pushes is None:
93+
self._unsub_map_pushes = await self._map_rpc_channel.subscribe_map_pushes(self.map_content.update_from_push)
94+
if self._unsub_device_pushes is None:
95+
self._unsub_device_pushes = await self._push_channel.subscribe_device_pushes(self._handle_device_push)
9296

9397
async def close(self) -> None:
94-
"""Stop listening for unsolicited map pushes."""
98+
"""Stop listening for unsolicited map frames and device pushes."""
9599
if self._unsub_map_pushes is not None:
96100
self._unsub_map_pushes()
97101
self._unsub_map_pushes = None
102+
if self._unsub_device_pushes is not None:
103+
self._unsub_device_pushes()
104+
self._unsub_device_pushes = None
105+
106+
def _handle_device_push(self, push: Q7DevicePush) -> None:
107+
"""Route an unsolicited device push to the state that depends on it."""
108+
self.map_content.update_from_device_push(push)
98109

99110
async def query_values(self, props: list[RoborockB01Props]) -> B01Props | None:
100111
"""Query the device for the values of the given Q7 properties."""
@@ -281,6 +292,7 @@ def create(
281292
device: HomeDataDevice,
282293
rpc_channel: Q7RpcChannel,
283294
map_rpc_channel: Q7MapRpcChannel,
295+
push_channel: Q7PushChannel,
284296
) -> Q7PropertiesApi:
285297
"""Create traits for B01 Q7 devices."""
286298
if device.sn is None or product.model is None:
@@ -290,6 +302,7 @@ def create(
290302
return Q7PropertiesApi(
291303
rpc_channel=rpc_channel,
292304
map_rpc_channel=map_rpc_channel,
305+
push_channel=push_channel,
293306
device=device,
294307
product=product,
295308
)

‎roborock/devices/traits/b01/q7/map_content.py‎

Lines changed: 66 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -15,15 +15,17 @@
1515
import asyncio
1616
import logging
1717
from dataclasses import dataclass
18+
from typing import Any
1819

1920
from vacuum_map_parser_base.map_data import MapData
2021

21-
from roborock.data import RoborockBase
22+
from roborock.data import Q7RoborockPoint, RoborockBase
2223
from roborock.devices.rpc.b01_q7_channel import Q7MapRpcChannel
2324
from roborock.devices.traits import Trait
2425
from roborock.devices.traits.common import TraitUpdateListener
2526
from roborock.exceptions import RoborockException
26-
from roborock.map.b01_map_parser import B01MapParser, B01MapParserConfig, parse_map_type
27+
from roborock.map.b01_map_parser import B01MapParser, B01MapParserConfig, parse_map_type, parse_robot_pose
28+
from roborock.protocols.b01_q7_protocol import Q7DevicePush
2729
from roborock.roborock_typing import RoborockB01Q7Methods
2830

2931
from .map import MapTrait
@@ -50,11 +52,34 @@ class MapContent(RoborockBase):
5052
library to re-parse the map data when needed.
5153
"""
5254

55+
robot_position: Q7RoborockPoint | None = None
56+
"""Latest known robot position in common Roborock coordinates.
57+
58+
Updated from map frames and, while the robot moves, from the live
59+
``cur_path`` pushes the device publishes roughly once per second.
60+
"""
61+
5362
def __repr__(self) -> str:
5463
img = self.image_content
5564
if img and len(img) > _TRUNCATE_LENGTH:
5665
img = img[: _TRUNCATE_LENGTH - 3] + b"..."
57-
return f"MapContent(image_content={img!r}, map_data={self.map_data!r})"
66+
return f"MapContent(image_content={img!r}, map_data={self.map_data!r}, robot_position={self.robot_position!r})"
67+
68+
69+
def latest_position_from_cur_path(cur_path: list[Any]) -> Q7RoborockPoint | None:
70+
"""Return the newest pose of a ``cur_path`` push, or ``None`` if malformed.
71+
72+
The device sends ``[start_index, x, y, phi, kind, (x, y, phi, kind)..., timestamp]``
73+
with world coordinates in meters; ``kind`` is 0 while travelling and 5 while
74+
cleaning.
75+
"""
76+
samples = cur_path[1:-1]
77+
if not samples or len(samples) % 4:
78+
return None
79+
x, y = samples[-4], samples[-3]
80+
if not all(isinstance(value, int | float) and not isinstance(value, bool) for value in (x, y)):
81+
return None
82+
return Q7RoborockPoint.from_world(x, y)
5883

5984

6085
class MapContentTrait(MapContent, Trait, TraitUpdateListener):
@@ -93,6 +118,24 @@ async def refresh(self) -> None:
93118

94119
self._parse_and_store(raw_payload)
95120

121+
async def refresh_live(self) -> None:
122+
"""Fetch and parse the live map of the current task, including the robot pose.
123+
124+
Unlike :meth:`refresh` (the saved map, which carries no live pose), this
125+
requests the live map (``service.upload_by_maptype`` with map type 0), as the
126+
official app does. Requesting it also makes the device stream ``cur_path``
127+
pushes while it moves (verified on a Q7 sc05: without a live-map request no
128+
``cur_path`` is published), keeping :attr:`robot_position` current about
129+
once per second.
130+
"""
131+
async with self._map_command_lock:
132+
raw_payload = await self._map_rpc_channel.send_map_command(
133+
RoborockB01Q7Methods.UPLOAD_BY_MAPTYPE,
134+
{"map_type": _LIVE_MAP_TYPE},
135+
)
136+
137+
self._parse_and_store(raw_payload)
138+
96139
def update_from_push(self, raw_payload: bytes) -> None:
97140
"""Store an unsolicited SCMap frame pushed by the device during cleaning.
98141
@@ -132,3 +175,23 @@ def _parse_and_store(self, raw_payload: bytes) -> None:
132175
self.image_content = parsed_data.image_content
133176
self.map_data = parsed_data.map_data
134177
self.raw_api_response = raw_payload
178+
# Saved maps and frames without a fix carry no usable pose: keep the last known one.
179+
try:
180+
pose = parse_robot_pose(raw_payload)
181+
except RoborockException as ex:
182+
_LOGGER.debug("Failed to read robot pose from B01 map frame: %s", ex)
183+
return
184+
if pose is not None:
185+
self.robot_position = Q7RoborockPoint.from_world(*pose)
186+
187+
def update_from_device_push(self, push: Q7DevicePush) -> None:
188+
"""Update the robot position from a live ``cur_path`` push and notify listeners."""
189+
if push.method != "prop.post" or not isinstance(cur_path := push.params.get("cur_path"), list):
190+
return
191+
if (position := latest_position_from_cur_path(cur_path)) is None:
192+
_LOGGER.debug("Ignoring malformed Q7 cur_path push: %s", cur_path)
193+
return
194+
if position == self.robot_position:
195+
return
196+
self.robot_position = position
197+
self._notify_update()

‎roborock/map/b01_map_parser.py‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -113,6 +113,22 @@ def parse_map_type(payload: bytes) -> int:
113113
return _parse_scmap_payload(payload).mapType
114114

115115

116+
def parse_robot_pose(payload: bytes) -> tuple[float, float] | None:
117+
"""Return the robot's world position (meters) from an SCMap frame, if valid.
118+
119+
Frames without a live pose yield ``None``, as do the placeholders the firmware
120+
reports while it has no fix: ``(1100.0, 1100.0)`` (outside the map bounds) and
121+
an exact ``(0.0, 0.0)`` while a task is starting.
122+
"""
123+
parsed = _parse_scmap_payload(payload)
124+
if not parsed.HasField("mapHead") or not parsed.HasField("currentPose"):
125+
return None
126+
pose = parsed.currentPose
127+
if (pose.x == 0.0 and pose.y == 0.0) or not _WorldToPixel(parsed).in_bounds(pose.x, pose.y):
128+
return None
129+
return pose.x, pose.y
130+
131+
116132
def _parse_scmap_payload(payload: bytes) -> RobotMap:
117133
"""Parse inflated SCMap bytes into a generated protobuf message."""
118134
parsed = RobotMap()

‎roborock/protocols/b01_q7_protocol.py‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -89,6 +89,47 @@ def decode_rpc_response(message: RoborockMessage) -> dict[int, Any]:
8989
) from err
9090

9191

92+
@dataclass
93+
class Q7DevicePush:
94+
"""An unsolicited message the device publishes on its own.
95+
96+
Examples observed on a Q7 (sc05): ``prop.post`` with changed properties
97+
(``{"status": 5, "fault": 2110}``, ``{"cur_path": [...]}``) and events such
98+
as ``event.startClean.post``.
99+
"""
100+
101+
method: str
102+
params: dict[str, Any]
103+
104+
105+
def decode_device_push(message: RoborockMessage) -> Q7DevicePush | None:
106+
"""Return the device push carried by ``message``, or ``None`` if it is not one.
107+
108+
Pushes are distinguished from command responses by their ``*.post`` method
109+
and their ``params`` payload (responses carry ``data`` and a ``code``).
110+
"""
111+
if message.protocol != RoborockMessageProtocol.RPC_RESPONSE or message.version != B01_VERSION:
112+
return None
113+
try:
114+
datapoints = decode_rpc_response(message)
115+
except RoborockException:
116+
return None
117+
for value in datapoints.values():
118+
if not isinstance(value, str):
119+
continue
120+
try:
121+
inner = json.loads(value)
122+
except json.JSONDecodeError:
123+
continue
124+
if not isinstance(inner, dict):
125+
continue
126+
method = inner.get("method")
127+
params = inner.get("params")
128+
if isinstance(method, str) and method.endswith(".post") and isinstance(params, dict):
129+
return Q7DevicePush(method=method, params=params)
130+
return None
131+
132+
92133
@dataclass
93134
class MapKey:
94135
"""Data class for holding a B01 map decryption key."""

‎tests/devices/rpc/test_b01_q7_channel.py‎

Lines changed: 39 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
send_decoded_command,
1717
)
1818
from roborock.exceptions import RoborockException
19-
from roborock.protocols.b01_q7_protocol import B01_VERSION, Q7RequestMessage
19+
from roborock.protocols.b01_q7_protocol import B01_VERSION, Q7DevicePush, Q7RequestMessage
2020
from roborock.roborock_message import RoborockMessage, RoborockMessageProtocol
2121
from tests.fixtures.channel_fixtures import FakeChannel
2222

@@ -52,6 +52,17 @@ def _build_dps(self, message: dict[str, Any] | str) -> RoborockMessage:
5252
seq=self.seq,
5353
)
5454

55+
def build_push(self, method: str, params: dict[str, Any]) -> RoborockMessage:
56+
"""Build an unsolicited device push (``*.post``) as published on dps 10001."""
57+
self.seq += 1
58+
message = {"msgId": "323022", "method": method, "params": params}
59+
return RoborockMessage(
60+
protocol=RoborockMessageProtocol.RPC_RESPONSE,
61+
payload=pad(json.dumps({"dps": {"10001": json.dumps(message)}}).encode(), AES.block_size),
62+
version=b"B01",
63+
seq=self.seq,
64+
)
65+
5566
def build_map_response(self, payload: bytes) -> RoborockMessage:
5667
"""Build a dummy MAP_RESPONSE message."""
5768
self.seq += 1
@@ -329,6 +340,33 @@ async def test_subscribe_map_pushes_delivers_decoded_frames(
329340
assert received == [b"inflated-payload"]
330341

331342

343+
async def test_subscribe_device_pushes_delivers_post_messages(
344+
device: HomeDataDevice,
345+
product: HomeDataProduct,
346+
fake_channel: FakeChannel,
347+
message_builder: B01MessageBuilder,
348+
) -> None:
349+
"""``*.post`` messages are delivered; command responses and map frames are not."""
350+
channel = create_b01_q7_channel(device, product, fake_channel) # type: ignore[arg-type]
351+
352+
received: list[Q7DevicePush] = []
353+
unsub = await channel.subscribe_device_pushes(received.append)
354+
355+
fake_channel.notify_subscribers(message_builder.build_push("prop.post", {"status": 5, "fault": 2110}))
356+
fake_channel.notify_subscribers(message_builder.build_push("event.startClean.post", {}))
357+
fake_channel.notify_subscribers(message_builder.build({"status": 1}))
358+
fake_channel.notify_subscribers(message_builder.build_map_response(b"raw-map-payload"))
359+
360+
assert received == [
361+
Q7DevicePush(method="prop.post", params={"status": 5, "fault": 2110}),
362+
Q7DevicePush(method="event.startClean.post", params={}),
363+
]
364+
365+
unsub()
366+
fake_channel.notify_subscribers(message_builder.build_push("prop.post", {"quantity": 89}))
367+
assert len(received) == 2
368+
369+
332370
async def test_subscribe_map_pushes_skips_undecodable_frames(
333371
device: HomeDataDevice,
334372
product: HomeDataProduct,

0 commit comments

Comments
 (0)