Skip to content

Commit b504f24

Browse files
author
Harry Coureau
committed
feat: expose Q10 current cleaning room
1 parent 8e471dc commit b504f24

5 files changed

Lines changed: 254 additions & 37 deletions

File tree

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

Lines changed: 18 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@
3030
Q10TracePacket,
3131
)
3232
from roborock.map.b01_q10_overlays import parse_virtual_wall_blob, parse_zone_blob
33-
from roborock.map.b01_q10_render import Q10MapOverlays, render_q10_map
33+
from roborock.map.b01_q10_render import Q10MapOverlays, render_q10_map, resolve_q10_current_room
3434

3535
from .command import CommandTrait
3636
from .common import UpdatableTrait
@@ -148,6 +148,17 @@ def robot_heading(self) -> int | None:
148148
"""Current heading for orienting a robot marker on a caller-rendered map."""
149149
return self._trace_packet.heading if self._trace_packet else None
150150

151+
@property
152+
def current_room(self) -> Q10Room | None:
153+
"""Room inferred from the latest live robot position and segmented map.
154+
155+
Returns ``None`` when position or calibration is unavailable, the pose
156+
lies outside the map, or nearby room labels are ambiguous.
157+
"""
158+
if self._map_packet is None or self._trace_packet is None:
159+
return None
160+
return resolve_q10_current_room(self._map_packet, self._trace_packet)
161+
151162
def update_from_map_packet(self, packet: Q10MapPacket) -> None:
152163
"""Store a map-protocol update and render the latest sources."""
153164
self._map_packet = packet
@@ -156,12 +167,15 @@ def update_from_map_packet(self, packet: Q10MapPacket) -> None:
156167

157168
def update_from_trace_packet(self, packet: Q10TracePacket) -> None:
158169
"""Store a trace-protocol update and render the latest sources."""
159-
self._trace_packet = packet
170+
self._trace_packet = None if self._map_dps.robot_at_dock else packet
160171
self._render()
161172
self._notify_update()
162173

163174
def _map_dps_updated(self) -> None:
164175
"""Render after the low-level map DPS source changes."""
176+
if self._map_dps.robot_at_dock:
177+
# A completed path no longer identifies the current pose at dock.
178+
self._trace_packet = None
165179
if self._map_packet is None:
166180
return
167181
self._render()
@@ -186,13 +200,15 @@ def _render(self) -> None:
186200
def as_dict(self, exclude: set[str] | None = None) -> dict[str, Any]:
187201
"""Return the trait data as a dictionary, excluding large binary data."""
188202
exclude_set = exclude or set()
203+
current_room = self.current_room
189204
data = {
190205
"rooms": [room.as_dict() for room in self.rooms],
191206
"path": [point.as_dict() for point in self.path],
192207
"robotPosition": (
193208
{"x": position.x, "y": position.y} if (position := self.robot_position) is not None else None
194209
),
195210
"robotHeading": self.robot_heading,
211+
"currentRoom": current_room.as_dict() if current_room is not None else None,
196212
}
197213
for key in exclude_set:
198214
data.pop(key, None)

‎roborock/map/b01_q10_render.py‎

Lines changed: 109 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,6 @@
3131
GridCalibration,
3232
GridLayers,
3333
solve_calibration,
34-
solve_calibration_with_origin,
3534
)
3635
from .b01_q10_map_parser import (
3736
_BACKGROUND_VALUE,
@@ -41,6 +40,8 @@
4140
Q10EraseZone,
4241
Q10HistoricalTracePacket,
4342
Q10MapPacket,
43+
Q10Point,
44+
Q10Room,
4445
Q10TracePacket,
4546
erased_packet,
4647
)
@@ -62,13 +63,11 @@
6263
# erase/restriction vectors use 5 mm units: one header unit therefore equals
6364
# two vector units. Trace points use a separate 2.5 mm coordinate scale.
6465
_Q10_VECTOR_UNITS_PER_HEADER_RESOLUTION_UNIT = 2.0
66+
# One centimetre header unit spans four 2.5 mm trace units.
67+
_Q10_TRACE_UNITS_PER_HEADER_RESOLUTION_UNIT = 4.0
6568
# A path needs enough shape to constrain a full (origin + resolution) fit; a few
6669
# points cannot.
6770
_MIN_CALIBRATION_POINTS = 20
68-
# When the grid-frame header supplies the origin, only the resolution is fit, so
69-
# a much shorter path suffices to confirm it (early in a clean, not just a dense
70-
# one). See :func:`solve_calibration_with_origin`.
71-
_MIN_HEADER_CALIBRATION_POINTS = 4
7271
_Q10_DRAWABLE_TYPES = {
7372
Drawable.CHARGER,
7473
Drawable.NO_GO_AREAS,
@@ -143,30 +142,120 @@ def solve_q10_calibration(
143142
) -> GridCalibration | None:
144143
"""Derive world-to-pixel calibration from a map and its current trace.
145144
146-
When the map packet's grid-frame header carries a calibration origin (ss07),
147-
only the resolution is fit -- around that fixed origin -- so a short path
148-
suffices and the origin is exact rather than recovered by a slide. Otherwise
149-
the full origin + resolution fit is used, which needs a reasonably dense
150-
cleaning path. Returns ``None`` if the path is too short/featureless to fit.
145+
When the map packet carries usable ss07 header metadata, its fixed origin
146+
and resolution are authoritative and work from the first live pose. Older
147+
or incomplete packets fall back to a full origin/resolution fit, which needs
148+
a reasonably dense cleaning path. Returns ``None`` when neither is usable.
151149
"""
152-
if trace is None:
150+
if trace is None or not trace.points:
153151
return None
152+
if calibration := _trace_calibration_from_header_metadata(packet, trace.points):
153+
return calibration
154154
points: list[tuple[float, float]] = [(point.x, point.y) for point in trace.points]
155-
return _calibration_from_header(packet, points) or _calibration_from_fit(packet.layers, points)
155+
return _calibration_from_fit(packet.layers, points)
156156

157157

158-
def _calibration_from_header(
158+
def _trace_calibration_from_header_metadata(
159159
packet: Q10MapPacket,
160-
points: list[tuple[float, float]],
160+
points: Sequence[Q10Point],
161161
) -> GridCalibration | None:
162-
"""Calibrate around the header-supplied origin (resolution fit to a path)."""
163-
header_calibration = packet.header_calibration
164-
if header_calibration is None or len(points) < _MIN_HEADER_CALIBRATION_POINTS:
162+
"""Build the live-trace transform directly from validated header metadata."""
163+
header = packet.header_calibration
164+
if header is None or header.resolution <= 0 or (origin := header.origin_pixels()) is None:
165+
return None
166+
resolution = header.resolution * _Q10_TRACE_UNITS_PER_HEADER_RESOLUTION_UNIT
167+
if not min(_Q10_RESOLUTIONS) <= resolution <= max(_Q10_RESOLUTIONS):
168+
return None
169+
calibration = GridCalibration(
170+
resolution=resolution,
171+
origin_x=origin[0],
172+
origin_y=origin[1],
173+
y_sign=1,
174+
)
175+
return calibration if _trace_projects_onto_floor(packet.layers, points, calibration) else None
176+
177+
178+
def _trace_projects_onto_floor(
179+
layers: GridLayers,
180+
points: Sequence[Q10Point],
181+
calibration: GridCalibration,
182+
) -> bool:
183+
"""Validate header calibration against a bounded sample of trace points."""
184+
if not points:
185+
return False
186+
stride = max(1, len(points) // 256)
187+
sample = points[::stride]
188+
on_floor = 0
189+
for point in sample:
190+
pixel_x, pixel_y = calibration.world_to_pixel(point.x, point.y)
191+
column, row = math.floor(pixel_x), math.floor(pixel_y)
192+
if not (0 <= column < layers.width and 0 <= row < layers.height):
193+
continue
194+
if layers.cell_class(layers.grid[row * layers.width + column]) == "floor":
195+
on_floor += 1
196+
return on_floor >= len(sample) * 0.5
197+
198+
199+
def _room_at_position(
200+
packet: Q10MapPacket,
201+
position: Q10Point,
202+
calibration: GridCalibration,
203+
*,
204+
search_radius: int = 2,
205+
) -> Q10Room | None:
206+
"""Resolve a live position to a segmented room in the occupancy grid.
207+
208+
The exact cell wins. A robot centre can briefly land on a wall or doorway
209+
pixel, so a small expanding neighbourhood is used only when the exact cell
210+
is not segmented. Ambiguous ties stay unknown rather than naming the wrong
211+
room.
212+
"""
213+
pixel_x, pixel_y = calibration.world_to_pixel(position.x, position.y)
214+
column, row = math.floor(pixel_x), math.floor(pixel_y)
215+
if not (0 <= column < packet.width and 0 <= row < packet.height):
216+
return None
217+
218+
rooms_by_value = {room.pixel_value: room for room in packet.rooms}
219+
220+
def room_at(column: int, row: int) -> Q10Room | None:
221+
if not (0 <= column < packet.width and 0 <= row < packet.height):
222+
return None
223+
return rooms_by_value.get(packet.grid[row * packet.width + column])
224+
225+
if room := room_at(column, row):
226+
return room
227+
228+
for radius in range(1, max(0, search_radius) + 1):
229+
room_ids = {
230+
candidate.id
231+
for nearby_row in range(row - radius, row + radius + 1)
232+
for nearby_column in range(column - radius, column + radius + 1)
233+
if (candidate := room_at(nearby_column, nearby_row)) is not None
234+
}
235+
if not room_ids:
236+
continue
237+
if len(room_ids) != 1:
238+
return None
239+
room_id = room_ids.pop()
240+
return next((room for room in packet.rooms if room.id == room_id), None)
241+
return None
242+
243+
244+
def resolve_q10_current_room(packet: Q10MapPacket, trace: Q10TracePacket | None) -> Q10Room | None:
245+
"""Infer the robot's current room from its latest live pose and map grid.
246+
247+
No authoritative active-segment field has been identified or observed on
248+
ss07 firmware. The live trace's final point is the robot pose, and the full
249+
map labels every segmented floor cell. Header metadata supplies the fixed
250+
transform from trace coordinates to that grid; older/incomplete packets
251+
fall back to the path-fit calibration used by rendering.
252+
"""
253+
if trace is None or (position := trace.robot_position) is None:
165254
return None
166-
origin = header_calibration.origin_pixels()
167-
if origin is None: # keepalive frame -- no usable origin
255+
calibration = solve_q10_calibration(packet, trace)
256+
if calibration is None:
168257
return None
169-
return solve_calibration_with_origin(packet.layers, points, origin, resolutions=_Q10_RESOLUTIONS)
258+
return _room_at_position(packet, position, calibration)
170259

171260

172261
def _calibration_from_header_metadata(

‎tests/devices/traits/b01/q10/__snapshots__/test_status.ambr‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
'dustSwitch': True,
2525
}),
2626
'map': dict({
27+
'currentRoom': None,
2728
'path': list([
2829
]),
2930
'robotHeading': None,
@@ -404,6 +405,7 @@
404405
'dustSwitch': True,
405406
}),
406407
'map': dict({
408+
'currentRoom': None,
407409
'path': list([
408410
]),
409411
'robotHeading': None,
@@ -480,6 +482,7 @@
480482
'dust_collection': dict({
481483
}),
482484
'map': dict({
485+
'currentRoom': None,
483486
'path': list([
484487
]),
485488
'robotHeading': None,

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

Lines changed: 66 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
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
@@ -23,6 +24,7 @@
2324
from roborock.devices.traits.b01.q10.maps import MapsTrait
2425
from roborock.exceptions import RoborockException
2526
from roborock.map.b01_q10_map_parser import (
27+
Q10HeaderCalibration,
2628
Q10Point,
2729
Q10TracePacket,
2830
parse_map_packet,
@@ -490,8 +492,8 @@ async def test_charging_status_renders_robot_at_dock(render_map: Mock) -> None:
490492
assert render_map.call_args.kwargs["robot_at_dock"] is True
491493

492494

493-
def test_docked_state_hides_trace_only_from_rendering(render_map: Mock) -> None:
494-
"""A docked render omits the valid trace without deleting source data."""
495+
def test_docked_state_clears_live_trace(render_map: Mock) -> None:
496+
"""Docking clears the completed path and current robot pose."""
495497
map_dps = MapDpsTrait()
496498
trait = _map_trait(map_dps)
497499
packet = parse_map_packet(FIXTURE.read_bytes())
@@ -504,13 +506,15 @@ def test_docked_state_hides_trace_only_from_rendering(render_map: Mock) -> None:
504506

505507
map_dps.update_from_dps({B01_Q10_DP.STATUS: YXDeviceState.CHARGING.code})
506508

507-
assert trait.path == trace.points
509+
assert trait.path == []
510+
assert trait.robot_position is None
511+
assert trait.current_room is None
508512
assert render_map.call_args.args[1] is None
509513
assert render_map.call_args.kwargs["robot_at_dock"] is True
510514

511515

512-
def test_late_trace_is_retained_but_hidden_while_docked(render_map: Mock) -> None:
513-
"""A late trace stays available but is not part of a docked render."""
516+
def test_late_trace_is_ignored_while_docked(render_map: Mock) -> None:
517+
"""A late completed trace cannot repopulate the docked robot pose."""
514518
map_dps = MapDpsTrait()
515519
map_dps.update_from_dps({B01_Q10_DP.STATUS: YXDeviceState.CHARGING.code})
516520
trait = _map_trait(map_dps)
@@ -520,7 +524,9 @@ def test_late_trace_is_retained_but_hidden_while_docked(render_map: Mock) -> Non
520524
trait.update_from_map_packet(parse_map_packet(FIXTURE.read_bytes()))
521525
trait.update_from_trace_packet(trace)
522526

523-
assert trait.path == trace.points
527+
assert trait.path == []
528+
assert trait.robot_position is None
529+
assert trait.current_room is None
524530
assert render_map.call_args.args[1] is None
525531

526532

@@ -586,3 +592,57 @@ def test_map_content_trait_as_dict_camelizes_child_keys() -> None:
586592
}
587593
assert data["path"] == [{"x": 100, "y": 200}, {"x": 150, "y": 250}]
588594
assert data["robotPosition"] == {"x": 25875, "y": 26125}
595+
596+
597+
@pytest.mark.parametrize("trace_first", [False, True])
598+
def test_current_room_tracks_latest_fixture_pose_and_map(trace_first: bool) -> None:
599+
"""Current room and robot position consume the same latest map/trace inputs."""
600+
map_dps = MapDpsTrait()
601+
trait = _map_trait(map_dps)
602+
packet = replace(
603+
parse_map_packet(FIXTURE.read_bytes()), header_calibration=Q10HeaderCalibration(0, 50, 5, -1, -1, 0)
604+
)
605+
trace = Q10TracePacket(points=[Q10Point(100, 60)])
606+
assert trait.current_room is None
607+
if trace_first:
608+
trait.update_from_trace_packet(trace)
609+
assert trait.current_room is None
610+
trait.update_from_map_packet(packet)
611+
else:
612+
trait.update_from_map_packet(packet)
613+
assert trait.current_room is None
614+
trait.update_from_trace_packet(trace)
615+
assert trait.current_room == packet.rooms[1]
616+
assert trait.robot_position == trace.points[-1].to_roborock()
617+
assert trait.as_dict()["currentRoom"] == packet.rooms[1].as_dict()
618+
assert "currentRoom" not in trait.as_dict(exclude={"currentRoom"})
619+
map_dps.update_from_dps({B01_Q10_DP.STATUS: YXDeviceState.IDLE.code})
620+
assert trait.current_room == packet.rooms[1]
621+
trait.update_from_trace_packet(Q10TracePacket(points=[Q10Point(40, 60)]))
622+
assert trait.current_room == packet.rooms[0]
623+
trait.update_from_map_packet(replace(packet, rooms=[]))
624+
assert trait.current_room is None
625+
626+
627+
@pytest.mark.parametrize("status", [YXDeviceState.CHARGING, YXDeviceState.EMPTYING_THE_BIN])
628+
def test_docking_clears_room_and_ignores_late_fixture_trace(status: YXDeviceState) -> None:
629+
map_dps = MapDpsTrait()
630+
trait = _map_trait(map_dps)
631+
packet = replace(
632+
parse_map_packet(FIXTURE.read_bytes()), header_calibration=Q10HeaderCalibration(0, 50, 5, -1, -1, 0)
633+
)
634+
trace = Q10TracePacket(points=[Q10Point(100, 60)])
635+
trait.update_from_map_packet(packet)
636+
trait.update_from_trace_packet(trace)
637+
assert trait.current_room == packet.rooms[1]
638+
map_dps.update_from_dps({B01_Q10_DP.STATUS: status.code})
639+
assert trait.current_room is None
640+
assert trait.robot_position is None
641+
assert trait.path == []
642+
trait.update_from_trace_packet(trace)
643+
assert trait.current_room is None
644+
assert trait.path == []
645+
map_dps.update_from_dps({B01_Q10_DP.STATUS: YXDeviceState.CLEANING.code})
646+
assert trait.current_room is None
647+
trait.update_from_trace_packet(trace)
648+
assert trait.current_room == packet.rooms[1]

0 commit comments

Comments
 (0)