|
31 | 31 | GridCalibration, |
32 | 32 | GridLayers, |
33 | 33 | solve_calibration, |
34 | | - solve_calibration_with_origin, |
35 | 34 | ) |
36 | 35 | from .b01_q10_map_parser import ( |
37 | 36 | _BACKGROUND_VALUE, |
|
41 | 40 | Q10EraseZone, |
42 | 41 | Q10HistoricalTracePacket, |
43 | 42 | Q10MapPacket, |
| 43 | + Q10Point, |
| 44 | + Q10Room, |
44 | 45 | Q10TracePacket, |
45 | 46 | erased_packet, |
46 | 47 | ) |
|
62 | 63 | # erase/restriction vectors use 5 mm units: one header unit therefore equals |
63 | 64 | # two vector units. Trace points use a separate 2.5 mm coordinate scale. |
64 | 65 | _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 |
65 | 68 | # A path needs enough shape to constrain a full (origin + resolution) fit; a few |
66 | 69 | # points cannot. |
67 | 70 | _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 | 71 | _Q10_DRAWABLE_TYPES = { |
73 | 72 | Drawable.CHARGER, |
74 | 73 | Drawable.NO_GO_AREAS, |
@@ -143,30 +142,120 @@ def solve_q10_calibration( |
143 | 142 | ) -> GridCalibration | None: |
144 | 143 | """Derive world-to-pixel calibration from a map and its current trace. |
145 | 144 |
|
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. |
151 | 149 | """ |
152 | | - if trace is None: |
| 150 | + if trace is None or not trace.points: |
153 | 151 | return None |
| 152 | + if calibration := _trace_calibration_from_header_metadata(packet, trace.points): |
| 153 | + return calibration |
154 | 154 | 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) |
156 | 156 |
|
157 | 157 |
|
158 | | -def _calibration_from_header( |
| 158 | +def _trace_calibration_from_header_metadata( |
159 | 159 | packet: Q10MapPacket, |
160 | | - points: list[tuple[float, float]], |
| 160 | + points: Sequence[Q10Point], |
161 | 161 | ) -> 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: |
165 | 254 | 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: |
168 | 257 | return None |
169 | | - return solve_calibration_with_origin(packet.layers, points, origin, resolutions=_Q10_RESOLUTIONS) |
| 258 | + return _room_at_position(packet, position, calibration) |
170 | 259 |
|
171 | 260 |
|
172 | 261 | def _calibration_from_header_metadata( |
|
0 commit comments