|
28 | 28 | from roborock.exceptions import RoborockException |
29 | 29 |
|
30 | 30 | from .b01_grid_layers import ( |
| 31 | + LAYER_FLOOR, |
31 | 32 | GridCalibration, |
32 | 33 | GridLayers, |
33 | 34 | solve_calibration, |
34 | | - solve_calibration_with_origin, |
35 | 35 | ) |
36 | 36 | from .b01_q10_map_parser import ( |
37 | 37 | _BACKGROUND_VALUE, |
|
41 | 41 | Q10EraseZone, |
42 | 42 | Q10HistoricalTracePacket, |
43 | 43 | Q10MapPacket, |
| 44 | + Q10Point, |
| 45 | + Q10Room, |
44 | 46 | Q10TracePacket, |
| 47 | + classify_q10_cell, |
45 | 48 | erased_packet, |
46 | 49 | ) |
47 | 50 | from .b01_q10_overlays import ZONE_TYPE_NO_GO, ZONE_TYPE_NO_MOP, Q10Zone |
|
62 | 65 | # erase/restriction vectors use 5 mm units: one header unit therefore equals |
63 | 66 | # two vector units. Trace points use a separate 2.5 mm coordinate scale. |
64 | 67 | _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 |
65 | 70 | # A path needs enough shape to constrain a full (origin + resolution) fit; a few |
66 | 71 | # points cannot. |
67 | 72 | _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 |
72 | 73 | _Q10_DRAWABLE_TYPES = { |
73 | 74 | Drawable.CHARGER, |
74 | 75 | Drawable.NO_GO_AREAS, |
@@ -157,30 +158,120 @@ def solve_q10_calibration( |
157 | 158 | ) -> GridCalibration | None: |
158 | 159 | """Derive world-to-pixel calibration from a map and its current trace. |
159 | 160 |
|
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. |
165 | 165 | """ |
166 | | - if trace is None: |
| 166 | + if trace is None or not trace.points: |
167 | 167 | return None |
| 168 | + if calibration := _trace_calibration_from_header_metadata(packet, trace.points): |
| 169 | + return calibration |
168 | 170 | 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) |
170 | 172 |
|
171 | 173 |
|
172 | | -def _calibration_from_header( |
| 174 | +def _trace_calibration_from_header_metadata( |
173 | 175 | packet: Q10MapPacket, |
174 | | - points: list[tuple[float, float]], |
| 176 | + points: Sequence[Q10Point], |
175 | 177 | ) -> 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: |
179 | 270 | 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: |
182 | 273 | return None |
183 | | - return solve_calibration_with_origin(packet.layers, points, origin, resolutions=_Q10_RESOLUTIONS) |
| 274 | + return _room_at_position(packet, position, calibration) |
184 | 275 |
|
185 | 276 |
|
186 | 277 | def _calibration_from_header_metadata( |
|
0 commit comments