Release v0.7.1: rosbag replay, counterfactual demo, Nav2 plugins, LeRobot validation.

Adds the full rosbag-to-imagination loop, bundled counterfactual demo, Nav2
avoidance preview, compiled costmap layer and DWB critic with loopback smoke,
and optional LeRobot loader validation after v2.1→v3.0 convert.

Co-authored-by: Cursor <cursoragent@cursor.com>

Author: rsasaki0109

.github/workflows/ci.yml

@@ -55,5 +55,5 @@ jobs:
         run: |
           source /opt/ros/jazzy/setup.bash
           source install/setup.bash
-          colcon test --packages-select world_model_py world_model_msgs world_model_bringup world_model_viz world_model_datasets world_model_nav2
+          colcon test --packages-select world_model_py world_model_msgs world_model_bringup world_model_viz world_model_datasets world_model_nav2 world_model_costmap world_model_dwb_critics
           colcon test-result --verbose

CHANGELOG.md

@@ -4,6 +4,49 @@ All notable changes to this project are documented in this file. The format is
 based on [Keep a Changelog](https://keepachangelog.com/en/1.1.0/), and this
 project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
 
+## [0.7.1] - 2026-06-08
+
+LeRobot loader validation and a compiled Nav2 DWB critic plugin.
+
+### Added
+- **LeRobot loader validation** — `test/validate_lerobot_loader.py` exports a toy
+  dataset, converts v2.1 → v3.0, and loads it with `LeRobotDataset` when
+  `lerobot` is installed. Parquet writer fix: 1-D state/action columns are stored
+  as scalars so v3 loading succeeds.
+- **`world_model_dwb_critics`** — compiled `dwb_core::TrajectoryCritic` plugin
+  (`WorldModelOccupancyCritic`) that scores DWB rollouts against predicted
+  `FutureOccupancy` using the same lethal-union rule as `world_model_costmap`.
+- **Nav2 loopback integration** — `nav2_loopback_world_model.launch.py` patches
+  Nav2 bringup params with the costmap layer + DWB critic; `smoke_nav2_loopback.py`
+  verifies both plugins initialise in a live stack (requires
+  `ros-jazzy-nav2-loopback-sim`).
+
+### Notes
+- Loopback smoke confirms plugin load + subscription, not full navigation to a goal.
+  LeRobot validation is optional (`pip install lerobot`).
+
+## [0.7.0] - 2026-06-08
+
+From rosbag replay to counterfactual imagination and Nav2-style avoidance — the
+full “use your own data” loop, plus one-command demos.
+
+### Added
+- **Rosbag replay** — `bag_relay` node + `replay_imagination.launch.py`: play a
+  bag through the World Model runtime and RViz imagination viewer.
+- **Experience recording** — `experience_recorder` writes `experience.npz` when a
+  replay finishes; `planning_node` hot-reloads the memory for `ImagineFutures`.
+- **Counterfactual demo** — bundled `drive_demo.mcap`,
+  `replay_counterfactual_demo.launch.py`, `counterfactual_marker_node` (mosaic +
+  MarkerArray), and Foxglove layout `foxglove/counterfactual.json`.
+- **Nav2 avoidance preview** — `costmap_preview_node` (predicted lethal union),
+  `avoidance_demo_node` (pick a detour path using the same rule as
+  `world_model_costmap::WorldModelLayer`), `nav2_avoidance_demo.launch.py`, and
+  `docs/nav2_avoidance.gif`.
+
+### Notes
+- Nav2 avoidance demo visualises the costmap-layer logic in Python + RViz; closed-
+  loop Nav2 in Gazebo is still future work (see `world_model_costmap/README.md`).
+
 ## [0.6.0] - 2026-06-08
 
 Generate the future instead of retrieving it: a learned `latent → pixel` decoder

README.md

@@ -30,7 +30,10 @@ diverge by action. Learning-free episodic planning — see
   over a shared JSON wire; ROS 2 clients are unchanged.
 - **Compiled Nav2 costmap layer** — predicted occupancy flows straight into the
   Nav2 costmap (real `nav2_costmap_2d::Layer`, not a mock).
-- **rosbag2 → LeRobot** dataset export, **GPU-free**.
+- **Compiled Nav2 DWB critic** — score DWB rollouts against the same predicted
+  occupancy (`world_model_dwb_critics`).
+- **rosbag2 → LeRobot** dataset export, **GPU-free** (optional `lerobot` loader
+  validation test included).
 - **Counterfactual imagination & planning (learning-free)** — imagine *"what if I
   steer left / straight / right"* and plan to an image goal from an episodic
   retrieval world model, **no dynamics training**; validated on real public data.
@@ -152,16 +155,80 @@ ros2 bag record -s mcap -o my_run \
 panel and enable the `/world_model_viz/imagination` topic — markers carry their
 own colors.)
 
+### 5. Replay your own rosbag2
+
+Feed a recorded drive through the World Model — no robot, no synthetic publisher.
+The `bag_relay` node joins camera + `cmd_vel` + `odom` into
+`world_model_msgs/Observation`; the launch file starts bag play, the runtime,
+the imagination viewer and RViz.
+
+```bash
+ros2 launch world_model_bringup replay_imagination.launch.py \
+    bag:=/path/to/my_drive \
+    adapter:=dummy          # or ijepa / vjepa2 on a GPU box
+
+# topic names differ in your bag? override them:
+#   image_topic:=/front_camera/image_raw action_topic:=/control/cmd state_topic:=/localization/odom
+# headless:
+#   rviz:=false
+```
+
+Record your own bag first:
+
+```bash
+ros2 bag record -s mcap -o my_drive /camera/image_raw /cmd_vel /odom
+```
+
+**Record an experience memory while replaying** (for learning-free counterfactual
+planning). The `experience_recorder` encodes each frame, pairs consecutive
+latents with steering (`cmd_vel.angular.z`), and writes `experience.npz` when
+the bag finishes. `planning_node` reloads the file automatically.
+
+```bash
+ros2 launch world_model_bringup replay_imagination.launch.py \
+    bag:=/path/to/my_drive \
+    record_experience:=true \
+    experience_out:=/tmp/experience.npz
+
+# after bag play ends (~3s idle), imagine L / straight / R:
+ros2 service call /world_model_planning/imagine_futures \
+    world_model_msgs/srv/ImagineFutures \
+    "{steering_options: [-0.7, 0.0, 0.7], horizon: 12}"
+```
+
+Use `adapter:=ijepa` on a GPU box for real latent encoding; `dummy` works
+GPU-free for pipeline smoke tests.
+
+### 6. Counterfactual replay demo (bundled bag, one command)
+
+A synthetic driving bag ships in the repo. This replays it, records
+`experience.npz`, auto-calls `ImagineFutures`, and shows the L / straight / R
+mosaic in RViz — no robot, no dataset download, GPU-free with `dummy`.
+
+```bash
+ros2 launch world_model_bringup replay_counterfactual_demo.launch.py
+# faster replay: rate:=3.0   headless: rviz:=false
+```
+
+Topics: `/counterfactual_viz/counterfactual/mosaic` (Image) and
+`…/markers` (MarkerArray). Rebuild the bundled bag with
+`python3 world_model_bringup/scripts/build_demo_bag.py`.
+
+Open the counterfactual clip in [Foxglove](https://foxglove.dev/) with layout
+[`world_model_viz/foxglove/counterfactual.json`](world_model_viz/foxglove/counterfactual.json)
+(record `/counterfactual_viz/counterfactual/mosaic` during a replay).
+
 ## Packages
 
 | package | type | what |
 |---|---|---|
 | `world_model_msgs` | ament_cmake | the message/action **contract**: `Observation`, `ActionCondition`, `FutureState`, `FutureOccupancy`, `LatentState`, `RiskScore`, `Rollout`, `PredictFuture.action` |
-| `world_model_py` | ament_python | adapter SDK (`load_model`), `dummy` / `ijepa` / `remote` adapters, lifecycle runtime node, reference `world-model-server`, benchmark, `world-model` CLI |
-| `world_model_viz` | ament_python | imagination viewer: imagined `FutureOccupancy` + `RiskScore` → RViz `MarkerArray` |
+| `world_model_py` | ament_python | adapter SDK (`load_model`), `dummy` / `ijepa` / `remote` adapters, lifecycle runtime node, `bag_relay` (rosbag2 → Observation), reference `world-model-server`, benchmark, `world-model` CLI |
+| `world_model_viz` | ament_python | imagination viewer (`FutureOccupancy` + `RiskScore` → RViz); counterfactual mosaic viewer (`ImagineFutures` → Image + MarkerArray) |
 | `world_model_datasets` | ament_python | `export_lerobot`: rosbag2 → LeRobot-compatible dataset (parquet + mp4 + meta), GPU-free |
 | `world_model_nav2` | ament_python | score Nav2 candidate trajectories by model-based risk (`ScoreTrajectories` service + risk-coloured path markers) |
 | `world_model_costmap` | ament_cmake (C++) | **compiled** `nav2_costmap_2d::Layer` that stamps predicted `FutureOccupancy` into the costmap |
+| `world_model_dwb_critics` | ament_cmake (C++) | **compiled** `dwb_core::TrajectoryCritic` that scores DWB rollouts against predicted occupancy |
 | `world_model_bringup` | ament_cmake | launch files + demo config |
 
 ## Architecture
@@ -295,9 +362,18 @@ Output (v2.1 layout): `meta/{info,episodes,tasks,stats}.json[l]`,
 Supported state/action types: `nav_msgs/Odometry`, `sensor_msgs/JointState`,
 `geometry_msgs/Twist[Stamped]`, `std_msgs/Float{32,64}MultiArray`.
 
-> Honesty: validated structurally (parquet round-trips, mp4 is ffprobe-readable,
-> counts consistent), **not** against the `lerobot` loader (not a dependency
-> here). Verify against your `lerobot` version before training.
+**Loader validation (optional).** With `pip install lerobot`, convert v2.1 → v3.0
+and open the dataset:
+
+```bash
+pip install lerobot
+cd world_model_datasets
+python3 -m pytest test/validate_lerobot_loader.py -q
+```
+
+The test exports a toy dataset, runs `lerobot.scripts.convert_dataset_v21_to_v30`,
+and loads it with `LeRobotDataset`. Structural checks (parquet round-trip, ffprobe-
+readable mp4, counts) run in CI without `lerobot`.
 
 **Validated on real public data.** `test/validate_real_lerobot.py` builds a
 rosbag2 from a public **LeRobot SO-101** episode (real camera video + real 6-DoF
@@ -522,6 +598,34 @@ unit-tested. This follows recent World-Model failure/OOD-monitoring work, e.g.
 [failure detection without failure data](https://arxiv.org/html/2503.08558v1),
 and [foundation world models detecting manipulation failures](https://arxiv.org/pdf/2603.06987).
 
+## Nav2 avoidance preview (predicted occupancy → detour)
+
+![World Model predicted occupancy steers around the lethal union](docs/nav2_avoidance.gif)
+
+<sub>**Predicted occupancy → avoid.** The dummy World Model imagines a moving
+obstacle blob; `costmap_preview_node` shows the lethal union that
+`world_model_costmap::WorldModelLayer` would stamp into Nav2, and
+`avoidance_demo_node` picks a detour path that misses it (naive straight in red,
+safe arc in green). GPU-free, no Gazebo.</sub>
+
+```bash
+ros2 launch world_model_bringup nav2_avoidance_demo.launch.py
+```
+
+Honest scope: this previews the **costmap layer** logic in RViz; for a **live Nav2
+stack** (loopback sim, not Gazebo) with both plugins loaded:
+
+```bash
+sudo apt install ros-jazzy-nav2-loopback-sim   # once
+ros2 launch world_model_bringup nav2_loopback_world_model.launch.py use_rviz:=false
+# optional headless smoke:
+python3 $(ros2 pkg prefix world_model_bringup)/share/world_model_bringup/scripts/smoke_nav2_loopback.py
+```
+
+For trajectory *ranking* by model risk, see the scorer mock below. For a **compiled
+DWB critic**, see
+[world_model_dwb_critics/README.md](world_model_dwb_critics/README.md).
+
 ## Nav2 trajectory scoring (mock)
 
 ![Nav2 trajectory scoring by World Model risk](docs/nav2_scoring.gif)
@@ -551,16 +655,20 @@ For the **production path**, `world_model_costmap` is a *compiled*
 `nav2_costmap_2d::Layer` that stamps the model's predicted `FutureOccupancy`
 straight into the Nav2 costmap — so any planner/controller avoids predicted
 obstacles. See [world_model_costmap/README.md](world_model_costmap/README.md).
+`world_model_dwb_critics` adds the same rule as a `dwb_core::TrajectoryCritic`
+for DWB local planners.
 
 ## Roadmap (90-day MVP)
 
 - **0–30d (this scaffold):** msgs, adapter SDK, dummy + remote adapters,
   lifecycle node, CLI, HTML smoke/bench report — **all GPU-free**. ✅
 - **31–60d:** JEPA latent adapter (image→latent→surprise) ✅, RViz imagination
-  markers ✅, rosbag2 replay demo (next).
-- **61–90d:** rosbag2 → LeRobotDataset converter ✅, Nav2 trajectory-scoring
-  mock ✅, compiled Nav2 costmap layer ✅, remote adapter + reference server
-  (Cosmos/DreamZero-ready) ✅, benchmark dashboard, VLA Zoo / Walking Zoo examples.
+  markers ✅, rosbag2 replay demo ✅, counterfactual replay demo ✅, Nav2
+  avoidance preview ✅.
+- **61–90d:** rosbag2 → LeRobotDataset converter ✅ (optional `lerobot` loader
+  validation ✅), Nav2 trajectory-scoring mock ✅, compiled Nav2 costmap layer ✅,
+  compiled DWB critic ✅, remote adapter + reference server (Cosmos/DreamZero-ready)
+  ✅, benchmark dashboard. VLA Zoo / Walking Zoo — out of scope for now.
 
 ## Contributing
 

docs/nav2_avoidance.gif

@@ -3,8 +3,15 @@ project(world_model_bringup)
 
 find_package(ament_cmake REQUIRED)
 
-install(DIRECTORY launch config
+install(DIRECTORY launch config demo
   DESTINATION share/${PROJECT_NAME}
 )
 
+install(PROGRAMS
+  scripts/nav2_world_model_params.py
+  scripts/smoke_nav2_loopback.py
+  scripts/build_demo_bag.py
+  DESTINATION share/${PROJECT_NAME}/scripts
+)
+
 ament_package()

world_model_bringup/CMakeLists.txt

@@ -0,0 +1,46 @@
+rosbag2_bagfile_information:
+  version: 9
+  storage_identifier: mcap
+  duration:
+    nanoseconds: 4702000000
+  starting_time:
+    nanoseconds_since_epoch: 1000000000
+  message_count: 144
+  topics_with_message_count:
+    - topic_metadata:
+        name: /cmd_vel
+        type: geometry_msgs/msg/Twist
+        serialization_format: cdr
+        offered_qos_profiles:
+          []
+        type_description_hash: ""
+      message_count: 48
+    - topic_metadata:
+        name: /odom
+        type: nav_msgs/msg/Odometry
+        serialization_format: cdr
+        offered_qos_profiles:
+          []
+        type_description_hash: ""
+      message_count: 48
+    - topic_metadata:
+        name: /camera/image_raw
+        type: sensor_msgs/msg/Image
+        serialization_format: cdr
+        offered_qos_profiles:
+          []
+        type_description_hash: ""
+      message_count: 48
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - drive_demo.mcap_0.mcap
+  files:
+    - path: drive_demo.mcap_0.mcap
+      starting_time:
+        nanoseconds_since_epoch: 1000000000
+      duration:
+        nanoseconds: 4702000000
+      message_count: 288
+  custom_data: ~
+  ros_distro: jazzy

world_model_bringup/demo/drive_demo.mcap/drive_demo.mcap_0.mcap

@@ -0,0 +1,71 @@
+"""World Model predicted occupancy -> avoid the lethal union (GPU-free).
+
+Shows what ``world_model_costmap::WorldModelLayer`` would stamp into Nav2, then
+picks a detour path that misses the predicted lethal cells. No Gazebo / full
+Nav2 stack required.
+
+    ros2 launch world_model_bringup nav2_avoidance_demo.launch.py
+    # rviz:=false on headless
+"""
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import LifecycleNode, Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    viz_share = get_package_share_directory("world_model_viz")
+    rviz_cfg = os.path.join(viz_share, "rviz", "nav2_avoidance.rviz")
+
+    runtime = LifecycleNode(
+        package="world_model_py",
+        executable="runtime_node",
+        name="world_model_runtime",
+        namespace="",
+        parameters=[{"adapter": LaunchConfiguration("adapter"), "horizon": 8, "autostart": True}],
+        output="screen",
+    )
+    sample = Node(
+        package="world_model_py",
+        executable="sample_publisher",
+        name="sample_observation_publisher",
+        remappings=[("observation", "/world_model_runtime/observation")],
+        parameters=[{"rate_hz": 4.0}],
+        output="screen",
+    )
+    preview = Node(
+        package="world_model_viz",
+        executable="costmap_preview_node",
+        name="costmap_preview",
+        output="screen",
+    )
+    avoidance = Node(
+        package="world_model_nav2",
+        executable="avoidance_demo_node",
+        name="avoidance_demo",
+        output="screen",
+    )
+    rviz = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        arguments=["-d", rviz_cfg],
+        condition=IfCondition(LaunchConfiguration("rviz")),
+        output="screen",
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("adapter", default_value="dummy"),
+            DeclareLaunchArgument("rviz", default_value="true"),
+            runtime,
+            sample,
+            preview,
+            avoidance,
+            rviz,
+        ]
+    )