patch..

resources/nav2-dev-image/Dockerfile

@@ -0,0 +1,41 @@
+FROM future-nav2:humble
+
+SHELL ["/bin/bash", "-o", "pipefail", "-c"]
+
+ENV DEBIAN_FRONTEND=noninteractive \
+    ROS_DISTRO=humble \
+    TURTLEBOT3_MODEL=burger \
+    NAV2_WORKSPACE=/workspace/nav2-slam_ws
+
+# Nav2 and SLAM Toolbox are provided by the base image. Module workspaces may
+# override those packages with local sources, while Gazebo and TurtleBot3 are
+# added here for the source-based simulator modules.
+RUN apt-get update \
+    && apt-get install -y --no-install-recommends \
+        build-essential \
+        cmake \
+        git \
+        python3-argcomplete \
+        python3-colcon-common-extensions \
+        python3-rosdep \
+        python3-vcstool \
+        python3-pip \
+        ros-humble-gazebo-plugins \
+        ros-humble-gazebo-ros-pkgs \
+        ros-humble-turtlebot3-description \
+        ros-humble-turtlebot3-gazebo \
+        ros-humble-turtlebot3-msgs \
+        ros-humble-xacro \
+        ros-humble-tf-transformations \
+    && rm -rf /var/lib/apt/lists/* \
+    && rosdep init 2>/dev/null || true
+
+RUN rosdep update
+
+COPY docker-entrypoint.sh /usr/local/bin/studio-nav2-entrypoint
+RUN chmod +x /usr/local/bin/studio-nav2-entrypoint \
+    && mkdir -p "${NAV2_WORKSPACE}/src"
+
+WORKDIR /workspace
+ENTRYPOINT ["/usr/local/bin/studio-nav2-entrypoint"]
+CMD ["bash"]

resources/nav2-dev-image/README.md

@@ -0,0 +1,34 @@
+# Nav2 Development Image
+
+`nav2-dev:humble` is the source-build runtime for Nav2 learning modules. It
+contains ROS 2 Humble, Gazebo, colcon, and rosdep, but intentionally does not
+contain Nav2, TurtleBot3, or SLAM Toolbox source code.
+
+Build it from this directory:
+
+```bash
+./build.sh
+```
+
+Mount a composed module workspace and build its sources:
+
+```bash
+docker run --rm -it --network host \
+  -e DISPLAY="$DISPLAY" \
+  -e TURTLEBOT3_MODEL=burger \
+  -v /tmp/.X11-unix:/tmp/.X11-unix \
+  -v "$HOME/autonomy-studio/workspaces/nav2-slam_ws:/workspace/nav2-slam_ws" \
+  nav2-dev:humble bash
+
+cd "$NAV2_WORKSPACE"
+rosdep install --from-paths src --ignore-src -r -y
+colcon build
+```
+
+The composed workspace must contain all direct source dependencies. For Nav2,
+include its shared packages such as `nav2_common`, `nav2_util`, and `nav2_msgs`
+alongside the module packages being built.
+
+On later container starts, the entrypoint automatically sources
+`/workspace/nav2-slam_ws/install/setup.bash`, so source-built packages override
+the base ROS installation.

resources/nav2-dev-image/build.sh

@@ -0,0 +1,4 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+docker build -t nav2-dev:humble "$(cd "$(dirname "$0")" && pwd)"

resources/nav2-dev-image/docker-entrypoint.sh

@@ -0,0 +1,12 @@
+#!/usr/bin/env bash
+set -e
+
+source "/opt/ros/${ROS_DISTRO}/setup.bash"
+
+# A prior colcon build takes precedence over apt packages when the workspace
+# is mounted. This makes source edits immediately available to ROS commands.
+if [[ -f "${NAV2_WORKSPACE}/install/setup.bash" ]]; then
+  source "${NAV2_WORKSPACE}/install/setup.bash"
+fi
+
+exec "$@"

resources/nav2-ros-bridge/bridge.py

@@ -209,6 +209,9 @@ class SubscriberNode(Node):
         self._trajectory_event = threading.Event()
         self._behavior_path = None
         self._behavior_path_event = threading.Event()
+        self._nav2_plan = None
+        self._nav2_plan_revision = 0
+        self._nav2_plan_lock = threading.Lock()
         if _ROBOT_STACK == "autoware" and HAS_PLANNING:
             self.create_subscription(
                 Trajectory,
@@ -350,11 +353,20 @@ class SubscriberNode(Node):
 
     def _on_nav2_plan(self, msg: NavPath):
         if len(msg.poses) > 0:
+            with self._nav2_plan_lock:
+                self._nav2_plan = msg
+                self._nav2_plan_revision += 1
             self._behavior_path = msg
             self._trajectory = msg
             self._behavior_path_event.set()
             self._trajectory_event.set()
 
+    def get_nav2_plan_snapshot(self):
+        with self._nav2_plan_lock:
+            if self._nav2_plan is None:
+                return self._nav2_plan_revision, []
+            return self._nav2_plan_revision, self._nav_path_to_points(self._nav2_plan)
+
     def _on_kinematic_state(self, msg: Odometry):
         with self._kinematic_lock:
             self._kinematic_state = msg
@@ -1104,6 +1116,36 @@ async def ws_map(websocket: WebSocket, stack: Optional[str] = None):
         pass
 
 
+@app.websocket("/ws/nav2")
+async def ws_nav2(websocket: WebSocket):
+    await websocket.accept()
+    if _ROBOT_STACK != "nav2":
+        await websocket.send_json({
+            "type": "stack_mismatch",
+            "requested": "nav2",
+            "active": _ROBOT_STACK,
+            "message": "Restart bridge with ROBOT_STACK=nav2 to stream Nav2 paths",
+        })
+        await websocket.close()
+        return
+
+    last_revision = -1
+    try:
+        while True:
+            if sub_node:
+                revision, poses = sub_node.get_nav2_plan_snapshot()
+                if poses and revision != last_revision:
+                    last_revision = revision
+                    await websocket.send_json({
+                        "type": "nav2_path",
+                        "revision": revision,
+                        "poses": poses,
+                    })
+            await asyncio.sleep(0.25)
+    except (WebSocketDisconnect, Exception):
+        pass
+
+
 @app.post("/set_initial_pose")
 def set_initial_pose(payload: PosePayload):
     try:

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": {