patch..

resources/slam-ros-bridge/bridge.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 import asyncio
 import array
+import base64
 import json
 import logging
 import math
@@ -83,6 +84,44 @@ def compose_2d(a: dict, b: dict) -> dict:
     }
 
 
+def invert_2d(transform: dict) -> dict:
+    """Return the inverse of a planar TF transform."""
+    yaw = -transform["yaw"]
+    c = math.cos(yaw)
+    s = math.sin(yaw)
+    return {
+        "x": c * -transform["x"] - s * -transform["y"],
+        "y": s * -transform["x"] + c * -transform["y"],
+        "z": -transform.get("z", 0.0),
+        "yaw": yaw,
+    }
+
+
+def resolve_2d_transform(transforms: dict, target_frame: str, source_frame: str) -> Optional[dict]:
+    """Resolve target <- source through the currently published TF graph."""
+    if not target_frame or not source_frame:
+        return None
+    if target_frame == source_frame:
+        return {"x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0}
+
+    pending = [(target_frame, {"x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0})]
+    visited = set()
+    while pending:
+        frame, accumulated = pending.pop(0)
+        if frame in visited:
+            continue
+        visited.add(frame)
+        if frame == source_frame:
+            return accumulated
+
+        for (parent, child), transform in transforms.items():
+            if parent == frame and child not in visited:
+                pending.append((child, compose_2d(accumulated, transform)))
+            elif child == frame and parent not in visited:
+                pending.append((parent, compose_2d(accumulated, invert_2d(transform))))
+    return None
+
+
 class VelocityPayload(BaseModel):
     linear: float = 0.0
     angular: float = 0.0
@@ -238,6 +277,28 @@ class SlamNode(Node):
                 value = data[row * width + column]
                 pixels.append(0 if value >= 50 else 254 if value == 0 else 205)
         pgm_path.write_bytes(f"P5\n{width} {height}\n255\n".encode("ascii") + pixels)
+
+        # Keep a compact copy with the map record so the LMS can render an
+        # actual saved-map preview without accessing the student's workspace.
+        preview_limit = 192
+        preview_scale = max(1, math.ceil(width / preview_limit), math.ceil(height / preview_limit))
+        preview_width = math.ceil(width / preview_scale)
+        preview_height = math.ceil(height / preview_scale)
+        preview_pixels = bytearray()
+        for preview_row in range(preview_height):
+            source_top = preview_row * preview_scale
+            source_bottom = min(height, source_top + preview_scale)
+            for preview_column in range(preview_width):
+                source_left = preview_column * preview_scale
+                source_right = min(width, source_left + preview_scale)
+                # Preserve occupied cells while reducing the grid: black is
+                # lower than unknown/free in the generated PGM palette.
+                preview_pixels.append(min(
+                    pixels[row * width + column]
+                    for row in range(source_top, source_bottom)
+                    for column in range(source_left, source_right)
+                ))
+        preview_pgm = f"P5\n{preview_width} {preview_height}\n255\n".encode("ascii") + preview_pixels
         yaml_path.write_text(
             "\n".join([
                 f"image: {pgm_path.name}",
@@ -285,6 +346,7 @@ class SlamNode(Node):
             "resolution": resolution,
             "width": width,
             "height": height,
+            "previewPgm": base64.b64encode(preview_pgm).decode("ascii"),
         }
 
     def _on_scan(self, msg: LaserScan):
@@ -553,25 +615,24 @@ class SlamNode(Node):
                 "angular": vel.angular.z,
                 "t": odom.header.stamp.sec + odom.header.stamp.nanosec * 1e-9,
             }
-            base_to_scan = (
-                tf_snapshot.get((odom.child_frame_id, scan.header.frame_id))
+            odom_to_scan = (
+                resolve_2d_transform(tf_snapshot, odom.header.frame_id, scan.header.frame_id)
                 if scan is not None
                 else None
             )
-            if base_to_scan is not None:
+            if odom_to_scan is not None:
                 scan_pose = {
-                    **compose_2d(pose, base_to_scan),
-                    "z": pose.get("z", 0.0) + base_to_scan.get("z", 0.0),
+                    **odom_to_scan,
                     "roll": 0.0,
                     "pitch": 0.0,
                     "frame_id": pose["frame_id"],
                     "source_frame_id": scan.header.frame_id,
                     "t": scan.header.stamp.sec + scan.header.stamp.nanosec * 1e-9,
                 }
-            map_to_odom = tf_snapshot.get(("map", odom.header.frame_id))
-            if map_to_odom is not None:
+            map_to_base = resolve_2d_transform(tf_snapshot, "map", odom.child_frame_id)
+            if map_to_base is not None:
                 pose_map = {
-                    **compose_2d(map_to_odom, pose),
+                    **map_to_base,
                     "frame_id": "map",
                     "child_frame_id": odom.child_frame_id,
                     "source_frame_id": odom.header.frame_id,
@@ -579,14 +640,12 @@ class SlamNode(Node):
                 }
 
         if scan is not None and pose_map is not None:
-            # A scan is measured from its sensor frame, not base_link. Use the
-            # published base_link -> scan-frame TF so its endpoints overlay the
-            # occupancy grid in the same map coordinate system.
-            base_to_scan = tf_snapshot.get((pose_map["child_frame_id"], scan.header.frame_id))
-            if base_to_scan is not None:
+            # Resolve map -> sensor directly through the published TF graph so
+            # the scan uses the same localization correction as the map.
+            map_to_scan = resolve_2d_transform(tf_snapshot, "map", scan.header.frame_id)
+            if map_to_scan is not None:
                 scan_pose_map = {
-                    **compose_2d(pose_map, base_to_scan),
-                    "z": pose_map.get("z", 0.0) + base_to_scan.get("z", 0.0),
+                    **map_to_scan,
                     "roll": 0.0,
                     "pitch": 0.0,
                     "frame_id": "map",
@@ -676,7 +735,7 @@ class SlamNode(Node):
             "point_cloud": point_cloud_payload,
             "map": map_payload,
             "tf": {
-                "map_to_odom": tf_snapshot.get(("map", "odom")),
+                "map_to_odom": resolve_2d_transform(tf_snapshot, "map", "odom"),
             },
             "navigation": navigation,
             "plan": {