Skip to content

Commit 3ec517e

Browse files
hCoureauHarry Coureau
andauthored
feat: expose Q10 current cleaning room (#939)
* feat: expose Q10 current cleaning room * test: separate Q10 current room arrival scenarios * fix: preserve Q10 room labels and sample header calibration --------- Co-authored-by: Harry Coureau <hcoureau@amazon.com>
1 parent f4fb1ec commit 3ec517e

12 files changed

Lines changed: 296 additions & 122 deletions

File tree

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

Lines changed: 18 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@
3232
Q10TracePacket,
3333
)
3434
from roborock.map.b01_q10_overlays import Q10RestrictedZone, Q10VirtualWall, parse_virtual_wall_blob, parse_zone_blob
35-
from roborock.map.b01_q10_render import Q10MapOverlays, render_q10_map
35+
from roborock.map.b01_q10_render import Q10MapOverlays, render_q10_map, resolve_q10_current_room
3636
from roborock.protocols.b01_q10_protocol import encode_restricted_zones, encode_room_name, encode_virtual_walls
3737

3838
from .command import CommandTrait
@@ -209,6 +209,17 @@ def robot_heading(self) -> int | None:
209209
"""Current heading for orienting a robot marker on a caller-rendered map."""
210210
return self._trace_packet.heading if self._trace_packet else None
211211

212+
@property
213+
def current_room(self) -> Q10Room | None:
214+
"""Room inferred from the latest live robot position and segmented map.
215+
216+
Returns ``None`` when position or calibration is unavailable, the pose
217+
lies outside the map, or nearby room labels are ambiguous.
218+
"""
219+
if self._map_packet is None or self._trace_packet is None:
220+
return None
221+
return resolve_q10_current_room(self._map_packet, self._trace_packet)
222+
212223
def update_from_map_packet(self, packet: Q10MapPacket) -> None:
213224
"""Store a map-protocol update and render the latest sources."""
214225
if packet.kind is not Q10MapPacketKind.CURRENT:
@@ -219,12 +230,15 @@ def update_from_map_packet(self, packet: Q10MapPacket) -> None:
219230

220231
def update_from_trace_packet(self, packet: Q10TracePacket) -> None:
221232
"""Store a trace-protocol update and render the latest sources."""
222-
self._trace_packet = packet
233+
self._trace_packet = None if self._map_dps.robot_at_dock else packet
223234
self._render()
224235
self._notify_update()
225236

226237
def _map_dps_updated(self) -> None:
227238
"""Render after the low-level map DPS source changes."""
239+
if self._map_dps.robot_at_dock:
240+
# A completed path no longer identifies the current pose at dock.
241+
self._trace_packet = None
228242
if self._map_packet is None:
229243
return
230244
self._render()
@@ -249,13 +263,15 @@ def _render(self) -> None:
249263
def as_dict(self, exclude: set[str] | None = None) -> dict[str, Any]:
250264
"""Return the trait data as a dictionary, excluding large binary data."""
251265
exclude_set = exclude or set()
266+
current_room = self.current_room
252267
data = {
253268
"rooms": [room.as_dict() for room in self.rooms],
254269
"path": [point.as_dict() for point in self.path],
255270
"robotPosition": (
256271
{"x": position.x, "y": position.y} if (position := self.robot_position) is not None else None
257272
),
258273
"robotHeading": self.robot_heading,
274+
"currentRoom": current_room.as_dict() if current_room is not None else None,
259275
}
260276
for key in exclude_set:
261277
data.pop(key, None)

‎roborock/diagnostics.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,7 @@ def reset(self) -> None:
106106
"ipAddress",
107107
"ipAdress",
108108
"wifiName",
109+
"rawName",
109110
"lat",
110111
"long",
111112
}

‎roborock/map/b01_grid_layers.py‎

Lines changed: 0 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -220,58 +220,6 @@ def solve_calibration(
220220
return best[1]
221221

222222

223-
def solve_calibration_with_origin(
224-
layers: GridLayers,
225-
points: list[tuple[float, float]],
226-
origin: tuple[float, float],
227-
*,
228-
resolutions: Iterable[float],
229-
y_signs: Iterable[int] = (1, -1),
230-
min_on_floor: float = 0.5,
231-
) -> GridCalibration | None:
232-
"""Fit resolution + Y orientation around a *known* grid-pixel origin.
233-
234-
Unlike :func:`solve_calibration`, the pixel origin ``(ox, oy)`` is fixed --
235-
e.g. read straight from the Q10 grid-frame header -- so this only sweeps the
236-
candidate ``resolutions`` and ``y_signs`` and keeps the placement landing the
237-
most ``points`` on floor. With the expensive 2-D offset slide gone, far fewer
238-
points are needed to confirm the fit, so it works from a short path rather
239-
than a dense clean. Returns ``None`` if no candidate lands a ``min_on_floor``
240-
fraction of points on floor (e.g. the origin or points are bogus).
241-
"""
242-
if not points:
243-
return None
244-
w, h = layers.width, layers.height
245-
ox, oy = origin
246-
cells = _calibration_cells(layers)
247-
248-
best: tuple[float, GridCalibration] | None = None
249-
for resolution in resolutions:
250-
if resolution <= 0:
251-
continue
252-
for y_sign in y_signs:
253-
on_floor = 0
254-
blocked = 0
255-
for x, y in points:
256-
px = int(x / resolution + ox)
257-
py = int(oy - y_sign * y / resolution)
258-
if not (0 <= px < w and 0 <= py < h):
259-
blocked += 1
260-
continue
261-
cell_type = cells[py * w + px]
262-
if cell_type == _CalibrationCell.FLOOR:
263-
on_floor += 1
264-
elif cell_type == _CalibrationCell.BLOCKED:
265-
blocked += 1
266-
score = on_floor - 1.5 * blocked
267-
if best is None or score > best[0]:
268-
best = (score, GridCalibration(float(resolution), float(ox), float(oy), y_sign))
269-
270-
if best is None or best[0] < len(points) * min_on_floor:
271-
return None
272-
return best[1]
273-
274-
275223
def decompose_grid(
276224
width: int,
277225
height: int,

‎roborock/map/b01_q10_map_parser.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -119,6 +119,8 @@ class Q10Room(RoborockBase):
119119
@property
120120
def name(self) -> str:
121121
"""User friendly room name (firmware ``rr_`` defaults are normalized)."""
122+
if not self.raw_name.startswith("rr_"):
123+
return self.raw_name
122124
return self.raw_name.removeprefix("rr_").replace("_", " ").strip().title()
123125

124126

‎roborock/map/b01_q10_render.py‎

Lines changed: 111 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -28,10 +28,10 @@
2828
from roborock.exceptions import RoborockException
2929

3030
from .b01_grid_layers import (
31+
LAYER_FLOOR,
3132
GridCalibration,
3233
GridLayers,
3334
solve_calibration,
34-
solve_calibration_with_origin,
3535
)
3636
from .b01_q10_map_parser import (
3737
_BACKGROUND_VALUE,
@@ -41,7 +41,10 @@
4141
Q10EraseZone,
4242
Q10HistoricalTracePacket,
4343
Q10MapPacket,
44+
Q10Point,
45+
Q10Room,
4446
Q10TracePacket,
47+
classify_q10_cell,
4548
erased_packet,
4649
)
4750
from .b01_q10_overlays import ZONE_TYPE_NO_GO, ZONE_TYPE_NO_MOP, Q10Zone
@@ -62,13 +65,11 @@
6265
# erase/restriction vectors use 5 mm units: one header unit therefore equals
6366
# two vector units. Trace points use a separate 2.5 mm coordinate scale.
6467
_Q10_VECTOR_UNITS_PER_HEADER_RESOLUTION_UNIT = 2.0
68+
# One centimetre header unit spans four 2.5 mm trace units.
69+
_Q10_TRACE_UNITS_PER_HEADER_RESOLUTION_UNIT = 4.0
6570
# A path needs enough shape to constrain a full (origin + resolution) fit; a few
6671
# points cannot.
6772
_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
7273
_Q10_DRAWABLE_TYPES = {
7374
Drawable.CHARGER,
7475
Drawable.NO_GO_AREAS,
@@ -157,30 +158,120 @@ def solve_q10_calibration(
157158
) -> GridCalibration | None:
158159
"""Derive world-to-pixel calibration from a map and its current trace.
159160
160-
When the map packet's grid-frame header carries a calibration origin (ss07),
161-
only the resolution is fit -- around that fixed origin -- so a short path
162-
suffices and the origin is exact rather than recovered by a slide. Otherwise
163-
the full origin + resolution fit is used, which needs a reasonably dense
164-
cleaning path. Returns ``None`` if the path is too short/featureless to fit.
161+
When the map packet carries usable ss07 header metadata, its fixed origin
162+
and resolution are authoritative and work from the first live pose. Older
163+
or incomplete packets fall back to a full origin/resolution fit, which needs
164+
a reasonably dense cleaning path. Returns ``None`` when neither is usable.
165165
"""
166-
if trace is None:
166+
if trace is None or not trace.points:
167167
return None
168+
if calibration := _trace_calibration_from_header_metadata(packet, trace.points):
169+
return calibration
168170
points: list[tuple[float, float]] = [(point.x, point.y) for point in trace.points]
169-
return _calibration_from_header(packet, points) or _calibration_from_fit(packet.layers, points)
171+
return _calibration_from_fit(packet.layers, points)
170172

171173

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

185276

186277
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
@@ -30,6 +30,7 @@
3030
'dustSwitch': True,
3131
}),
3232
'map': dict({
33+
'currentRoom': None,
3334
'path': list([
3435
]),
3536
'robotHeading': None,
@@ -416,6 +417,7 @@
416417
'dustSwitch': True,
417418
}),
418419
'map': dict({
420+
'currentRoom': None,
419421
'path': list([
420422
]),
421423
'robotHeading': None,
@@ -498,6 +500,7 @@
498500
'dust_collection': dict({
499501
}),
500502
'map': dict({
503+
'currentRoom': None,
501504
'path': list([
502505
]),
503506
'robotHeading': None,

0 commit comments

Comments
 (0)