diff --git a/roborock/map/b01_q10_map_parser.py b/roborock/map/b01_q10_map_parser.py index 062f85f3..ad72749d 100644 --- a/roborock/map/b01_q10_map_parser.py +++ b/roborock/map/b01_q10_map_parser.py @@ -4,7 +4,8 @@ few seconds after a ``dpRequestDps`` request). Unlike the Q7 ``SCMap`` protobuf format, the Q10 uses a custom, unencrypted binary packet: -- ``01 01`` marker, then a ``u32be`` map id (bytes 2-5) and two consecutive +- ``01 01`` current-map or ``03 01`` saved-map marker, then a ``u32be`` map id + (bytes 2-5) and two consecutive ``u16be`` dimensions: grid width (bytes 7-8) and grid height (bytes 9-10). - A header field at offset 27 (``u16be``) giving the compressed layout length. - An LZ4-block-compressed occupancy grid starting at offset 29. Once inflated it @@ -66,6 +67,7 @@ def classify_q10_cell(value: int) -> str: MAP_PACKET_MARKER = b"\x01\x01" TRACE_PACKET_MARKER = b"\x02\x01" +SAVED_MAP_PACKET_MARKER = b"\x03\x01" _MAP_ID_OFFSET = 2 # Width and height are two consecutive u16be fields. An earlier revision read the @@ -179,7 +181,7 @@ def origin_pixels(self) -> tuple[float, float] | None: @dataclass class Q10MapPacket: - """Decoded contents of a Q10 ``01 01`` map packet.""" + """Decoded contents of a Q10 ``01 01`` or ``03 01`` map packet.""" map_id: int width: int @@ -194,6 +196,8 @@ class Q10MapPacket: """Carpet mask decoded from the packet tail: a full ``width*height`` grid in the same (top-down) pixel space as :attr:`grid`, where a non-zero cell is carpet (the value is the carpet kind). ``None`` if the packet carried none.""" + obstacles: list["Q10Point"] = field(default_factory=list) + """Saved-map obstacle markers in their raw header-anchored coordinates.""" @property def layers(self) -> GridLayers: @@ -283,10 +287,15 @@ def robot_position(self) -> Q10Point | None: def is_map_packet(payload: bytes) -> bool: - """Return True if the payload is a Q10 full-map (``01 01``) packet.""" + """Return True if the payload is a Q10 current-map (``01 01``) packet.""" return payload[:2] == MAP_PACKET_MARKER +def is_saved_map_packet(payload: bytes) -> bool: + """Return True if the payload is a Q10 saved-map (``03 01``) packet.""" + return payload[:2] == SAVED_MAP_PACKET_MARKER + + def is_trace_packet(payload: bytes) -> bool: """Return True if the payload is a Q10 live trace (``02 01``) packet.""" return payload[:2] == TRACE_PACKET_MARKER @@ -441,8 +450,9 @@ def _parse_rooms(room_data: bytes, grid: bytes) -> list[Q10Room]: def parse_map_packet(payload: bytes) -> Q10MapPacket: - """Parse a Q10 ``01 01`` map packet into grid + room metadata.""" - if len(payload) < _LAYOUT_COMPRESSED_OFFSET or not is_map_packet(payload): + """Parse a Q10 current-map or saved-map packet into typed source data.""" + saved_map = is_saved_map_packet(payload) + if len(payload) < _LAYOUT_COMPRESSED_OFFSET or not (is_map_packet(payload) or saved_map): raise RoborockException("Payload is not a Q10 map packet") map_id = int.from_bytes(payload[_MAP_ID_OFFSET : _MAP_ID_OFFSET + 4], "big") @@ -469,7 +479,8 @@ def parse_map_packet(payload: bytes) -> Q10MapPacket: rooms = _parse_rooms(room_data, grid) tail = payload[layout_end:] erase_zones = _parse_erase_zones(tail) - carpet_mask = _parse_carpet_mask(tail, width, height) + carpet_mask, carpet_end = _parse_carpet_block(tail, width, height) + obstacles = _parse_obstacles(tail, carpet_end) if saved_map and carpet_end is not None else [] header_calibration = _parse_header_calibration(payload) return Q10MapPacket( map_id=map_id, @@ -480,6 +491,7 @@ def parse_map_packet(payload: bytes) -> Q10MapPacket: erase_zones=erase_zones, header_calibration=header_calibration, carpet_mask=carpet_mask, + obstacles=obstacles, ) @@ -552,8 +564,8 @@ def _carpet_offset(tail: bytes) -> int: return 2 + count * vertices_per * 4 -def _parse_carpet_mask(tail: bytes, width: int, height: int) -> bytes | None: - """Decode the carpet mask that follows the erase section in the packet tail. +def _parse_carpet_block(tail: bytes, width: int, height: int) -> tuple[bytes | None, int | None]: + """Decode the carpet mask and return its validated end offset. Framing matches the main grid block: ``[u32 uncompressed_len]`` ``[u16 compressed_len][LZ4 block]``. The decompressed mask is a full @@ -561,23 +573,47 @@ def _parse_carpet_mask(tail: bytes, width: int, height: int) -> bytes | None: non-zero cell is carpet (the value is the carpet kind). Confirmed byte-exact on live ss07 captures (R1 / RDC), where ``uncompressed_len == width*height``. - Returns the decompressed mask, or ``None`` if the section is absent or does - not line up (the ``uncompressed_len == width*height`` invariant is used as the - guard so a mis-located section yields no carpet rather than garbage). + The end offset anchors the saved-map obstacle section that follows the carpet + block. Both values are ``None`` if the block is absent or invalid, preventing + trailing bytes from being misread as obstacle data. """ offset = _carpet_offset(tail) if offset + 6 > len(tail): - return None + return None, None uncompressed_len = int.from_bytes(tail[offset : offset + 4], "big") compressed_len = int.from_bytes(tail[offset + 4 : offset + 6], "big") block_end = offset + 6 + compressed_len if uncompressed_len != width * height or compressed_len <= 0 or block_end > len(tail): - return None + return None, None try: mask = lz4_block_decompress(tail[offset + 6 : block_end]) except RoborockException: - return None - return mask if len(mask) == width * height else None + return None, None + if len(mask) != width * height: + return None, None + return mask, block_end + + +def _parse_obstacles(tail: bytes, offset: int) -> list[Q10Point]: + """Decode saved-map obstacle markers following a validated carpet block. + + The section is ``[count: u8]`` followed by ``count`` signed int16-BE + ``(x, y)`` pairs. Coordinates use a fixed header-anchored scale distinct + from trace/path coordinates; placement remains the renderer's concern. + """ + if offset >= len(tail): + return [] + count = tail[offset] + end = offset + 1 + count * 4 + if count == 0 or end > len(tail): + return [] + return [ + Q10Point( + x=int.from_bytes(tail[offset + 1 + index * 4 : offset + 3 + index * 4], "big", signed=True), + y=int.from_bytes(tail[offset + 3 + index * 4 : offset + 5 + index * 4], "big", signed=True), + ) + for index in range(count) + ] def erased_packet(packet: "Q10MapPacket", cells: set[int]) -> "Q10MapPacket": diff --git a/roborock/map/b01_q10_render.py b/roborock/map/b01_q10_render.py index 8c241c98..74c6415d 100644 --- a/roborock/map/b01_q10_render.py +++ b/roborock/map/b01_q10_render.py @@ -33,7 +33,9 @@ B01Q10MapParser, B01Q10MapParserConfig, Q10EraseZone, + Q10HeaderCalibration, Q10MapPacket, + Q10Point, Q10TracePacket, erased_packet, ) @@ -56,6 +58,9 @@ # a much shorter path suffices to confirm it (early in a clean, not just a dense # one). See :func:`solve_calibration_with_origin`. _MIN_HEADER_CALIBRATION_POINTS = 4 +# Saved-map obstacle coordinates use this fixed number of raw units per grid +# pixel around the map header origin, independently of trace calibration. +_OBSTACLE_UNITS_PER_PIXEL = 50 @dataclass(frozen=True) @@ -100,8 +105,10 @@ def render_q10_map( if calibration is not None and trace is not None: _place_trace(map_data, calibration, trace) _place_overlays(map_data, calibration, overlays) - return _draw_map_content(parsed.image_content, map_data, config=config) + obstacles = _project_obstacles(packet.obstacles, packet.header_calibration) + if (calibration is not None and trace is not None) or obstacles: + return _draw_map_content(parsed.image_content, map_data, obstacles=obstacles, config=config) return parsed.image_content @@ -216,13 +223,34 @@ def to_area(zone: Q10Zone) -> Area | None: map_data.walls = walls or None +def _project_obstacles( + obstacles: Sequence[Q10Point], + header_calibration: Q10HeaderCalibration | None, +) -> list[Point]: + """Project saved-map obstacles using their fixed header-anchored scale.""" + if not obstacles or header_calibration is None: + return [] + origin = header_calibration.origin_pixels() + if origin is None: + return [] + calibration = GridCalibration( + resolution=_OBSTACLE_UNITS_PER_PIXEL, + origin_x=origin[0], + origin_y=origin[1], + y_sign=1, + ) + return [Point(*calibration.world_to_pixel(obstacle.x, obstacle.y)) for obstacle in obstacles] + + def _draw_map_content( image_content: bytes, map_data: MapData, *, + obstacles: Sequence[Point] = (), config: B01Q10MapParserConfig, line_color: tuple[int, int, int, int] = (235, 64, 52, 255), position_color: tuple[int, int, int, int] = (255, 211, 0, 255), + obstacle_color: tuple[int, int, int, int] = (0, 0, 0, 128), ) -> bytes: """Draw projected map content onto a base PNG and return a fresh PNG.""" scale = config.map_scale @@ -258,6 +286,14 @@ def to_image(point: Point) -> tuple[float, float]: width=max(2, scale), ) + obstacle_radius = 3 * scale + for obstacle in obstacles: + ox, oy = to_image(obstacle) + draw.ellipse( + [ox - obstacle_radius, oy - obstacle_radius, ox + obstacle_radius, oy + obstacle_radius], + fill=obstacle_color, + ) + for path in map_data.path.path if map_data.path else []: if len(path) >= 2: draw.line([to_image(point) for point in path], fill=line_color, width=max(1, scale // 2)) diff --git a/roborock/protocols/b01_q10_protocol.py b/roborock/protocols/b01_q10_protocol.py index c0cf6b9b..18601aab 100644 --- a/roborock/protocols/b01_q10_protocol.py +++ b/roborock/protocols/b01_q10_protocol.py @@ -11,6 +11,7 @@ Q10MapPacket, Q10TracePacket, is_map_packet, + is_saved_map_packet, is_trace_packet, parse_map_packet, parse_trace_packet, @@ -113,16 +114,17 @@ class Q10DpsUpdate: def decode_message(message: RoborockMessage) -> Q10Message | None: """Decode a pushed Q10 ``RoborockMessage`` into a typed message. - ``MAP_RESPONSE`` (protocol 301) payloads carry the binary map (``01 01``) or - trace (``02 01``) packets, which are parsed by the map parser; any other - ``MAP_RESPONSE`` marker is unrecognized and yields ``None``. Every other - protocol is treated as a DPS status update. + ``MAP_RESPONSE`` (protocol 301) payloads carry the binary current-map + (``01 01``) / saved-map (``03 01``) packets or the trace (``02 01``) packet, + which are parsed by the map parser; any other ``MAP_RESPONSE`` marker is + unrecognized and yields ``None``. Every other protocol is treated as a DPS + status update. Raises ``RoborockException`` if a recognized payload fails to parse. """ if message.protocol == RoborockMessageProtocol.MAP_RESPONSE: payload = message.payload or b"" - if is_map_packet(payload): + if is_map_packet(payload) or is_saved_map_packet(payload): return parse_map_packet(payload) if is_trace_packet(payload): return parse_trace_packet(payload) diff --git a/tests/map/test_b01_q10_map_parser.py b/tests/map/test_b01_q10_map_parser.py index 9e815e29..697229a0 100644 --- a/tests/map/test_b01_q10_map_parser.py +++ b/tests/map/test_b01_q10_map_parser.py @@ -13,6 +13,7 @@ Q10Room, classify_q10_cell, is_map_packet, + is_saved_map_packet, is_trace_packet, lz4_block_decompress, parse_map_packet, @@ -24,6 +25,8 @@ TRACE_MULTI_FIXTURE = Path(__file__).resolve().parent / "testdata" / "b01_q10_trace_multi.bin" # Real 14-point packet captured from an R1 corridor run (full session path). TRACE_SESSION_FIXTURE = Path(__file__).resolve().parent / "testdata" / "b01_q10_trace_session.bin" +SAVED_MAP_2OBSTACLES = Path(__file__).resolve().parent / "testdata" / "b01_q10_saved_map_2obstacles.bin" +SAVED_MAP_53OBSTACLES = Path(__file__).resolve().parent / "testdata" / "b01_q10_saved_map_53obstacles.bin" def _payload() -> bytes: @@ -392,6 +395,61 @@ def test_carpet_mask_ignored_when_uncompressed_len_mismatches() -> None: assert parse_map_packet(FIXTURE.read_bytes() + tail).carpet_mask is None +# --- Saved-map obstacle markers --------------------------------------------- + + +def _obstacle_section(obstacles: list[tuple[int, int]]) -> bytes: + """Build ``[count: u8]`` followed by signed int16-BE coordinate pairs.""" + return bytes([len(obstacles)]) + b"".join( + int.to_bytes(value & 0xFFFF, 2, "big") for point in obstacles for value in point + ) + + +def _as_saved_map(payload: bytes) -> bytes: + saved = bytearray(payload) + saved[:2] = b"\x03\x01" + return bytes(saved) + + +def test_saved_map_marker_and_layout_are_recognized() -> None: + payload = SAVED_MAP_2OBSTACLES.read_bytes() + assert is_saved_map_packet(payload) + assert not is_map_packet(payload) + packet = parse_map_packet(payload) + assert (packet.width, packet.height) == (219, 254) + assert packet.map_id == 0 + + +def test_parse_saved_map_obstacles_matches_capture() -> None: + packet = parse_map_packet(SAVED_MAP_2OBSTACLES.read_bytes()) + assert [(obstacle.x, obstacle.y) for obstacle in packet.obstacles] == [ + (-4633, -1946), + (-5231, -3852), + ] + + +def test_parse_saved_map_dense_obstacles_count() -> None: + assert len(parse_map_packet(SAVED_MAP_53OBSTACLES.read_bytes()).obstacles) == 53 + + +def test_parse_obstacles_after_valid_carpet_block() -> None: + width, height = 8, 6 + tail = _carpet_tail(width, height, bytes(width * height)) + _obstacle_section([(10, -20), (-30, 40)]) + packet = parse_map_packet(_as_saved_map(FIXTURE.read_bytes() + tail)) + assert [(obstacle.x, obstacle.y) for obstacle in packet.obstacles] == [(10, -20), (-30, 40)] + + +def test_current_map_does_not_decode_saved_map_obstacles() -> None: + width, height = 8, 6 + tail = _carpet_tail(width, height, bytes(width * height)) + _obstacle_section([(10, -20)]) + assert parse_map_packet(FIXTURE.read_bytes() + tail).obstacles == [] + + +def test_obstacles_require_a_valid_carpet_block() -> None: + tail = bytes([0, 0]) + _obstacle_section([(10, -20)]) + assert parse_map_packet(_as_saved_map(FIXTURE.read_bytes() + tail)).obstacles == [] + + def _calibrated_map_payload( width: int, height: int, diff --git a/tests/map/test_b01_q10_render.py b/tests/map/test_b01_q10_render.py index ff18ed65..56aa6533 100644 --- a/tests/map/test_b01_q10_render.py +++ b/tests/map/test_b01_q10_render.py @@ -31,11 +31,13 @@ _Q10_RESOLUTIONS, Q10MapOverlays, _erased_cells, + _project_obstacles, render_q10_map, solve_q10_calibration, ) FIXTURE = Path("tests/map/testdata/b01_q10_map.bin") +SAVED_MAP_2OBSTACLES = Path("tests/map/testdata/b01_q10_saved_map_2obstacles.bin") CONFIG = B01Q10MapParserConfig() # identity-ish calibration used across the geometry tests: world (x, y) -> grid @@ -157,6 +159,38 @@ def test_render_partial_erase() -> None: assert render != base +def test_render_draws_saved_map_obstacles_without_trace() -> None: + """Saved-map obstacles use their header origin without trace calibration.""" + packet = parse_map_packet(SAVED_MAP_2OBSTACLES.read_bytes()) + obstacles = _project_obstacles(packet.obstacles, packet.header_calibration) + assert [(round(point.x, 1), round(point.y, 1)) for point in obstacles] == [ + (72.4, 82.3), + (60.5, 120.4), + ] + + base = Image.open(io.BytesIO(_render(replace(packet, obstacles=[])))).convert("RGBA") + rendered = Image.open(io.BytesIO(_render(packet))).convert("RGBA") + first = obstacles[0] + pixel = (round(first.x * CONFIG.map_scale), round(first.y * CONFIG.map_scale)) + assert rendered.getpixel(pixel) != base.getpixel(pixel) + + +def test_render_obstacles_require_usable_header_origin() -> None: + packet = parse_map_packet(SAVED_MAP_2OBSTACLES.read_bytes()) + packet = replace( + packet, + header_calibration=Q10HeaderCalibration( + origin_x=0, + origin_y=0, + resolution=5, + charger_x=0, + charger_y=0, + charger_phi=0, + ), + ) + assert _render(packet) == _render(replace(packet, obstacles=[])) + + def test_render_draws_heading_indicator() -> None: """A known heading draws a facing tick from the robot marker. diff --git a/tests/map/testdata/b01_q10_saved_map_2obstacles.bin b/tests/map/testdata/b01_q10_saved_map_2obstacles.bin new file mode 100644 index 00000000..3422b7ec Binary files /dev/null and b/tests/map/testdata/b01_q10_saved_map_2obstacles.bin differ diff --git a/tests/map/testdata/b01_q10_saved_map_53obstacles.bin b/tests/map/testdata/b01_q10_saved_map_53obstacles.bin new file mode 100644 index 00000000..cd283d12 Binary files /dev/null and b/tests/map/testdata/b01_q10_saved_map_53obstacles.bin differ diff --git a/tests/protocols/test_b01_q10_protocol.py b/tests/protocols/test_b01_q10_protocol.py index f5d916d1..18265a6e 100644 --- a/tests/protocols/test_b01_q10_protocol.py +++ b/tests/protocols/test_b01_q10_protocol.py @@ -27,6 +27,7 @@ TESTDATA_IDS = [x.stem for x in TESTDATA_FILES] MAP_FIXTURE = pathlib.Path("tests/map/testdata/b01_q10_map.bin") +SAVED_MAP_FIXTURE = pathlib.Path("tests/map/testdata/b01_q10_saved_map_2obstacles.bin") TRACE_FIXTURE = pathlib.Path("tests/map/testdata/b01_q10_trace.bin") @@ -49,6 +50,14 @@ def test_decode_message_map_packet() -> None: assert {room.id: room.name for room in decoded.rooms} == {2: "Living Room", 3: "Bedroom"} +def test_decode_message_saved_map_packet() -> None: + """A MAP_RESPONSE 03 01 saved-map payload decodes into a Q10MapPacket w/ obstacles.""" + message = _message(SAVED_MAP_FIXTURE.read_bytes(), RoborockMessageProtocol.MAP_RESPONSE) + decoded = decode_message(message) + assert isinstance(decoded, Q10MapPacket) + assert len(decoded.obstacles) == 2 + + def test_decode_message_trace_packet() -> None: """A MAP_RESPONSE 02 01 payload decodes into a Q10TracePacket.""" message = _message(TRACE_FIXTURE.read_bytes(), RoborockMessageProtocol.MAP_RESPONSE)