Skip to content

Commit 49dd3f8

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 318dc35 commit 49dd3f8

10 files changed

Lines changed: 346 additions & 30 deletions

File tree

‎roborock/devices/device_manager.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -272,6 +272,7 @@ def device_creator(home_data: HomeData, device: HomeDataDevice, product: HomeDat
272272
device,
273273
rpc_channel=b01_q7_channel,
274274
map_rpc_channel=b01_q7_channel,
275+
push_channel=b01_q7_channel,
275276
)
276277
else:
277278
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
@@ -15,9 +15,11 @@
1515
CommandType,
1616
MapKey,
1717
ParamsType,
18+
Q7DevicePush,
1819
Q7RequestMessage,
1920
create_map_key,
2021
create_map_response_assembler,
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
async def _send_command(
6272
mqtt_channel: MqttChannel,
6373
request_message: Q7RequestMessage,
@@ -155,7 +165,7 @@ def find_response(response_message: RoborockMessage) -> DecodedB01Response | Non
155165
raise
156166

157167

158-
class B01Q7Channel(Channel, Q7RpcChannel, Q7MapRpcChannel):
168+
class B01Q7Channel(Channel, Q7RpcChannel, Q7MapRpcChannel, Q7PushChannel):
159169
"""Unified B01 Q7 channel wrapping MQTT transport."""
160170

161171
def __init__(self, mqtt_channel: MqttChannel, map_key: MapKey) -> None:
@@ -222,6 +232,15 @@ def on_message(message: RoborockMessage) -> None:
222232

223233
return await self._mqtt_channel.subscribe(on_message)
224234

235+
async def subscribe_device_pushes(self, callback: Callable[[Q7DevicePush], None]) -> Callable[[], None]:
236+
"""Subscribe to unsolicited ``*.post`` messages (property changes, live path, events)."""
237+
238+
def on_message(message: RoborockMessage) -> None:
239+
if (push := decode_device_push(message)) is not None:
240+
callback(push)
241+
242+
return await self._mqtt_channel.subscribe(on_message)
243+
225244

226245
def create_b01_q7_channel(
227246
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

@@ -60,12 +60,14 @@ def __init__(
6060
self,
6161
rpc_channel: Q7RpcChannel,
6262
map_rpc_channel: Q7MapRpcChannel,
63+
push_channel: Q7PushChannel,
6364
device: HomeDataDevice,
6465
product: HomeDataProduct,
6566
) -> None:
6667
"""Initialize the Q7 API."""
6768
self._rpc_channel = rpc_channel
6869
self._map_rpc_channel = map_rpc_channel
70+
self._push_channel = push_channel
6971
self._device = device
7072
self._product = product
7173

@@ -80,18 +82,27 @@ def __init__(
8082
self.map,
8183
)
8284
self._unsub_map_pushes: Callable[[], None] | None = None
85+
self._unsub_device_pushes: Callable[[], None] | None = None
8386

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

9094
async def close(self) -> None:
91-
"""Stop listening for unsolicited map pushes."""
95+
"""Stop listening for unsolicited map frames and device pushes."""
9296
if self._unsub_map_pushes is not None:
9397
self._unsub_map_pushes()
9498
self._unsub_map_pushes = None
99+
if self._unsub_device_pushes is not None:
100+
self._unsub_device_pushes()
101+
self._unsub_device_pushes = None
102+
103+
def _handle_device_push(self, push: Q7DevicePush) -> None:
104+
"""Route an unsolicited device push to the state that depends on it."""
105+
self.map_content.update_from_device_push(push)
95106

96107
async def query_values(self, props: list[RoborockB01Props]) -> B01Props | None:
97108
"""Query the device for the values of the given Q7 properties."""
@@ -278,6 +289,7 @@ def create(
278289
device: HomeDataDevice,
279290
rpc_channel: Q7RpcChannel,
280291
map_rpc_channel: Q7MapRpcChannel,
292+
push_channel: Q7PushChannel,
281293
) -> Q7PropertiesApi:
282294
"""Create traits for B01 Q7 devices."""
283295
if device.sn is None or product.model is None:
@@ -287,6 +299,7 @@ def create(
287299
return Q7PropertiesApi(
288300
rpc_channel=rpc_channel,
289301
map_rpc_channel=map_rpc_channel,
302+
push_channel=push_channel,
290303
device=device,
291304
product=product,
292305
)

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

Lines changed: 63 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 ROBOT_WORLD_POSITION, B01MapParser, B01MapParserConfig, parse_map_type
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,20 @@ 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+
if parsed_data.map_data is None:
180+
return
181+
if (pose := parsed_data.map_data.additional_parameters.get(ROBOT_WORLD_POSITION)) is not None:
182+
self.robot_position = Q7RoborockPoint.from_world(*pose)
183+
184+
def update_from_device_push(self, push: Q7DevicePush) -> None:
185+
"""Update the robot position from a live ``cur_path`` push and notify listeners."""
186+
if push.method != "prop.post" or not isinstance(cur_path := push.params.get("cur_path"), list):
187+
return
188+
if (position := latest_position_from_cur_path(cur_path)) is None:
189+
_LOGGER.debug("Ignoring malformed Q7 cur_path push: %s", cur_path)
190+
return
191+
if position == self.robot_position:
192+
return
193+
self.robot_position = position
194+
self._notify_update()

‎roborock/map/b01_map_parser.py‎

Lines changed: 23 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,9 @@
2727
_FLOOR = 127
2828
_WALL = 128
2929

30+
ROBOT_WORLD_POSITION = "robot_world_position"
31+
"""``MapData.additional_parameters`` key holding the valid live robot pose ``(x, y)`` in world meters."""
32+
3033
_B01_DRAWABLES = [
3134
Drawable.CHARGER,
3235
Drawable.NO_GO_AREAS,
@@ -163,6 +166,21 @@ def to_pixel(self, x: float, y: float) -> tuple[float, float]:
163166
return px, py
164167

165168

169+
def _valid_current_pose(parsed: RobotMap, projector: _WorldToPixel) -> tuple[float, float, float] | None:
170+
"""Return the live robot pose ``(x, y, phi)`` in world meters, if the frame has a valid one.
171+
172+
Frames without a live pose yield ``None``, as do the placeholders the firmware
173+
reports while it has no fix: ``(1100.0, 1100.0)`` (outside the map bounds) and
174+
an exact ``(0.0, 0.0)`` while a task is starting.
175+
"""
176+
if not parsed.HasField("currentPose"):
177+
return None
178+
pose = parsed.currentPose
179+
if (pose.x == 0.0 and pose.y == 0.0) or not projector.in_bounds(pose.x, pose.y):
180+
return None
181+
return pose.x, pose.y, pose.phi
182+
183+
166184
def _place_poses(map_data: MapData, parsed: RobotMap, projector: _WorldToPixel) -> bool:
167185
"""Populate charger, robot position and path from the decoded SCMap."""
168186
has_drawables = False
@@ -172,9 +190,11 @@ def _place_poses(map_data: MapData, parsed: RobotMap, projector: _WorldToPixel)
172190
map_data.charger = Point(px, py, math.degrees(parsed.chargeStation.phi))
173191
has_drawables = True
174192

175-
if parsed.HasField("currentPose") and projector.in_bounds(parsed.currentPose.x, parsed.currentPose.y):
176-
px, py = projector.to_pixel(parsed.currentPose.x, parsed.currentPose.y)
177-
map_data.vacuum_position = Point(px, py, math.degrees(parsed.currentPose.phi))
193+
if (pose := _valid_current_pose(parsed, projector)) is not None:
194+
x, y, phi = pose
195+
px, py = projector.to_pixel(x, y)
196+
map_data.vacuum_position = Point(px, py, math.degrees(phi))
197+
map_data.additional_parameters[ROBOT_WORLD_POSITION] = (x, y)
178198
has_drawables = True
179199
elif map_data.charger is not None:
180200
# A saved map carries no live pose; show the robot at its dock.

‎roborock/protocols/b01_q7_protocol.py‎

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

9292

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

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

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

361372

373+
async def test_subscribe_device_pushes_delivers_post_messages(
374+
device: HomeDataDevice,
375+
product: HomeDataProduct,
376+
fake_channel: FakeChannel,
377+
message_builder: B01MessageBuilder,
378+
) -> None:
379+
"""``*.post`` messages are delivered; command responses and map frames are not."""
380+
channel = create_b01_q7_channel(device, product, fake_channel) # type: ignore[arg-type]
381+
382+
received: list[Q7DevicePush] = []
383+
unsub = await channel.subscribe_device_pushes(received.append)
384+
385+
fake_channel.notify_subscribers(message_builder.build_push("prop.post", {"status": 5, "fault": 2110}))
386+
fake_channel.notify_subscribers(message_builder.build_push("event.startClean.post", {}))
387+
fake_channel.notify_subscribers(message_builder.build({"status": 1}))
388+
fake_channel.notify_subscribers(message_builder.build_map_response(b"raw-map-payload"))
389+
390+
assert received == [
391+
Q7DevicePush(method="prop.post", params={"status": 5, "fault": 2110}),
392+
Q7DevicePush(method="event.startClean.post", params={}),
393+
]
394+
395+
unsub()
396+
fake_channel.notify_subscribers(message_builder.build_push("prop.post", {"quantity": 89}))
397+
assert len(received) == 2
398+
399+
362400
async def test_subscribe_map_pushes_skips_undecodable_frames(
363401
device: HomeDataDevice,
364402
product: HomeDataProduct,

0 commit comments

Comments
 (0)