Skip to content

Commit f0341e8

Browse files
hCoureauHarry Coureau
andauthored
feat: render Q10 map obstacle markers (#938)
* feat: render Q10 map obstacle markers * test: separate Q10 obstacle and historical trace assertions --------- Co-authored-by: Harry Coureau <hcoureau@amazon.com>
1 parent 16e7970 commit f0341e8

7 files changed

Lines changed: 212 additions & 36 deletions

File tree

‎roborock/devices/device_manager.py‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
from roborock.devices.device import DeviceReadyCallback, RoborockDevice
2020
from roborock.diagnostics import Diagnostics, redact_device_data
2121
from roborock.exceptions import RoborockException
22+
from roborock.map.b01_q10_map_parser import B01Q10MapParserConfig
2223
from roborock.map.map_parser import MapParserConfig
2324
from roborock.mqtt.roborock_session import create_lazy_mqtt_session
2425
from roborock.mqtt.session import MqttSession, SessionUnauthorizedHook
@@ -262,7 +263,16 @@ def device_creator(home_data: HomeData, device: HomeDataDevice, product: HomeDat
262263
if "ss" in model_part:
263264
b01_q10_channel = create_b01_q10_channel(mqtt_channel)
264265
channel = b01_q10_channel
265-
trait = b01.q10.create(channel)
266+
trait = b01.q10.create(
267+
channel,
268+
map_parser_config=(
269+
B01Q10MapParserConfig(
270+
map_scale=map_parser_config.map_scale, drawables=map_parser_config.drawables
271+
)
272+
if map_parser_config
273+
else None
274+
),
275+
)
266276
elif "sc" in model_part:
267277
# Q7 devices start with 'sc' in their model naming.
268278
b01_q7_channel = create_b01_q7_channel(device, product, mqtt_channel)

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
from roborock.data.containers import RoborockBase
99
from roborock.devices.rpc.b01_q10_channel import B01Q10Channel
1010
from roborock.devices.traits import Trait
11-
from roborock.map.b01_q10_map_parser import Q10MapPacket, Q10TracePacket
11+
from roborock.map.b01_q10_map_parser import B01Q10MapParserConfig, Q10MapPacket, Q10TracePacket
1212
from roborock.protocols.b01_q10_protocol import Q10DpsUpdate, Q10Message
1313

1414
from .button_light import ButtonLightTrait
@@ -92,7 +92,7 @@ class Q10PropertiesApi(Trait):
9292
clean_history: CleanHistoryTrait
9393
"""Trait for fetching the device clean-record history (``dpCleanRecord``)."""
9494

95-
def __init__(self, channel: B01Q10Channel) -> None:
95+
def __init__(self, channel: B01Q10Channel, *, map_parser_config: B01Q10MapParserConfig | None = None) -> None:
9696
"""Initialize the B01Props API."""
9797
self._channel = channel
9898
self.command = CommandTrait(channel)
@@ -107,7 +107,7 @@ def __init__(self, channel: B01Q10Channel) -> None:
107107
self.consumable = ConsumableTrait()
108108
self._map_dps = MapDpsTrait()
109109
self.maps = MapsTrait(self.command)
110-
self.map = MapContentTrait(self._map_dps, self.maps, self.command)
110+
self.map = MapContentTrait(self._map_dps, self.maps, self.command, map_parser_config=map_parser_config)
111111
self.vacuum = VacuumTrait(self.command, self.status, self.map)
112112
self.clean_history = CleanHistoryTrait(self.command)
113113
# Read-model traits updated from the device's DPS push stream.
@@ -179,6 +179,6 @@ def as_dict(self) -> dict[str, Any]:
179179
return result
180180

181181

182-
def create(channel: B01Q10Channel) -> Q10PropertiesApi:
182+
def create(channel: B01Q10Channel, *, map_parser_config: B01Q10MapParserConfig | None = None) -> Q10PropertiesApi:
183183
"""Create traits for B01 devices."""
184-
return Q10PropertiesApi(channel)
184+
return Q10PropertiesApi(channel, map_parser_config=map_parser_config)

‎roborock/map/b01_q10_map_parser.py‎

Lines changed: 65 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,8 @@
2525
import struct
2626
from dataclasses import dataclass, field, replace
2727

28+
from vacuum_map_parser_base.config.drawable import Drawable
29+
2830
from roborock.data.b01_q10.b01_q10_containers import Q10RoborockPoint
2931
from roborock.data.code_mappings import RoborockEnum
3032
from roborock.data.containers import RoborockBase
@@ -209,6 +211,30 @@ def from_payload(cls, payload: bytes) -> "Q10MapPacketKind | None":
209211
return None if kind is cls.unknown else kind
210212

211213

214+
@dataclass
215+
class Q10Point(RoborockBase):
216+
"""A point in the Q10 firmware's dock-relative trace coordinate space."""
217+
218+
x: int
219+
y: int
220+
221+
def to_roborock(self) -> Q10RoborockPoint:
222+
"""Convert this trace point to common Roborock coordinates."""
223+
return Q10RoborockPoint.from_trace(self.x, self.y)
224+
225+
226+
@dataclass
227+
class Q10Obstacle(RoborockBase):
228+
"""A position-only obstacle in the map package's coordinate frame.
229+
230+
Fifty raw units equal one grid pixel. These coordinates use the map header
231+
origin and carry no validated obstacle type, confidence, or photo identifier.
232+
"""
233+
234+
x: int
235+
y: int
236+
237+
212238
@dataclass
213239
class Q10MapPacket:
214240
"""Decoded contents of a Q10 current or archived map packet."""
@@ -228,6 +254,11 @@ class Q10MapPacket:
228254
the same (top-down) pixel space as :attr:`grid`, where a non-zero cell is
229255
carpet (the value is the carpet kind). ``None`` if the packet carried none."""
230256

257+
obstacles: list[Q10Obstacle] = field(default_factory=list)
258+
"""Obstacle positions embedded after the carpet mask (50 raw units/pixel)."""
259+
skip_cleaning_points: list[Q10Point] = field(default_factory=list)
260+
"""Skip-clean markers in their distinct firmware coordinate frame."""
261+
231262
@property
232263
def layers(self) -> GridLayers:
233264
"""Split the occupancy grid into separable grid-pixel layers."""
@@ -236,18 +267,6 @@ def layers(self) -> GridLayers:
236267
return decompose_grid(self.width, self.height, self.grid, rooms, classify_q10_cell, flip=False)
237268

238269

239-
@dataclass
240-
class Q10Point(RoborockBase):
241-
"""A point in the Q10 firmware's dock-relative trace coordinate space."""
242-
243-
x: int
244-
y: int
245-
246-
def to_roborock(self) -> Q10RoborockPoint:
247-
"""Convert this trace point to common Roborock coordinates."""
248-
return Q10RoborockPoint.from_trace(self.x, self.y)
249-
250-
251270
@dataclass
252271
class Q10TracePacket:
253272
"""Decoded contents of a Q10 ``02 01`` cleaning-path packet.
@@ -335,12 +354,11 @@ class Q10CleanRecordDetail:
335354
_TRACE_POINT_COUNT_OFFSET = 8
336355
_TRACE_HEADING_OFFSET = 10
337356

338-
_HISTORICAL_TRACE_HEADER_LENGTH = 14
339-
_HISTORICAL_TRACE_PREFIX_LENGTH = 1
357+
_HISTORICAL_TRACE_HEADER_LENGTH = 13
340358
_HISTORICAL_TRACE_VERSION = 1
341-
_HISTORICAL_TRACE_POINT_COUNT_OFFSET = 6
342-
_HISTORICAL_TRACE_HEADING_OFFSET = 10
343-
_HISTORICAL_TRACE_RESERVED_OFFSET = 12
359+
_HISTORICAL_TRACE_POINT_COUNT_OFFSET = 5
360+
_HISTORICAL_TRACE_HEADING_OFFSET = 9
361+
_HISTORICAL_TRACE_RESERVED_OFFSET = 11
344362

345363
# Some cleans still prepend a single near-origin sentinel as the first real
346364
# point (e.g. ~(5, 76) / (-3, 0) when the path proper starts near (-1700, -800));
@@ -572,6 +590,17 @@ def _parse_map_layout(payload: bytes) -> tuple[Q10MapPacket, bytes, int | None]:
572590
tail = payload[layout_end:]
573591
erase_zones = _parse_erase_zones(tail)
574592
carpet_mask, carpet_end = _parse_carpet_block(tail, width, height)
593+
obstacles: list[Q10Obstacle] = []
594+
skip_cleaning_points: list[Q10Point] = []
595+
trace_offset = None
596+
if carpet_end is not None:
597+
obstacle_coords, obstacle_end = _parse_counted_points(tail, carpet_end)
598+
if obstacle_end is not None:
599+
skip_coords, skip_end = _parse_counted_points(tail, obstacle_end)
600+
if skip_end is not None:
601+
obstacles = [Q10Obstacle(x, y) for x, y in obstacle_coords]
602+
skip_cleaning_points = [Q10Point(x, y) for x, y in skip_coords]
603+
trace_offset = skip_end
575604
header_calibration = _parse_header_calibration(payload)
576605
packet = Q10MapPacket(
577606
kind=kind,
@@ -583,8 +612,10 @@ def _parse_map_layout(payload: bytes) -> tuple[Q10MapPacket, bytes, int | None]:
583612
erase_zones=erase_zones,
584613
header_calibration=header_calibration,
585614
carpet_mask=carpet_mask,
615+
obstacles=obstacles,
616+
skip_cleaning_points=skip_cleaning_points,
586617
)
587-
return packet, tail, carpet_end
618+
return packet, tail, trace_offset
588619

589620

590621
def _parse_header_calibration(payload: bytes) -> Q10HeaderCalibration | None:
@@ -699,6 +730,16 @@ def _parse_carpet_block(tail: bytes, width: int, height: int) -> tuple[bytes | N
699730
return mask, block_end
700731

701732

733+
def _parse_counted_points(tail: bytes, offset: int) -> tuple[list[tuple[int, int]], int | None]:
734+
"""Decode a complete u8-counted signed-BE coordinate table atomically."""
735+
if offset >= len(tail):
736+
return [], None
737+
end = offset + 1 + tail[offset] * 4
738+
if end > len(tail):
739+
return [], None
740+
return list(struct.iter_unpack(">hh", memoryview(tail)[offset + 1 : end])), end
741+
742+
702743
def _parse_clean_record_trace(
703744
tail: bytes,
704745
offset: int,
@@ -707,20 +748,17 @@ def _parse_clean_record_trace(
707748
708749
The header and declared point count were validated against a physical ss07
709750
clean-record response and its point bytes match captured prefixes of the
710-
corresponding live trace exactly. One observed zero byte precedes the path;
711-
its meaning is unknown, so a non-zero value makes the entire section opaque.
712-
Any unsupported version, non-zero reserved word, or truncated point table is
751+
corresponding live trace exactly. The obstacle and skip-clean tables precede
752+
the one-byte path version. Any unsupported version, non-zero reserved word,
753+
or truncated point table is
713754
likewise left completely opaque. Bytes after the declared points are
714755
deliberately not consumed: the observed 12-byte suffix appears structured,
715756
but there is not enough controlled evidence to name or decode it safely.
716757
"""
717-
if offset >= len(tail) or tail[offset] != 0:
718-
return None
719-
offset += _HISTORICAL_TRACE_PREFIX_LENGTH
720758
header_end = offset + _HISTORICAL_TRACE_HEADER_LENGTH
721759
if header_end > len(tail):
722760
return None
723-
version = int.from_bytes(tail[offset : offset + 2], "big")
761+
version = tail[offset]
724762
reserved = int.from_bytes(
725763
tail[offset + _HISTORICAL_TRACE_RESERVED_OFFSET : offset + _HISTORICAL_TRACE_RESERVED_OFFSET + 2],
726764
"big",
@@ -766,3 +804,5 @@ class B01Q10MapParserConfig:
766804

767805
map_scale: int = 4
768806
"""Scale factor for the rendered map image."""
807+
drawables: list[Drawable] | None = None
808+
"""Enabled overlays, or ``None`` for the Q10 defaults."""

‎roborock/map/b01_q10_render.py‎

Lines changed: 21 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@
2323
from vacuum_map_parser_base.config.drawable import Drawable
2424
from vacuum_map_parser_base.config.image_config import ImageConfig
2525
from vacuum_map_parser_base.config.size import Size, Sizes
26-
from vacuum_map_parser_base.map_data import Area, ImageData, MapData, Path, Point, Wall
26+
from vacuum_map_parser_base.map_data import Area, ImageData, MapData, Obstacle, ObstacleDetails, Path, Point, Wall
2727

2828
from roborock.exceptions import RoborockException
2929

@@ -73,6 +73,7 @@
7373
Drawable.CHARGER,
7474
Drawable.NO_GO_AREAS,
7575
Drawable.NO_MOPPING_AREAS,
76+
Drawable.OBSTACLES,
7677
Drawable.PATH,
7778
Drawable.VACUUM_POSITION,
7879
Drawable.VIRTUAL_WALLS,
@@ -118,7 +119,7 @@ def render_q10_map(
118119
if map_data.image is None:
119120
raise RoborockException("Failed to render Q10 map image")
120121

121-
has_drawables = False
122+
has_drawables = _place_obstacles(map_data, packet)
122123
if trace_calibration is not None and trace is not None:
123124
charger_heading = packet.header_calibration.charger_phi if packet.header_calibration is not None else None
124125
_place_trace(map_data, trace_calibration, trace, charger_heading=charger_heading)
@@ -137,6 +138,19 @@ def render_q10_map(
137138
return buffer.getvalue()
138139

139140

141+
def _place_obstacles(map_data: MapData, packet: Q10MapPacket) -> bool:
142+
"""Project map-package obstacles with their own header-anchored scale."""
143+
header = packet.header_calibration
144+
if header is None or (origin := header.origin_pixels()) is None or not packet.obstacles:
145+
return False
146+
calibration = GridCalibration(resolution=50.0, origin_x=origin[0], origin_y=origin[1], y_sign=1)
147+
map_data.obstacles = [
148+
Obstacle(*calibration.world_to_pixel(obstacle.x, obstacle.y), ObstacleDetails())
149+
for obstacle in packet.obstacles
150+
]
151+
return True
152+
153+
140154
def solve_q10_calibration(
141155
packet: Q10MapPacket,
142156
trace: Q10TracePacket | Q10HistoricalTracePacket | None,
@@ -332,10 +346,12 @@ def _draw_map_content(
332346
"""Draw Q10 content with the shared V1 image generator."""
333347
if map_data.image is None:
334348
raise RoborockException("Failed to render Q10 map image")
335-
generator = _create_image_generator(
336-
MapParserConfig(map_scale=config.map_scale),
337-
drawables=_Q10_DRAWABLES,
349+
drawables = (
350+
_Q10_DRAWABLES
351+
if config.drawables is None
352+
else [drawable for drawable in config.drawables if drawable in _Q10_DRAWABLE_TYPES]
338353
)
354+
generator = _create_image_generator(MapParserConfig(map_scale=config.map_scale), drawables=drawables)
339355
generator.draw_map(map_data)
340356
buffer = io.BytesIO()
341357
map_data.image.data.save(buffer, format="PNG")

‎tests/devices/traits/b01/q10/test_map.py‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,11 +9,13 @@
99
import asyncio
1010
import base64
1111
from collections.abc import AsyncGenerator, Generator
12+
from dataclasses import replace
1213
from pathlib import Path
1314
from typing import cast
1415
from unittest.mock import Mock, patch
1516

1617
import pytest
18+
from vacuum_map_parser_base.config.drawable import Drawable
1719

1820
from roborock.cli import _await_q10_map_push, cli
1921
from roborock.data.b01_q10.b01_q10_code_mappings import B01_Q10_DP, YXDeviceState
@@ -23,6 +25,9 @@
2325
from roborock.devices.traits.b01.q10.maps import MapsTrait
2426
from roborock.exceptions import RoborockException
2527
from roborock.map.b01_q10_map_parser import (
28+
B01Q10MapParserConfig,
29+
Q10HeaderCalibration,
30+
Q10Obstacle,
2631
Q10Point,
2732
Q10TracePacket,
2833
parse_map_packet,
@@ -586,3 +591,17 @@ def test_map_content_trait_as_dict_camelizes_child_keys() -> None:
586591
}
587592
assert data["path"] == [{"x": 100, "y": 200}, {"x": 150, "y": 250}]
588593
assert data["robotPosition"] == {"x": 25875, "y": 26125}
594+
595+
596+
async def test_create_forwards_obstacle_drawables_to_rendering(fake_channel: FakeB01Q10Channel) -> None:
597+
"""The public Q10 factory config changes the rendered image."""
598+
enabled = create(fake_channel, map_parser_config=B01Q10MapParserConfig(drawables=[Drawable.OBSTACLES]))
599+
disabled = create(fake_channel, map_parser_config=B01Q10MapParserConfig(drawables=[]))
600+
packet = replace(
601+
parse_map_packet(FIXTURE.read_bytes()),
602+
header_calibration=Q10HeaderCalibration(0, 50, 5, -1, -1, 0),
603+
obstacles=[Q10Obstacle(150, 100)],
604+
)
605+
enabled.map.update_from_map_packet(packet)
606+
disabled.map.update_from_map_packet(packet)
607+
assert enabled.map.image_content != disabled.map.image_content

‎tests/map/test_b01_q10_map_parser.py‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
"""Tests for the Roborock Q10 (B01/ss07) map parser."""
22

33
import io
4+
import struct
45
from pathlib import Path
56
from typing import Any
67

@@ -11,6 +12,7 @@
1112
from roborock.map.b01_grid_layers import LAYER_BACKGROUND, LAYER_FLOOR, LAYER_WALL
1213
from roborock.map.b01_q10_map_parser import (
1314
Q10MapPacketKind,
15+
Q10Obstacle,
1416
Q10Point,
1517
Q10Room,
1618
classify_q10_cell,
@@ -585,3 +587,43 @@ def test_clean_record_path_stray_point_handling(points: list[tuple[int, int]], e
585587
def test_clean_record_parser_rejects_other_packet_kinds(marker: bytes) -> None:
586588
with pytest.raises(RoborockException, match="not a Q10 clean-record"):
587589
parse_clean_record_detail(marker + FIXTURE.read_bytes()[2:])
590+
591+
592+
@pytest.mark.parametrize("marker", [b"\x01\x01", b"\x03\x01", b"\x04\x01"])
593+
def test_obstacle_tables_preserve_signed_coordinates(marker: bytes) -> None:
594+
"""Complete counted tables retain signed coordinates for every map kind."""
595+
payload = _map_detail_payload(marker, [(10, -20)])
596+
offset = len(FIXTURE.read_bytes()) + len(_carpet_tail(8, 6, bytes(48)))
597+
tables = b"\x02" + struct.pack(">hhhh", 250, -300, -32768, 32767) + b"\x01" + struct.pack(">hh", -40, 50)
598+
payload = payload[:offset] + tables + payload[offset + 2 :]
599+
packet = parse_map_packet(payload)
600+
assert packet.obstacles == [Q10Obstacle(250, -300), Q10Obstacle(-32768, 32767)]
601+
assert packet.skip_cleaning_points == [Q10Point(-40, 50)]
602+
603+
604+
def test_nonempty_obstacle_tables_preserve_historical_trace_boundary() -> None:
605+
"""Historical points start after the complete obstacle and skip tables."""
606+
payload = _map_detail_payload(b"\x03\x01", [(10, -20)])
607+
offset = len(FIXTURE.read_bytes()) + len(_carpet_tail(8, 6, bytes(48)))
608+
tables = b"\x01" + struct.pack(">hh", 250, -300) + b"\x01" + struct.pack(">hh", -40, 50)
609+
detail = parse_clean_record_detail(payload[:offset] + tables + payload[offset + 2 :])
610+
assert detail.map.obstacles == [Q10Obstacle(250, -300)]
611+
assert detail.map.skip_cleaning_points == [Q10Point(-40, 50)]
612+
assert detail.trace is not None
613+
assert detail.trace.points == [Q10Point(10, -20)]
614+
615+
616+
@pytest.mark.parametrize("table", [b"", b"\x02\x00\x01\x00\x02", b"\x00", b"\x00\x02\x00\x01\x00\x02"])
617+
def test_truncated_obstacle_tables_are_ignored_atomically(table: bytes) -> None:
618+
payload = FIXTURE.read_bytes() + _carpet_tail(8, 6, bytes(48)) + table
619+
packet = parse_map_packet(payload)
620+
assert packet.obstacles == []
621+
assert packet.skip_cleaning_points == []
622+
assert packet.carpet_mask == bytes(48)
623+
624+
625+
def test_maximum_counted_obstacle_table_is_bounded() -> None:
626+
table = b"\xff" + struct.pack(">hh", -10, 20) * 255 + b"\x00future"
627+
packet = parse_map_packet(FIXTURE.read_bytes() + _carpet_tail(8, 6, bytes(48)) + table)
628+
assert packet.obstacles == [Q10Obstacle(-10, 20)] * 255
629+
assert packet.skip_cleaning_points == []

0 commit comments

Comments
 (0)