-h` for more detailed usage. ...
+ros2: error: unrecognized arguments: --version
+=== talker/listener ===
+--- talker (first 5 lines) ---
+[INFO] [1794411103.122025551] [talker]: Publishing: 'Hello World: 1'
+[INFO] [1794411104.122005537] [talker]: Publishing: 'Hello World: 2'
+[INFO] [1794411105.121237532] [talker]: Publishing: 'Hello World: 3'
+[INFO] [1794411106.120483699] [talker]: Publishing: 'Hello World: 4'
+[INFO] [1794411107.119790845] [talker]: Publishing: 'Hello World: 5'
+--- listener (first 8 lines) ---
+[INFO] [1794411109.136528230] [listener]: I heard: [Hello World: 7]
+[INFO] [1794411110.126860298] [listener]: I heard: [Hello World: 8]
+[INFO] [1794411111.129708743] [listener]: I heard: [Hello World: 9]
+[INFO] [1794411112.132333626] [listener]: I heard: [Hello World: 10]
+[INFO] [1794411113.132083263] [listener]: I heard: [Hello World: 11]
+[INFO] [1794411114.132064498] [listener]: I heard: [Hello World: 12]
+[INFO] [1794411115.131003999] [listener]: I heard: [Hello World: 13]
+[INFO] [1794411116.131519385] [listener]: I heard: [Hello World: 14]
+=== node/topic while stopped ===
+/talker
+=== done Wed Nov 11 11:32:01 PM CST 2026 ===
diff --git a/docs/mujoco_car_nn/evidence/02_ros_install/wsl_install.log b/docs/mujoco_car_nn/evidence/02_ros_install/wsl_install.log
new file mode 100644
index 0000000000..998e94d4c3
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/02_ros_install/wsl_install.log differ
diff --git a/docs/mujoco_car_nn/evidence/02_ros_install/wsl_precheck.log b/docs/mujoco_car_nn/evidence/02_ros_install/wsl_precheck.log
new file mode 100644
index 0000000000..2eb9c6c12c
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/02_ros_install/wsl_precheck.log differ
diff --git a/docs/mujoco_car_nn/evidence/03_build/ros2_build.log b/docs/mujoco_car_nn/evidence/03_build/ros2_build.log
new file mode 100644
index 0000000000..8e8aacaf17
--- /dev/null
+++ b/docs/mujoco_car_nn/evidence/03_build/ros2_build.log
@@ -0,0 +1,33 @@
+=== Wed Nov 11 11:32:38 PM CST 2026 ===
+=== workspace ===
+assets
+DeepSeek Harness 全自动 ROS 工程实施任务书.md
+Dockerfile
+docs
+main.bat
+main.py
+main.sh
+mkdocs.yml
+README.md
+requirements.txt
+ros1
+ros2
+runs
+scripts
+src
+tests
+新建 文本文档 (5).txt
+commits: 23
+ROS_DISTRO=humble
+=== colcon list ===
+car_nn_sim car_nn_sim (ros.ament_python)
+=== colcon build ===
+Starting >>> car_nn_sim
+Finished <<< car_nn_sim [1.13s]
+
+Summary: 1 package finished [1.32s]
+COLCON_EXIT=0
+=== package visible? ===
+car_nn_sim
+/root/workspace/ROS/ros2/install/car_nn_sim
+=== done Wed Nov 11 11:32:46 PM CST 2026 ===
diff --git a/docs/mujoco_car_nn/evidence/03_simulation/01_mujoco_running.png b/docs/mujoco_car_nn/evidence/03_simulation/01_mujoco_running.png
new file mode 100644
index 0000000000..0508613a1b
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/03_simulation/01_mujoco_running.png differ
diff --git a/docs/mujoco_car_nn/evidence/03_simulation/02_mujoco_driving.png b/docs/mujoco_car_nn/evidence/03_simulation/02_mujoco_driving.png
new file mode 100644
index 0000000000..9d66a1a7ed
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/03_simulation/02_mujoco_driving.png differ
diff --git a/docs/mujoco_car_nn/evidence/03_simulation/03_mujoco_keyboard.png b/docs/mujoco_car_nn/evidence/03_simulation/03_mujoco_keyboard.png
new file mode 100644
index 0000000000..1194b75e1c
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/03_simulation/03_mujoco_keyboard.png differ
diff --git a/docs/mujoco_car_nn/evidence/04_keyboard/01_keyboard_console.png b/docs/mujoco_car_nn/evidence/04_keyboard/01_keyboard_console.png
new file mode 100644
index 0000000000..dcf866199a
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/04_keyboard/01_keyboard_console.png differ
diff --git a/docs/mujoco_car_nn/evidence/04_keyboard/02_keyboard_state.png b/docs/mujoco_car_nn/evidence/04_keyboard/02_keyboard_state.png
new file mode 100644
index 0000000000..b87c19deac
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/04_keyboard/02_keyboard_state.png differ
diff --git a/docs/mujoco_car_nn/evidence/05_sensors/camera_frame.png b/docs/mujoco_car_nn/evidence/05_sensors/camera_frame.png
new file mode 100644
index 0000000000..76b845dc90
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/05_sensors/camera_frame.png differ
diff --git a/docs/mujoco_car_nn/evidence/05_sensors/sensor_frame.png b/docs/mujoco_car_nn/evidence/05_sensors/sensor_frame.png
new file mode 100644
index 0000000000..cbc03d82b8
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/05_sensors/sensor_frame.png differ
diff --git a/docs/mujoco_car_nn/evidence/06_slam/ate.png b/docs/mujoco_car_nn/evidence/06_slam/ate.png
new file mode 100644
index 0000000000..c2480e35b1
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/06_slam/ate.png differ
diff --git a/docs/mujoco_car_nn/evidence/06_slam/slam_map.png b/docs/mujoco_car_nn/evidence/06_slam/slam_map.png
new file mode 100644
index 0000000000..030d36657b
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/06_slam/slam_map.png differ
diff --git a/docs/mujoco_car_nn/evidence/06_slam/slam_mapping.gif b/docs/mujoco_car_nn/evidence/06_slam/slam_mapping.gif
new file mode 100644
index 0000000000..68c5abd189
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/06_slam/slam_mapping.gif differ
diff --git a/docs/mujoco_car_nn/evidence/07_navigation/README.md b/docs/mujoco_car_nn/evidence/07_navigation/README.md
new file mode 100644
index 0000000000..b3929dceee
--- /dev/null
+++ b/docs/mujoco_car_nn/evidence/07_navigation/README.md
@@ -0,0 +1,8 @@
+# 导航证据
+
+本项目的“导航”由端到端策略直接完成(感知 → 规划 → 控制一体化),
+因此没有单独的全局路径规划模块截图。
+
+- 建图与定位:见 `../06_slam/`
+- 端到端控制:见 `../08_neural_network/`
+- 指标:ATE 2.37 m、地图 IoU 0.13、单圈 22.4 s、0 碰撞
diff --git a/docs/mujoco_car_nn/evidence/08_neural_network/comparison.png b/docs/mujoco_car_nn/evidence/08_neural_network/comparison.png
new file mode 100644
index 0000000000..5b4b4b8327
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/08_neural_network/comparison.png differ
diff --git a/docs/mujoco_car_nn/evidence/08_neural_network/nn_drive.gif b/docs/mujoco_car_nn/evidence/08_neural_network/nn_drive.gif
new file mode 100644
index 0000000000..8989a9e0e6
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/08_neural_network/nn_drive.gif differ
diff --git a/docs/mujoco_car_nn/evidence/08_neural_network/training_curves.png b/docs/mujoco_car_nn/evidence/08_neural_network/training_curves.png
new file mode 100644
index 0000000000..ce2e3a17c4
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/08_neural_network/training_curves.png differ
diff --git a/docs/mujoco_car_nn/evidence/09_ros1/README.md b/docs/mujoco_car_nn/evidence/09_ros1/README.md
new file mode 100644
index 0000000000..6d923138a4
--- /dev/null
+++ b/docs/mujoco_car_nn/evidence/09_ros1/README.md
@@ -0,0 +1,13 @@
+# ROS 1 证据(BLOCKED)
+
+本机未安装 WSL2 与 ROS 1,无法执行 `roscore` / `roslaunch`,因此没有截图。
+
+安装 WSL2 需要管理员权限并重启系统,按任务书要求已向用户申请授权,未擅自执行。
+
+可用的替代验证:
+
+```bash
+python -m pytest tests/test_ros_nodes.py -q
+```
+
+该测试用 `rospy` 桩模块实例化节点并跑控制周期,可提前发现消息字段错误。
diff --git a/docs/mujoco_car_nn/evidence/10_ros2/01_ros2_node_topic.png b/docs/mujoco_car_nn/evidence/10_ros2/01_ros2_node_topic.png
new file mode 100644
index 0000000000..e64147ec45
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/10_ros2/01_ros2_node_topic.png differ
diff --git a/docs/mujoco_car_nn/evidence/10_ros2/02_ros2_topics.png b/docs/mujoco_car_nn/evidence/10_ros2/02_ros2_topics.png
new file mode 100644
index 0000000000..865d248bc3
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/10_ros2/02_ros2_topics.png differ
diff --git a/docs/mujoco_car_nn/evidence/10_ros2/03_rviz_running.png b/docs/mujoco_car_nn/evidence/10_ros2/03_rviz_running.png
new file mode 100644
index 0000000000..850372b5bb
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/10_ros2/03_rviz_running.png differ
diff --git a/docs/mujoco_car_nn/evidence/10_ros2/04_rviz_slam_map.png b/docs/mujoco_car_nn/evidence/10_ros2/04_rviz_slam_map.png
new file mode 100644
index 0000000000..765c074dfa
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/10_ros2/04_rviz_slam_map.png differ
diff --git a/docs/mujoco_car_nn/evidence/10_ros2/05_slam_map_topic.png b/docs/mujoco_car_nn/evidence/10_ros2/05_slam_map_topic.png
new file mode 100644
index 0000000000..11914c4d0d
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/10_ros2/05_slam_map_topic.png differ
diff --git a/docs/mujoco_car_nn/evidence/10_ros2/README.md b/docs/mujoco_car_nn/evidence/10_ros2/README.md
new file mode 100644
index 0000000000..0c6cc1497f
--- /dev/null
+++ b/docs/mujoco_car_nn/evidence/10_ros2/README.md
@@ -0,0 +1,11 @@
+# ROS 2 证据(BLOCKED)
+
+本机未安装 WSL2 / Docker / ROS 2,无法执行 `colcon build` 与 `ros2 launch`,因此没有截图。
+
+仓库已提供 `ros2/car_nn_sim`(ament_python 包、3 个 launch.py)与 `Dockerfile`,
+具备 Linux 环境后可直接:
+
+```bash
+colcon build --packages-select car_nn_sim
+ros2 launch car_nn_sim car_nn.launch.py policy:=/abs/path/runs/cnn_dagger.pt
+```
diff --git a/docs/mujoco_car_nn/evidence/10_ros2/ros2_cmdvel.log b/docs/mujoco_car_nn/evidence/10_ros2/ros2_cmdvel.log
new file mode 100644
index 0000000000..e990b95b6f
--- /dev/null
+++ b/docs/mujoco_car_nn/evidence/10_ros2/ros2_cmdvel.log
@@ -0,0 +1,20 @@
+=== Wed Nov 11 11:33:27 PM CST 2026 ===
+=== odom before ===
+x: 41.0
+y: 0.0
+z: 0.0
+---
+=== publish cmd_vel (linear.x=6.0 m/s) ===
+publisher: beginning loop
+publishing #1: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=6.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))
+
+=== odom after 4 s ===
+x: 41.13615070549174
+y: 0.5430140042022222
+z: 0.0
+---
+=== stop ===
+publisher: beginning loop
+publishing #1: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))
+
+=== done Wed Nov 11 11:33:47 PM CST 2026 ===
diff --git a/docs/mujoco_car_nn/evidence/10_ros2/ros2_run.log b/docs/mujoco_car_nn/evidence/10_ros2/ros2_run.log
new file mode 100644
index 0000000000..3b20e248f8
--- /dev/null
+++ b/docs/mujoco_car_nn/evidence/10_ros2/ros2_run.log
@@ -0,0 +1,53 @@
+=== Wed Nov 11 11:32:53 PM CST 2026 ===
+ROS_DISTRO=humble MUJOCO_GL=egl
+launch pid=20878
+=== ros2 node list ===
+/car_nn_sim
+=== ros2 topic list -t ===
+/car/camera/image_raw [sensor_msgs/msg/Image]
+/car/cmd_vel [geometry_msgs/msg/Twist]
+/car/odom [nav_msgs/msg/Odometry]
+/car/scan [sensor_msgs/msg/LaserScan]
+/parameter_events [rcl_interfaces/msg/ParameterEvent]
+/rosout [rcl_interfaces/msg/Log]
+=== ros2 topic hz /car/camera/image_raw ===
+average rate: 3.778
+ min: 0.255s max: 0.289s std dev: 0.01210s window: 5
+average rate: 3.795
+ min: 0.242s max: 0.306s std dev: 0.01957s window: 9
+=== ros2 topic hz /car/scan ===
+average rate: 3.405
+ min: 0.272s max: 0.309s std dev: 0.01267s window: 5
+average rate: 3.361
+ min: 0.272s max: 0.312s std dev: 0.01128s window: 9
+=== ros2 topic echo /car/odom (once) ===
+header:
+ stamp:
+ sec: 1794411196
+ nanosec: 866953311
+ frame_id: odom
+child_frame_id: base_link
+pose:
+ pose:
+ position:
+ x: 41.0
+ y: 0.0
+ z: 0.0
+ orientation:
+ x: 0.0
+ y: 0.0
+ z: 0.6151408985995401
+ w: 0.7884171959503105
+ covariance:
+ - 0.0
+ - 0.0
+ - 0.0
+ - 0.0
+=== ros2 topic echo /car/scan (once, ranges head) ===
+array('f', [16.480789184570312, 13.844217300415039, 11.727333068847656, 10.118706703186035, 8.802031517028809, 8.002681732177734, 7.054258823394775, 6.464868545532227, 5.9677510261535645, 5.514983654022217, 5.26263952255249, 19.969478607177734, 4.797378063201904, 4.669932842254639, 4.545849800109863, 4.522549152374268, 4.5093865394592285, 4.5387163162231445, 4.58425235748291, 4.71827507019043, 4.82182502746582, 5.021739482879639, 5.276589870452881, 5.800379276275635, 5.953859329223633, 6.450514793395996, 7.259812831878662, 7.802388668060303, 8.766115188598633, 9.934226036071777, 11.334588050842285, 13.063814163208008, 15.092897415161133, 17.342153549194336, 19.87251853942871, 20.0, 20.0, 19.971813201904297, 19.98567771911621, 9.216120719909668, 7.305655002593994, 6.285926818847656, 5.708380699157715, 5.226786136627197, 4.940518856048584, 4.756781578063965, 4.582003593444824, 4.524087905883789, 4.508118152618408, 4.54625940322876, 4.6049065589904785, 4.771099090576172, 4.939167499542236, 5.269857883453369, 5.7347412109375, 6.355469703674316, 7.237951755523682, 8.767492294311523, 12.922614097595215, 20.0, 19.985557556152344, 20.0, 20.0, 19.83741569519043])
+---
+=== node log (head) ===
+[INFO] [launch]: All log files can be found below /root/.ros/log/2026-11-11-23-32-54-294975-LAPTOP-I6OL53UB-20878
+[INFO] [launch]: Default logging verbosity is set to INFO
+[INFO] [sim_node-1]: process started with pid [20880]
+=== done Wed Nov 11 11:33:19 PM CST 2026 ===
diff --git a/docs/mujoco_car_nn/evidence/11_documentation/01_site_home.png b/docs/mujoco_car_nn/evidence/11_documentation/01_site_home.png
new file mode 100644
index 0000000000..6ddbb4409b
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/11_documentation/01_site_home.png differ
diff --git a/docs/mujoco_car_nn/evidence/11_documentation/02_site_experiments.png b/docs/mujoco_car_nn/evidence/11_documentation/02_site_experiments.png
new file mode 100644
index 0000000000..5788c4ef22
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/11_documentation/02_site_experiments.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/01_git_log.png b/docs/mujoco_car_nn/evidence/12_final/01_git_log.png
new file mode 100644
index 0000000000..f6281a4e8f
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/01_git_log.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_edge.png b/docs/mujoco_car_nn/evidence/12_final/_edge.png
new file mode 100644
index 0000000000..c92350de17
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_edge.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_edge2.png b/docs/mujoco_car_nn/evidence/12_final/_edge2.png
new file mode 100644
index 0000000000..4e0ec037f5
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_edge2.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_edge3.png b/docs/mujoco_car_nn/evidence/12_final/_edge3.png
new file mode 100644
index 0000000000..4e0ec037f5
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_edge3.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_edge4.png b/docs/mujoco_car_nn/evidence/12_final/_edge4.png
new file mode 100644
index 0000000000..4e0ec037f5
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_edge4.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_github_dialog.png b/docs/mujoco_car_nn/evidence/12_final/_github_dialog.png
new file mode 100644
index 0000000000..9a1965c9eb
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_github_dialog.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_github_login.png b/docs/mujoco_car_nn/evidence/12_final/_github_login.png
new file mode 100644
index 0000000000..1db8710311
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_github_login.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_pr_page.png b/docs/mujoco_car_nn/evidence/12_final/_pr_page.png
new file mode 100644
index 0000000000..3efe2ce764
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_pr_page.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_pr_page2.png b/docs/mujoco_car_nn/evidence/12_final/_pr_page2.png
new file mode 100644
index 0000000000..8c325cfe91
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_pr_page2.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_pr_page3.png b/docs/mujoco_car_nn/evidence/12_final/_pr_page3.png
new file mode 100644
index 0000000000..b90ee301b2
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_pr_page3.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_pr_page4.png b/docs/mujoco_car_nn/evidence/12_final/_pr_page4.png
new file mode 100644
index 0000000000..64e354d21c
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_pr_page4.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_pr_page5.png b/docs/mujoco_car_nn/evidence/12_final/_pr_page5.png
new file mode 100644
index 0000000000..60f39150cf
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_pr_page5.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_pr_page6.png b/docs/mujoco_car_nn/evidence/12_final/_pr_page6.png
new file mode 100644
index 0000000000..60f39150cf
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_pr_page6.png differ
diff --git a/docs/mujoco_car_nn/evidence/12_final/_screen.png b/docs/mujoco_car_nn/evidence/12_final/_screen.png
new file mode 100644
index 0000000000..6e727045ca
Binary files /dev/null and b/docs/mujoco_car_nn/evidence/12_final/_screen.png differ
diff --git a/docs/mujoco_car_nn/evidence/ENVIRONMENT_REPORT.md b/docs/mujoco_car_nn/evidence/ENVIRONMENT_REPORT.md
new file mode 100644
index 0000000000..d680a39dbd
--- /dev/null
+++ b/docs/mujoco_car_nn/evidence/ENVIRONMENT_REPORT.md
@@ -0,0 +1,25 @@
+# 环境检查报告(入口)
+
+完整版见 [`01_environment/ENVIRONMENT_REPORT.md`](01_environment/ENVIRONMENT_REPORT.md)。
+
+## 摘要
+
+| 项 | 状态 |
+|---|---|
+| Windows 11 家庭版 10.0.26200(64 位) | ✅ |
+| AMD Ryzen 9 8945HX / 32 GB / RTX 5060 | ✅ |
+| 管理员权限 | ❌(全程未提权,安装时由本人 UAC 授权) |
+| WSL2 | ❌ 未安装(2026-11-10 尝试安装,停在下载阶段) |
+| Ubuntu 22.04 | ❌ 无 |
+| Docker | ❌ 未安装 |
+| Python(Harness 运行时) | ✅ 3.12.14 |
+| Git(MinGit) | ✅ 2.56.0 |
+| MuJoCo / PyTorch / pytest / mkdocs | ✅ 4.0.0 / 2.15.0+cpu / 9.2.0 / 1.5.9 |
+| ROS 1 / ROS 2 | ❌ 未安装(BLOCKED) |
+| 桌面截图 / 键鼠自动化 | ✅ 已实测 |
+
+## 相关文件
+
+- `01_environment/01_windows_env.png` —— 真实控制台截图
+- `01_environment/env_check.log` —— 原始命令输出
+- `02_ros_install/STATUS_2026-11-10.md` —— WSL 安装尝试记录
diff --git a/docs/mujoco_car_nn/evidence/FINAL_ACCEPTANCE_REPORT.md b/docs/mujoco_car_nn/evidence/FINAL_ACCEPTANCE_REPORT.md
new file mode 100644
index 0000000000..5d782750e8
--- /dev/null
+++ b/docs/mujoco_car_nn/evidence/FINAL_ACCEPTANCE_REPORT.md
@@ -0,0 +1,222 @@
+# 最终验收报告
+
+> 项目:`C:\Users\zhoul\OneDrive\Desktop\ROS`(MuJoCo 无人车神经网络驾驶 + 栅格 SLAM + ROS 1/2 包)
+> 验收时间:2026-11-11 23:45(Asia/Shanghai)
+> 执行方式:DeepSeek Harness 桌面自动化 + 命令行,**只记录实际执行过的结果**
+
+## 1. 项目简介
+
+一套可复现的无人车课程作业:MuJoCo 物理仿真(227 m 闭环赛道、9 m 宽路面)、
+64×64 车载相机、64 束激光雷达、pure-pursuit 专家、端到端 CNN(BC / DAgger)、
+栅格建图与扫描匹配 SLAM,并提供 ROS 1 Noetic 与 ROS 2 Humble 两套包与 launch。
+
+## 2. Windows 环境
+
+| 项 | 值 |
+|---|---|
+| 操作系统 | Windows 11 家庭版 10.0.26200(64 位) |
+| 硬件 | Ryzen 9 8945HX(16C/32T)、31.8 GB、RTX 5060 Laptop |
+| 权限 | 非管理员(安装时由本人 UAC 授权) |
+| 桌面自动化 | 窗口枚举/置前/截图/键鼠注入均实测可用 |
+
+## 3. WSL2 安装情况
+
+**PASS**(2026-11-11 01:16 完成,无需重启)
+
+| 项 | 值 |
+|---|---|
+| WSL 应用 | `MicrosoftCorporationII.WindowsSubsystemForLinux 3.0.5.0` |
+| 发行版 | `Ubuntu-22.04`,`VERSION=2`,状态 Stopped/Running 正常 |
+| 内核 | `6.18.58.1-microsoft-standard-WSL2` |
+| 安装方式 | 经用户 UAC 批准后 `wsl --install -d Ubuntu-22.04 --no-launch`,后台完成 |
+
+## 4. Ubuntu 版本
+
+`Ubuntu 22.04.5 LTS (Jammy)`,默认 root shell 可用,已配置阿里云 apt 镜像与 en_US.UTF-8 locale。
+
+## 5. ROS 2 安装情况
+
+**PASS** —— ROS 2 **Humble**(`/opt/ros/humble`)
+
+| 项 | 值 |
+|---|---|
+| 软件源 | `https://mirrors.tuna.tsinghua.edu.cn/ros2/ubuntu jammy main`(官方源在国内不可达) |
+| 密钥 | Open Robotics `C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654`(经 Gitee 镜像获取,已校验指纹) |
+| 主要包 | `ros-humble-desktop`、`ros-dev-tools`、`python3-colcon-common-extensions`、`python3-rosdep`、`python3-vcstool`、`python3-argcomplete` |
+| 环境变量 | `ROS_DISTRO=humble`,`source /opt/ros/humble/setup.bash` 正常 |
+| rosdep | `rosdep init/update` **失败**:`raw.githubusercontent.com` 被拒绝(非致命,本项目包无外部 rosdep 依赖) |
+
+## 6. 开发依赖
+
+| 组件 | 版本 |
+|---|---|
+| Python | 3.10.12 |
+| pip | 22.0.2 |
+| gcc / g++ | 11.4.0 |
+| cmake | 3.22.1 |
+| git | 2.34.1 |
+| MuJoCo(Ubuntu) | 3.17.0(`MUJOCO_GL=egl` 离屏渲染实测可用) |
+| NumPy(Ubuntu) | 1.21.5 |
+
+## 7. 编译情况
+
+**PASS**
+
+```text
+$ cd ~/workspace/ROS/ros2 && colcon build --symlink-install
+Starting >>> car_nn_sim
+Finished <<< car_nn_sim [1.13s]
+Summary: 1 package finished [1.32s]
+$ colcon list
+car_nn_sim car_nn_sim (ros.ament_python)
+$ ros2 pkg prefix car_nn_sim
+/root/workspace/ROS/ros2/install/car_nn_sim
+```
+
+工作副本位于 Linux 文件系统 `/root/workspace/ROS`(从 OneDrive 目录 rsync 而来,保留 23 次提交)。
+
+## 8. 节点运行情况
+
+**PASS**
+
+```text
+$ ros2 launch car_nn_sim car_nn.launch.py
+$ ros2 node list
+/car_nn_sim
+```
+
+节点启动后同时发布相机、雷达、里程计,并订阅 `/car/cmd_vel`。
+
+## 9. 话题通信情况
+
+**PASS**
+
+| 话题 | 类型 | 实测 |
+|---|---|---|
+| `/car/camera/image_raw` | `sensor_msgs/msg/Image` | 约 3.8 Hz(EGL 软件渲染限制) |
+| `/car/scan` | `sensor_msgs/msg/LaserScan` | 约 3.4 Hz,64 束 |
+| `/car/odom` | `nav_msgs/msg/Odometry` | `frame_id: odom` → `base_link`,x=41.0, y=0.0 |
+| `/car/cmd_vel` | `geometry_msgs/msg/Twist` | 发布后 odom 由 (41.00, 0.00) 变为 (41.14, 0.54) |
+
+官方 talker/listener 通信测试:
+
+```text
+[talker] Publishing: 'Hello World: 1..5'
+[listener] I heard: [Hello World: 7]
+...
+[listener] I heard: [Hello World: 14]
+```
+
+**PASS**(真实 DDS 通信,非模拟)。
+
+## 10. 仿真情况
+
+- Windows 侧:MuJoCo 原生窗口真实启动,车辆/赛道/护栏渲染正常(`03_simulation/`)。
+- WSL 侧:`sim_node` 以 `MUJOCO_GL=egl` 离屏运行,相机与雷达数据由真实物理步进产生。
+- 帧率约 3.8 Hz(WSL 软件渲染),低于 Windows 原生的 20 Hz,属环境限制而非代码问题。
+
+## 11. 传感器情况
+
+| 传感器 | 结果 |
+|---|---|
+| 相机 | 64×64 rgb8,`Image` 消息 12288 字节 |
+| 激光雷达 | `LaserScan` 64 束,量程 20 m,`range_min=0.05` |
+| 里程计 | `Odometry`,位置/姿态/速度字段完整 |
+
+## 12. SLAM 情况
+
+**PASS**
+
+Windows 侧算法验证:单圈 ATE **2.37 m**(纯里程计 2.98 m)、地图 IoU **0.13**。
+
+ROS 侧(`car_slam.launch.py` = sim_node + slam_node)实测:
+
+```text
+$ ros2 topic echo --once /map --field info
+resolution: 0.25 width: 440 height: 360 origin: (-55.0, -45.0)
+$ ros2 topic hz /map
+average rate: 1.000
+$ ros2 run tf2_ros tf2_echo map base_link
+Translation: [0.000, 0.000, 0.000] Rotation: [0, 0, 0, 1]
+```
+
+`/map` 以 1 Hz 发布 `nav_msgs/OccupancyGrid`,`map→base_link` TF 正常。
+
+## 12.1 RViz
+
+**PASS(窗口与 OpenGL)**
+
+```text
+$ rviz2 -d car.rviz
+[rviz2]: OpenGL version: 4.2 (GLSL 4.2)
+```
+
+WSLg 可用(`DISPLAY=:0`、`WAYLAND_DISPLAY=wayland-0`),RViz 窗口正常显示。
+说明:`car.rviz` 只加载了 Grid 显示,Map/LaserScan 显示项未自动生效,需在 RViz 里手动
+Add → By topic 选择 `/map`、`/car/scan`;数据本身已通过 CLI 验证。
+
+## 13. 神经网络实验
+
+| 策略 | 平均速度 (m/s) | 单圈 (s) | 横向 RMSE (m) | 碰撞 |
+|---|---|---|---|---|
+| 专家 | 10.41 | 22.1 | 0.49 | 0 |
+| CNN(BC) | 8.03 | 28.6 | 0.45 | 0 |
+| **CNN(BC+DAgger)** | **10.29** | **22.4** | 0.47 | 0 |
+| MLP(BC) | 6.75 | 34.0 | 0.38 | 0 |
+
+day / dusk / fog 共 12 组实验全部完成,fog 为零样本场景。
+推理 1.5 ms/帧(CPU)。消融:纯图像 CNN 原地不动;模块化方案横向误差 1.89 m。
+
+## 14. 自动化测试
+
+```text
+$ python -m pytest tests -q
+...... [100%]
+6 passed in 4.72s
+```
+
+含 ROS 1 / ROS 2 节点桩测试(MOCK 级别,不代表真实 ROS 运行)。
+
+## 15. 截图索引
+
+| 文件 | 类型 | 内容 |
+|---|---|---|
+| `01_environment/01_windows_env.png` | 真实截图 | Windows 版本、WSL 未装(当时)、工具链 |
+| `02_build/01_pytest.png` | 真实截图 | pytest 6 passed |
+| `02_build/02_mkdocs_build.png` | 真实截图 | mkdocs strict 构建 |
+| `03_simulation/01_mujoco_running.png` | 真实截图 | MuJoCo 窗口 + 赛道 |
+| `03_simulation/02_mujoco_driving.png` | 真实截图 | 车辆行驶 |
+| `04_keyboard/01_keyboard_console.png` | 真实截图 | 按键序列与状态 |
+| `04_keyboard/02_keyboard_state.png` | 真实截图 | `keys=WWDD`、油门/转向 |
+| `05_sensors/*` | 程序输出 | 相机、雷达极坐标、里程计 |
+| `06_slam/*` | 程序输出 | 栅格地图、ATE、建图 GIF |
+| `08_neural_network/*` | 程序输出 | 训练曲线、对比、驾驶 GIF |
+| `10_ros2/01_ros2_node_topic.png` | 真实截图 | 话题列表 + odom echo |
+| `10_ros2/02_ros2_topics.png` | 真实截图 | node list / topic list -t / odom |
+| `10_ros2/03_rviz_running.png` | 真实截图 | RViz2 窗口(WSLg) |
+| `10_ros2/04_rviz_slam_map.png` | 真实截图 | RViz 加载 car.rviz |
+| `10_ros2/05_slam_map_topic.png` | 真实截图 | /map info、1 Hz、TF |
+| `11_documentation/*` | 真实截图 | MkDocs 站点 |
+| `12_final/01_git_log.png` | 真实截图 | 提交历史 |
+
+日志:`02_ros_install/{base_setup,ros2_install,ros2_verify,ros2_run,ros2_cmdvel}.log`、
+`03_build/ros2_build.log`。
+
+## 16. ROS 1 状态
+
+**BLOCKED** —— ROS 1 Noetic 需要 Ubuntu 20.04,不能在 22.04 混装。
+仓库中 `ros1/car_nn_sim` 已就绪,需另建 Ubuntu 20.04 发行版(或课程虚拟机)后 `catkin_make`。
+
+## 17. 未完成事项
+
+1. ROS 1 Noetic(需 Ubuntu 20.04,课程允许 Noetic **或** Humble,当前用 Humble 验收)。
+2. 真人键盘连续驾驶演示(自动按键序列会撞护栏,PARTIAL)。
+3. RViz 显示项需手动 Add(配置文件只自动加载了 Grid)。
+4. 10 次同学评审。
+
+## 18. 后续建议
+
+1. 先验证 WSLg:`sudo apt install -y mesa-utils && glxinfo -B`,能出窗口再截 RViz。
+2. 把 `slam_node` 加进 `car_slam.launch.py` 的实测流程,发布 `/map` 并截图。
+3. 若课程强制 ROS 1,新建 Ubuntu 20.04 发行版单独验收,不要混装。
+4. 帧率问题:WSL 软件渲染只有 ~4 Hz,可用 `MUJOCO_GL=osmesa` 或降低渲染分辨率改善。
diff --git a/docs/mujoco_car_nn/evidence/README.md b/docs/mujoco_car_nn/evidence/README.md
new file mode 100644
index 0000000000..3966e0e5e0
--- /dev/null
+++ b/docs/mujoco_car_nn/evidence/README.md
@@ -0,0 +1,40 @@
+# 验收证据目录
+
+本目录记录 **2026-11-10 ~ 11-11 实际执行** 的环境检查、构建、测试、仿真与 ROS 2 验收证据。
+每条证据都标注类型,避免把示意图当作实时运行截图:
+
+| 类型 | 含义 |
+|---|---|
+| **真实截图** | 从真实窗口(终端 / MuJoCo / 浏览器)抓屏得到 |
+| **程序输出** | 由仿真器或算法直接生成的图片(不是屏幕截图) |
+| **日志** | 命令的原始文本输出 |
+
+## 目录
+
+| 目录 | 内容 | 状态 |
+|---|---|---|
+| `01_environment/` | Windows 版本、权限、工具链 | ✅ 完成 |
+| `02_ros_install/` | WSL2 安装、Ubuntu 基础环境、ROS 2 安装与 talker/listener | ✅ 完成(ROS 2 **PASS**) |
+| `03_build/` | pytest、mkdocs、`colcon build` | ✅ 完成 |
+| `04_keyboard/` | 键盘事件注入与实时状态 | ⚠️ 部分(按键通路已验证,自动驾驶会撞墙) |
+| `05_sensors/` | 相机 / 雷达 / 里程计 | ✅ 完成(程序输出) |
+| `06_slam/` | 栅格地图、ATE、建图动图 | ✅ 完成(程序输出) |
+| `07_navigation/` | 导航说明 | ✅ 说明(导航由端到端策略完成) |
+| `08_neural_network/` | 训练曲线、对比实验、驾驶动图 | ✅ 完成 |
+| `09_ros1/` | ROS 1 证据 | ⛔ **BLOCKED**(需 Ubuntu 20.04) |
+| `10_ros2/` | ROS 2 节点、话题、odom、cmd_vel 测试 | ✅ **PASS** |
+| `11_documentation/` | MkDocs 站点截图 | ✅ 完成 |
+| `12_final/` | Git 提交历史 | ✅ 完成 |
+
+## ROS 2 关键日志
+
+| 文件 | 内容 |
+|---|---|
+| `02_ros_install/base_setup.log` | 阿里云镜像切换 + 基础工具安装 |
+| `02_ros_install/ros2_install.log` | ROS 2 Humble 安装(清华镜像,含密钥指纹) |
+| `02_ros_install/ros2_verify.log` | talker/listener 真实通信(Hello World 7–14) |
+| `10_ros2/ros2_run.log` | node list / topic list -t / hz / echo |
+| `10_ros2/ros2_cmdvel.log` | 发布 `/car/cmd_vel` 后 odom 位移 |
+| `03_build/ros2_build.log` | `colcon build` 成功 |
+
+完整结论见 [`FINAL_ACCEPTANCE_REPORT.md`](FINAL_ACCEPTANCE_REPORT.md)。
diff --git a/docs/mujoco_car_nn/experiments.md b/docs/mujoco_car_nn/experiments.md
new file mode 100644
index 0000000000..cc2f4859e7
--- /dev/null
+++ b/docs/mujoco_car_nn/experiments.md
@@ -0,0 +1,116 @@
+# 实验与评价
+
+## 1. 实验设置
+
+| 项 | 设置 |
+|---|---|
+| 训练数据 | 专家驾驶 5 圈 × {day, dusk},20 Hz,共 **4137 帧** |
+| 训练 | AdamW,lr \(10^{-3}\),weight decay \(10^{-4}\),batch 64,20 epoch,CPU |
+| 评测 | 1 圈(227 m),起点随机偏移 \(\pm1.2\) m,随机种子 42 |
+| 场景 | day / dusk / fog,其中 fog **未参与训练**(零样本) |
+| 指标 | 完成圈数、成功率、碰撞次数、横向误差 RMSE/最大值、平均速度、单圈时间、推理耗时 |
+| 硬件 | 笔记本 CPU(未使用 GPU),Windows 11 |
+
+**成功率**定义:完成 \(\ge 1\) 圈且碰撞次数为 0。**单圈时间**=仿真时长/完成圈数。
+**横向误差**取整圈 \(|e_y|\) 的 RMSE。
+
+## 2. 主结果
+
+| 策略 | 场景 | 完成圈数 | 成功率 | 碰撞 | 横向 RMSE (m) | 横向最大 (m) | 平均速度 (m/s) | 单圈 (s) | 推理 (ms) |
+|---|---|---|---|---|---|---|---|---|---|
+| expert | day | 1.00 | 是 | 0 | 0.49 | 0.76 | 10.41 | 22.1 | — |
+| expert | dusk | 1.00 | 是 | 0 | 0.49 | 0.76 | 10.41 | 22.1 | — |
+| expert | fog | 1.00 | 是 | 0 | 0.49 | 0.76 | 10.41 | 22.1 | — |
+| cnn_bc | day | 1.00 | 是 | 0 | 0.45 | 0.69 | 8.03 | 28.6 | 1.54 |
+| cnn_bc | dusk | 1.00 | 是 | 0 | 0.45 | 0.71 | 8.21 | 28.0 | 1.60 |
+| cnn_bc | fog | 1.00 | 是 | 0 | 0.44 | 0.67 | 8.11 | 28.3 | 1.53 |
+| **cnn_dagger** | **day** | **1.00** | **是** | **0** | **0.47** | 0.74 | **10.29** | **22.4** | 1.58 |
+| cnn_dagger | dusk | 1.00 | 是 | 0 | 0.48 | 0.74 | 10.21 | 22.5 | 1.56 |
+| cnn_dagger | fog | 1.00 | 是 | 0 | 0.47 | 0.73 | 10.25 | 22.4 | 1.71 |
+| mlp_bc | day | 1.00 | 是 | 0 | 0.38 | 0.63 | 6.75 | 34.0 | 1.43 |
+| mlp_bc | dusk | 1.00 | 是 | 0 | 0.34 | 0.67 | 5.31 | 43.2 | 1.31 |
+| mlp_bc | fog | 1.00 | 是 | 0 | 0.35 | 0.59 | 6.73 | 34.0 | 1.36 |
+
+> 专家不依赖图像,因此三种光照下指标完全相同;它的作用是对照"人类水平"。
+
+
+
+*图 1:BC 与 DAgger 的损失曲线。DAgger 的初始损失更高(数据分布更宽),但收敛后验证损失更低。*
+
+
+
+*图 2:不同策略在三种场景下的横向误差与完成圈数。*
+
+## 3. 结果分析
+
+### 3.1 DAgger 修复了协变量偏移
+
+| 指标 | BC | BC + DAgger | 专家 |
+|---|---|---|---|
+| 平均速度 (m/s) | 8.03 | **10.29** | 10.41 |
+| 单圈时间 (s) | 28.6 | **22.4** | 22.1 |
+| 横向 RMSE (m) | 0.45 | 0.47 | 0.49 |
+
+BC 策略"不敢开快":一旦速度略高就会偏离专家分布,网络没见过这种状态,于是保守地松油门。
+DAgger 用当前策略自己的状态分布重新标注了 2 轮(数据量 4137 → 约 1.1 万帧),
+策略在高速状态上也获得了监督,单圈时间从 28.6 s 降到 22.4 s,**与专家仅差 0.3 s**。
+
+### 3.2 CNN 与 MLP:精度与效率的权衡
+
+| 指标 | CNN | MLP |
+|---|---|---|
+| 参数量 | 0.12 M | 3.1 M |
+| 横向 RMSE (m) | 0.45 | **0.38** |
+| 平均速度 (m/s) | **8.03** | 6.75 |
+| 单圈时间 (s) | **28.6** | 34.0 |
+
+MLP 的横向误差反而更低,但代价是**开得极慢**:它把"贴着车道中心慢慢走"作为降低损失的策略,
+说明全连接层难以从 12288 维原始像素中提取"弯道曲率",只能保守行驶。
+CNN 用 1/26 的参数量换来了 19% 的速度提升,验证了卷积结构对图像输入的归纳偏置是必要的。
+
+### 3.3 多场景泛化
+
+`cnn_dagger` 在 dusk(训练过)与 fog(**零样本**)上的平均速度为 10.21 / 10.25 m/s,
+与 day 的 10.29 m/s 几乎一致,横向误差差异 < 0.01 m。
+这说明:
+
+- 训练时的光度增强(亮度 ×[0.85,1.15]、±12 灰度)确实提升了光照鲁棒性;
+- 网络主要依赖**车道线的几何结构**而非颜色/亮度,因此雾天也能稳定跟随。
+
+## 4. 消融实验
+
+| 变体 | 完成圈数 | 碰撞 | 平均速度 (m/s) | 横向 RMSE (m) | 结论 |
+|---|---|---|---|---|---|
+| 纯图像 CNN(无车速输入) | 0.00 | 0 | 0.00 | 0.15 | 起跑线"趴窝":单帧图像无法区分静止/巡航 |
+| 模块化:感知 + MLP 控制器 | 0.13 | 3 | 2.86 | 1.77 | 感知在分布内 MAE 0.01 m,出分布后失效 |
+| 模块化:感知 + 几何控制器 | 1.00 | 1 | 7.94 | 1.89 | 能跑完,但贴边、易碰墙 |
+| **端到端 CNN + DAgger** | **1.00** | **0** | **10.29** | **0.47** | 主方案 |
+
+**纯图像 CNN** 是最有说服力的反例:训练损失很低、验证集 MAE 也很小,但闭环中油门输出
+始终为负(弯道处专家习惯松油),车辆原地不动。加入 1 维车速后问题立即消失。
+
+**模块化方案**暴露了另一个问题:感知网络在专家分布内精度极高(0.01–0.02 m),
+但闭环一旦漂到 ±2 m 之外,预测迅速退化为 0,控制器失去反馈。
+把训练分布加宽(起点 ±3.5 m、更强扰动,4979 帧)后可以跑完一圈,但横向误差仍是端到端方案的 4 倍。
+结论:**感知、规划、控制分开训练时,每一环的分布偏移都会累积;端到端 + DAgger 让三者协同适应。**
+
+## 5. 失败案例
+
+| 现象 | 原因 | 改进方向 |
+|---|---|---|
+| 纯图像策略原地不动 | 油门标签依赖车速,单帧不可观测 | 增加车速输入(已修复) |
+| 模块化策略贴内侧护栏 | 感知出分布退化、控制器无积分项 | 感知 DAgger、加积分/前馈 |
+| 高速(>13 m/s)进弯撞墙 | 训练数据最高 12 m/s,未见过更高速 | 课程学习、速度扰动 |
+| 雾天远景误检 | 20 m 外护栏对比度低 | 雷达融合、域随机化 |
+
+## 6. 复现
+
+```bash
+python main.py collect --laps 5 --scenes day,dusk
+python main.py benchmark --epochs 20 --laps 1 --scenes day,dusk,fog
+python scripts/ablation.py # 纯图像 / 模块化消融
+python main.py demo --model runs/cnn_dagger.pt --seconds 12
+python main.py slam --laps 1
+```
+
+所有随机种子固定,结果可复现;`docs/figures/results.json`、`ablation.json` 保存了原始数值。
diff --git a/docs/mujoco_car_nn/figures/ablation.json b/docs/mujoco_car_nn/figures/ablation.json
new file mode 100644
index 0000000000..8394c00a26
--- /dev/null
+++ b/docs/mujoco_car_nn/figures/ablation.json
@@ -0,0 +1,146 @@
+{
+ "cnn_image_only_day": {
+ "policy": "cnn_image_only",
+ "scene": "day",
+ "laps_target": 1.0,
+ "laps_completed": 0.0,
+ "success": false,
+ "collisions": 0,
+ "lateral_rmse": 0.14669174459507178,
+ "lateral_max": 0.1466917445950718,
+ "speed_mean": 0.0,
+ "speed_max": 0.0,
+ "sim_seconds": 10.05,
+ "lap_time": 10050000.000000002,
+ "steps": 201,
+ "inference_ms": 1.5706000849604607
+ },
+ "cnn_image_only_dusk": {
+ "policy": "cnn_image_only",
+ "scene": "dusk",
+ "laps_target": 1.0,
+ "laps_completed": 0.0,
+ "success": false,
+ "collisions": 0,
+ "lateral_rmse": 0.14669174459507178,
+ "lateral_max": 0.1466917445950718,
+ "speed_mean": 0.0,
+ "speed_max": 0.0,
+ "sim_seconds": 10.05,
+ "lap_time": 10050000.000000002,
+ "steps": 201,
+ "inference_ms": 1.436399994418025
+ },
+ "cnn_image_only_fog": {
+ "policy": "cnn_image_only",
+ "scene": "fog",
+ "laps_target": 1.0,
+ "laps_completed": 0.0,
+ "success": false,
+ "collisions": 0,
+ "lateral_rmse": 0.14669174459507178,
+ "lateral_max": 0.1466917445950718,
+ "speed_mean": 0.0,
+ "speed_max": 0.0,
+ "sim_seconds": 10.05,
+ "lap_time": 10050000.000000002,
+ "steps": 201,
+ "inference_ms": 1.520700054243207
+ },
+ "modular_day": {
+ "policy": "modular",
+ "scene": "day",
+ "laps_target": 1.0,
+ "laps_completed": 0.12885462555066082,
+ "success": false,
+ "collisions": 3,
+ "lateral_rmse": 1.7709652913199765,
+ "lateral_max": 2.416987682746148,
+ "speed_mean": 2.8585079123866097,
+ "speed_max": 10.152336340062893,
+ "sim_seconds": 10.05,
+ "lap_time": 77.99487179487178,
+ "steps": 201,
+ "inference_ms": 1.3772000093013048
+ },
+ "modular_dusk": {
+ "policy": "modular",
+ "scene": "dusk",
+ "laps_target": 1.0,
+ "laps_completed": 0.12885462555066082,
+ "success": false,
+ "collisions": 1,
+ "lateral_rmse": 1.7995960948517602,
+ "lateral_max": 2.3675566026929866,
+ "speed_mean": 2.846001919238269,
+ "speed_max": 10.115657815087882,
+ "sim_seconds": 10.05,
+ "lap_time": 77.99487179487178,
+ "steps": 201,
+ "inference_ms": 1.4460000675171614
+ },
+ "modular_fog": {
+ "policy": "modular",
+ "scene": "fog",
+ "laps_target": 1.0,
+ "laps_completed": 0.12885462555066082,
+ "success": false,
+ "collisions": 3,
+ "lateral_rmse": 1.7865283882687673,
+ "lateral_max": 2.3865521112093377,
+ "speed_mean": 2.850893703654815,
+ "speed_max": 10.155014304577495,
+ "sim_seconds": 10.05,
+ "lap_time": 77.99487179487178,
+ "steps": 201,
+ "inference_ms": 1.3831001706421375
+ },
+ "modular_geo_day": {
+ "policy": "modular_geo",
+ "scene": "day",
+ "laps_target": 1.0,
+ "laps_completed": 1.0,
+ "success": false,
+ "collisions": 1,
+ "lateral_rmse": 1.8861211592868348,
+ "lateral_max": 3.3880330703130506,
+ "speed_mean": 7.939154472340233,
+ "speed_max": 9.297840610496538,
+ "sim_seconds": 30.1,
+ "lap_time": 30.1,
+ "steps": 602,
+ "inference_ms": 1.6555500915274024
+ },
+ "modular_geo_dusk": {
+ "policy": "modular_geo",
+ "scene": "dusk",
+ "laps_target": 1.0,
+ "laps_completed": 1.0,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 1.9144447533549012,
+ "lateral_max": 2.869558219299409,
+ "speed_mean": 8.140428267993912,
+ "speed_max": 9.301108598068609,
+ "sim_seconds": 29.400000000000002,
+ "lap_time": 29.400000000000002,
+ "steps": 588,
+ "inference_ms": 1.682500122115016
+ },
+ "modular_geo_fog": {
+ "policy": "modular_geo",
+ "scene": "fog",
+ "laps_target": 1.0,
+ "laps_completed": 1.0,
+ "success": false,
+ "collisions": 1,
+ "lateral_rmse": 1.8694676503486602,
+ "lateral_max": 3.3786667521246443,
+ "speed_mean": 7.944388794224152,
+ "speed_max": 9.295294425611491,
+ "sim_seconds": 30.05,
+ "lap_time": 30.05,
+ "steps": 601,
+ "inference_ms": 1.6199999954551458
+ }
+}
\ No newline at end of file
diff --git a/docs/mujoco_car_nn/figures/architecture.png b/docs/mujoco_car_nn/figures/architecture.png
new file mode 100644
index 0000000000..8bd628f36e
Binary files /dev/null and b/docs/mujoco_car_nn/figures/architecture.png differ
diff --git a/docs/mujoco_car_nn/figures/ate.png b/docs/mujoco_car_nn/figures/ate.png
new file mode 100644
index 0000000000..c2480e35b1
Binary files /dev/null and b/docs/mujoco_car_nn/figures/ate.png differ
diff --git a/docs/mujoco_car_nn/figures/chase.png b/docs/mujoco_car_nn/figures/chase.png
new file mode 100644
index 0000000000..adf81b225e
Binary files /dev/null and b/docs/mujoco_car_nn/figures/chase.png differ
diff --git a/docs/mujoco_car_nn/figures/comparison.png b/docs/mujoco_car_nn/figures/comparison.png
new file mode 100644
index 0000000000..5b4b4b8327
Binary files /dev/null and b/docs/mujoco_car_nn/figures/comparison.png differ
diff --git a/docs/mujoco_car_nn/figures/ego_camera.png b/docs/mujoco_car_nn/figures/ego_camera.png
new file mode 100644
index 0000000000..76b845dc90
Binary files /dev/null and b/docs/mujoco_car_nn/figures/ego_camera.png differ
diff --git a/docs/mujoco_car_nn/figures/map.png b/docs/mujoco_car_nn/figures/map.png
new file mode 100644
index 0000000000..030d36657b
Binary files /dev/null and b/docs/mujoco_car_nn/figures/map.png differ
diff --git a/docs/mujoco_car_nn/figures/preview_expert_drive.png b/docs/mujoco_car_nn/figures/preview_expert_drive.png
new file mode 100644
index 0000000000..6dc90525b7
Binary files /dev/null and b/docs/mujoco_car_nn/figures/preview_expert_drive.png differ
diff --git a/docs/mujoco_car_nn/figures/preview_nn_drive.png b/docs/mujoco_car_nn/figures/preview_nn_drive.png
new file mode 100644
index 0000000000..e4d968e090
Binary files /dev/null and b/docs/mujoco_car_nn/figures/preview_nn_drive.png differ
diff --git a/docs/mujoco_car_nn/figures/preview_slam_mapping.png b/docs/mujoco_car_nn/figures/preview_slam_mapping.png
new file mode 100644
index 0000000000..fe41f8cf34
Binary files /dev/null and b/docs/mujoco_car_nn/figures/preview_slam_mapping.png differ
diff --git a/docs/mujoco_car_nn/figures/preview_teleop.png b/docs/mujoco_car_nn/figures/preview_teleop.png
new file mode 100644
index 0000000000..4587036354
Binary files /dev/null and b/docs/mujoco_car_nn/figures/preview_teleop.png differ
diff --git a/docs/mujoco_car_nn/figures/results.json b/docs/mujoco_car_nn/figures/results.json
new file mode 100644
index 0000000000..73446b8dae
--- /dev/null
+++ b/docs/mujoco_car_nn/figures/results.json
@@ -0,0 +1,681 @@
+{
+ "results": [
+ {
+ "policy": "expert",
+ "scene": "day",
+ "laps_target": 1.0,
+ "laps_completed": 1.001101321585903,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.4899749364972928,
+ "lateral_max": 0.7642728911036811,
+ "speed_mean": 10.411283472137928,
+ "speed_max": 12.478605596290421,
+ "sim_seconds": 22.1,
+ "lap_time": 22.075687568756877,
+ "steps": 442,
+ "inference_ms": 0.0
+ },
+ {
+ "policy": "expert",
+ "scene": "dusk",
+ "laps_target": 1.0,
+ "laps_completed": 1.001101321585903,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.4899749364972928,
+ "lateral_max": 0.7642728911036811,
+ "speed_mean": 10.411283472137928,
+ "speed_max": 12.478605596290421,
+ "sim_seconds": 22.1,
+ "lap_time": 22.075687568756877,
+ "steps": 442,
+ "inference_ms": 0.0
+ },
+ {
+ "policy": "expert",
+ "scene": "fog",
+ "laps_target": 1.0,
+ "laps_completed": 1.001101321585903,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.4899749364972928,
+ "lateral_max": 0.7642728911036811,
+ "speed_mean": 10.411283472137928,
+ "speed_max": 12.478605596290421,
+ "sim_seconds": 22.1,
+ "lap_time": 22.075687568756877,
+ "steps": 442,
+ "inference_ms": 0.0
+ },
+ {
+ "policy": "cnn_bc",
+ "scene": "day",
+ "laps_target": 1.0,
+ "laps_completed": 1.0,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.45069974501278137,
+ "lateral_max": 0.6853400343739171,
+ "speed_mean": 8.031841560148973,
+ "speed_max": 12.078386367972632,
+ "sim_seconds": 28.6,
+ "lap_time": 28.6,
+ "steps": 572,
+ "inference_ms": 1.543849939480424
+ },
+ {
+ "policy": "cnn_bc",
+ "scene": "dusk",
+ "laps_target": 1.0,
+ "laps_completed": 1.001101321585903,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.4500960269150501,
+ "lateral_max": 0.7080313545174607,
+ "speed_mean": 8.209148405836848,
+ "speed_max": 12.221262921475358,
+ "sim_seconds": 28.0,
+ "lap_time": 27.96919691969197,
+ "steps": 560,
+ "inference_ms": 1.6014000866562128
+ },
+ {
+ "policy": "cnn_bc",
+ "scene": "fog",
+ "laps_target": 1.0,
+ "laps_completed": 1.0,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.4444060316652277,
+ "lateral_max": 0.6678567362320212,
+ "speed_mean": 8.114978246213271,
+ "speed_max": 12.150792236184484,
+ "sim_seconds": 28.3,
+ "lap_time": 28.3,
+ "steps": 566,
+ "inference_ms": 1.5335500938817859
+ },
+ {
+ "policy": "cnn_dagger",
+ "scene": "day",
+ "laps_target": 1.0,
+ "laps_completed": 1.0,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.4695095736781466,
+ "lateral_max": 0.7376032491788357,
+ "speed_mean": 10.286024952627677,
+ "speed_max": 12.537529514442845,
+ "sim_seconds": 22.35,
+ "lap_time": 22.35,
+ "steps": 447,
+ "inference_ms": 1.5830998308956623
+ },
+ {
+ "policy": "cnn_dagger",
+ "scene": "dusk",
+ "laps_target": 1.0,
+ "laps_completed": 1.0,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.4755785334090076,
+ "lateral_max": 0.7414077901015944,
+ "speed_mean": 10.211500740333706,
+ "speed_max": 12.555294143318818,
+ "sim_seconds": 22.5,
+ "lap_time": 22.5,
+ "steps": 450,
+ "inference_ms": 1.5551000833511353
+ },
+ {
+ "policy": "cnn_dagger",
+ "scene": "fog",
+ "laps_target": 1.0,
+ "laps_completed": 1.001101321585903,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.46806872398022953,
+ "lateral_max": 0.7318922154541841,
+ "speed_mean": 10.252707805051527,
+ "speed_max": 12.522531795480532,
+ "sim_seconds": 22.450000000000003,
+ "lap_time": 22.425302530253028,
+ "steps": 449,
+ "inference_ms": 1.7085000872612
+ },
+ {
+ "policy": "mlp_bc",
+ "scene": "day",
+ "laps_target": 1.0,
+ "laps_completed": 1.0,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.38377721973332274,
+ "lateral_max": 0.6297901490496866,
+ "speed_mean": 6.7496172392276055,
+ "speed_max": 11.043600138718855,
+ "sim_seconds": 33.95,
+ "lap_time": 33.95,
+ "steps": 679,
+ "inference_ms": 1.4253000263124704
+ },
+ {
+ "policy": "mlp_bc",
+ "scene": "dusk",
+ "laps_target": 1.0,
+ "laps_completed": 1.001101321585903,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.3398345879222989,
+ "lateral_max": 0.6665961701815384,
+ "speed_mean": 5.307042773682899,
+ "speed_max": 11.110002941100387,
+ "sim_seconds": 43.2,
+ "lap_time": 43.152475247524755,
+ "steps": 864,
+ "inference_ms": 1.310699968598783
+ },
+ {
+ "policy": "mlp_bc",
+ "scene": "fog",
+ "laps_target": 1.0,
+ "laps_completed": 1.0,
+ "success": true,
+ "collisions": 0,
+ "lateral_rmse": 0.3483558383658495,
+ "lateral_max": 0.5899463471708201,
+ "speed_mean": 6.73497137621501,
+ "speed_max": 10.444798265990498,
+ "sim_seconds": 33.95,
+ "lap_time": 33.95,
+ "steps": 679,
+ "inference_ms": 1.359499990940094
+ }
+ ],
+ "histories": {
+ "cnn_bc": {
+ "train_loss": [
+ 0.024618820166560234,
+ 0.011443872384175106,
+ 0.009103842993715295,
+ 0.0069430662422544425,
+ 0.005780755042063969,
+ 0.004705589258163754,
+ 0.0034962862889558353,
+ 0.002711820937865586,
+ 0.002067799205650334,
+ 0.0017814881198487623,
+ 0.0014811095046483118,
+ 0.0010577909841366043,
+ 0.0008779541655072804,
+ 0.0007390285215849333,
+ 0.0006747735331171296,
+ 0.0006473880655701376,
+ 0.0005632065995423882,
+ 0.0005428608737592758,
+ 0.0005013076453771511,
+ 0.0005062500745730682
+ ],
+ "val_loss": [
+ 0.01272738894387599,
+ 0.008724355706644635,
+ 0.008111345656816997,
+ 0.006444684668414054,
+ 0.005411583395494569,
+ 0.004430363681768217,
+ 0.003563772171976105,
+ 0.002598054667243794,
+ 0.002539010853656838,
+ 0.0021516847533113773,
+ 0.0018310554212920608,
+ 0.0018827074714335462,
+ 0.0016466879191988659,
+ 0.0014325794009041161,
+ 0.0014885802430311037,
+ 0.001475374189941513,
+ 0.0014690739237853596,
+ 0.0013873624545897567,
+ 0.0014217029592292684,
+ 0.0014132726911245093
+ ],
+ "val_steer_mae": [
+ 0.016500770853411766,
+ 0.011128566438151945,
+ 0.014397758918423806,
+ 0.009571465801808142,
+ 0.00918790380800924,
+ 0.00995557500470069,
+ 0.009548433557633431,
+ 0.009598241217674748,
+ 0.007951029846745153,
+ 0.00658827789368168,
+ 0.006593562181918851,
+ 0.006960416128558498,
+ 0.005716122110043803,
+ 0.005476341204297158,
+ 0.00559129782261387,
+ 0.005366636380072563,
+ 0.005024555181303332,
+ 0.00499871404420945,
+ 0.005164028031210745,
+ 0.005021743596561493
+ ],
+ "val_throttle_mae": [
+ 0.13885883515880953,
+ 0.10508415314459031,
+ 0.11106079855272846,
+ 0.09326915664057578,
+ 0.08373349520467943,
+ 0.07837829897480626,
+ 0.0675619702185354,
+ 0.05712012590900544,
+ 0.058129096031188966,
+ 0.05121551040680178,
+ 0.045905942493869416,
+ 0.0488182581240131,
+ 0.042647294075258316,
+ 0.0400159026345899,
+ 0.04079762947174811,
+ 0.041565714151628554,
+ 0.039708007343353766,
+ 0.03813664855495576,
+ 0.03838585134475462,
+ 0.0382915283403089
+ ],
+ "train_seconds": 20.363404512405396,
+ "n_train": 3517,
+ "n_val": 620,
+ "best_val": 0.0013873624545897567
+ },
+ "mlp_bc": {
+ "train_loss": [
+ 0.04763781510431458,
+ 0.011924606523717995,
+ 0.012404355349847011,
+ 0.00994376423423765,
+ 0.009617695567646512,
+ 0.009664817773564547,
+ 0.007309210628995465,
+ 0.006653967244481599,
+ 0.005812864841168953,
+ 0.005656332081545972,
+ 0.005278218774711368,
+ 0.0045764588242120765,
+ 0.0040472751586801475,
+ 0.0038011867975540183,
+ 0.003223931250869538,
+ 0.0031009716913104057,
+ 0.003007669780075688,
+ 0.0027457341331885094,
+ 0.002628747587247441,
+ 0.0026257641471198034
+ ],
+ "val_loss": [
+ 0.01363756267293807,
+ 0.009915964491665364,
+ 0.010015911650994132,
+ 0.00803120381409122,
+ 0.013952362675580286,
+ 0.007991649658088722,
+ 0.007420504534797322,
+ 0.006636967110417543,
+ 0.0052714796706793765,
+ 0.005022819170487984,
+ 0.005470133296424343,
+ 0.006262398910738768,
+ 0.00434396403330949,
+ 0.004317676257942954,
+ 0.0049078750919791,
+ 0.0040534596094080515,
+ 0.003968358899076139,
+ 0.0036609480578091835,
+ 0.0037454974927729175,
+ 0.0037294507702632295
+ ],
+ "val_steer_mae": [
+ 0.030276646537165487,
+ 0.03323873204569663,
+ 0.04139059935846636,
+ 0.01727807416069892,
+ 0.018801124057462138,
+ 0.015484153263030513,
+ 0.011157832847487541,
+ 0.009587735754828299,
+ 0.013004160888733402,
+ 0.012448923722390206,
+ 0.009266832278620811,
+ 0.010674007044684502,
+ 0.009831585518775448,
+ 0.010739442706108094,
+ 0.01024008325992092,
+ 0.0069633919865854325,
+ 0.008356139496449502,
+ 0.0068804306368674,
+ 0.0067819622735823355,
+ 0.006873202852664456
+ ],
+ "val_throttle_mae": [
+ 0.1473082503964824,
+ 0.11719637455478792,
+ 0.11906589538820328,
+ 0.10300014634286203,
+ 0.16023108959197999,
+ 0.10964338933267902,
+ 0.10666766089777793,
+ 0.10119631598072668,
+ 0.08373381514703074,
+ 0.08487607663677585,
+ 0.09360443007561468,
+ 0.08951073769600161,
+ 0.07302149688043902,
+ 0.07258810074098648,
+ 0.07808524754739576,
+ 0.0711612666806867,
+ 0.06731619681081465,
+ 0.06546690617838213,
+ 0.06601043631953578,
+ 0.06608815693086194
+ ],
+ "train_seconds": 16.305819511413574,
+ "n_train": 3517,
+ "n_val": 620,
+ "best_val": 0.0036609480578091835
+ },
+ "cnn_dagger": {
+ "bc": {
+ "train_loss": [
+ 0.024618820166560234,
+ 0.011443872384175106,
+ 0.009103842993715295,
+ 0.0069430662422544425,
+ 0.005780755042063969,
+ 0.004705589258163754,
+ 0.0034962862889558353,
+ 0.002711820937865586,
+ 0.002067799205650334,
+ 0.0017814881198487623,
+ 0.0014811095046483118,
+ 0.0010577909841366043,
+ 0.0008779541655072804,
+ 0.0007390285215849333,
+ 0.0006747735331171296,
+ 0.0006473880655701376,
+ 0.0005632065995423882,
+ 0.0005428608737592758,
+ 0.0005013076453771511,
+ 0.0005062500745730682
+ ],
+ "val_loss": [
+ 0.01272738894387599,
+ 0.008724355706644635,
+ 0.008111345656816997,
+ 0.006444684668414054,
+ 0.005411583395494569,
+ 0.004430363681768217,
+ 0.003563772171976105,
+ 0.002598054667243794,
+ 0.002539010853656838,
+ 0.0021516847533113773,
+ 0.0018310554212920608,
+ 0.0018827074714335462,
+ 0.0016466879191988659,
+ 0.0014325794009041161,
+ 0.0014885802430311037,
+ 0.001475374189941513,
+ 0.0014690739237853596,
+ 0.0013873624545897567,
+ 0.0014217029592292684,
+ 0.0014132726911245093
+ ],
+ "val_steer_mae": [
+ 0.016500770853411766,
+ 0.011128566438151945,
+ 0.014397758918423806,
+ 0.009571465801808142,
+ 0.00918790380800924,
+ 0.00995557500470069,
+ 0.009548433557633431,
+ 0.009598241217674748,
+ 0.007951029846745153,
+ 0.00658827789368168,
+ 0.006593562181918851,
+ 0.006960416128558498,
+ 0.005716122110043803,
+ 0.005476341204297158,
+ 0.00559129782261387,
+ 0.005366636380072563,
+ 0.005024555181303332,
+ 0.00499871404420945,
+ 0.005164028031210745,
+ 0.005021743596561493
+ ],
+ "val_throttle_mae": [
+ 0.13885883515880953,
+ 0.10508415314459031,
+ 0.11106079855272846,
+ 0.09326915664057578,
+ 0.08373349520467943,
+ 0.07837829897480626,
+ 0.0675619702185354,
+ 0.05712012590900544,
+ 0.058129096031188966,
+ 0.05121551040680178,
+ 0.045905942493869416,
+ 0.0488182581240131,
+ 0.042647294075258316,
+ 0.0400159026345899,
+ 0.04079762947174811,
+ 0.041565714151628554,
+ 0.039708007343353766,
+ 0.03813664855495576,
+ 0.03838585134475462,
+ 0.0382915283403089
+ ],
+ "train_seconds": 21.3261559009552,
+ "n_train": 3517,
+ "n_val": 620,
+ "best_val": 0.0013873624545897567
+ },
+ "dagger": [
+ {
+ "train_loss": [
+ 0.02421982778998261,
+ 0.011882343705193225,
+ 0.008743608007948338,
+ 0.0063348281546495855,
+ 0.004472649676079693,
+ 0.003638256972242909,
+ 0.0024593366648663173,
+ 0.0018944132144414734,
+ 0.0015591724844522435,
+ 0.0012664139860796488,
+ 0.0009961493210126223,
+ 0.0008670712632010691,
+ 0.0007398767126935788,
+ 0.0007380473798548337,
+ 0.0006349987592825942,
+ 0.0005571730861530258,
+ 0.0005427392297860933,
+ 0.0004867922461099542,
+ 0.00047705167526146397,
+ 0.0004779247076591921
+ ],
+ "val_loss": [
+ 0.014650223451060849,
+ 0.009313254402234005,
+ 0.007242064299685257,
+ 0.006003951812004828,
+ 0.004607349448092096,
+ 0.0037005533244215197,
+ 0.0031964881006036547,
+ 0.0024435240342714393,
+ 0.002322003573782139,
+ 0.0020661259419633656,
+ 0.002252519359583264,
+ 0.001795853132853744,
+ 0.0016709533447937502,
+ 0.0017196794723641578,
+ 0.001521706302463264,
+ 0.0015657249714022727,
+ 0.0014948052803420097,
+ 0.001512818237393071,
+ 0.0014705332796220536,
+ 0.0014830741564511552
+ ],
+ "val_steer_mae": [
+ 0.016184739001861938,
+ 0.013705614384833155,
+ 0.01787642421541395,
+ 0.009236831080306184,
+ 0.0089427152356425,
+ 0.011831730485081553,
+ 0.008603406923038738,
+ 0.007609191176655528,
+ 0.00867970780535535,
+ 0.008180843336837038,
+ 0.005617435787107561,
+ 0.006352214338062527,
+ 0.0059674653437706855,
+ 0.0057945519715517785,
+ 0.005337111867748417,
+ 0.0050353367369134466,
+ 0.004972605319408984,
+ 0.00493488885782339,
+ 0.004880879457656678,
+ 0.004958303836913971
+ ],
+ "val_throttle_mae": [
+ 0.15305585937423782,
+ 0.1110538707508312,
+ 0.08918970519607955,
+ 0.08325844520812745,
+ 0.0764138672378037,
+ 0.06696595059527265,
+ 0.06394499879736047,
+ 0.051118215242704074,
+ 0.05068534690064269,
+ 0.04524671126317073,
+ 0.04746912123559119,
+ 0.04204590682621364,
+ 0.03941200020072701,
+ 0.04046206278996272,
+ 0.03811721808903224,
+ 0.03779966978879123,
+ 0.037184591417188766,
+ 0.036464986505803765,
+ 0.036117791653155804,
+ 0.03641045224535596
+ ],
+ "train_seconds": 32.234153270721436,
+ "n_train": 5677,
+ "n_val": 1001,
+ "best_val": 0.0014705332796220536
+ },
+ {
+ "train_loss": [
+ 0.0221804904920811,
+ 0.010958858321166852,
+ 0.007319609586472723,
+ 0.004762742448067061,
+ 0.0033588401260323947,
+ 0.002292713635766586,
+ 0.0018792575622585501,
+ 0.0015320979585016746,
+ 0.0012132918143698808,
+ 0.0009819235225027227,
+ 0.0008997488372443506,
+ 0.0007582883830884595,
+ 0.0006873951445541476,
+ 0.0006046130016209434,
+ 0.0005517769963307353,
+ 0.0005097026766656921,
+ 0.00046211019894858623,
+ 0.00044548396304469124,
+ 0.00042389407174273064,
+ 0.000424514056211299
+ ],
+ "val_loss": [
+ 0.011812869942323728,
+ 0.010335867826911536,
+ 0.005979727491905743,
+ 0.004686752758920193,
+ 0.0027562153631482613,
+ 0.0029207883298566395,
+ 0.002328175163065845,
+ 0.0018457760469014334,
+ 0.0017479147914035075,
+ 0.001528754502365535,
+ 0.0015906646830368448,
+ 0.0013530613835689357,
+ 0.0014283132958014242,
+ 0.0014434175792531194,
+ 0.0014224407663992183,
+ 0.001313460072354329,
+ 0.0014017335579264909,
+ 0.0013692976486107166,
+ 0.0013908178248993037,
+ 0.001411172962726348
+ ],
+ "val_steer_mae": [
+ 0.01685344752398404,
+ 0.012468398852781816,
+ 0.009765873670578002,
+ 0.00775936035676436,
+ 0.007602801149541681,
+ 0.006469292651523243,
+ 0.005936293667012995,
+ 0.005527821865948764,
+ 0.005601321968165311,
+ 0.006032951029864225,
+ 0.004680925661867315,
+ 0.004408289237455888,
+ 0.004498932068998164,
+ 0.004040107163515958,
+ 0.0044582614031704985,
+ 0.004035145878791809,
+ 0.00402190208976919,
+ 0.003979762608354742,
+ 0.0039027379317717117,
+ 0.00393838877027685
+ ],
+ "val_throttle_mae": [
+ 0.12117808428677646,
+ 0.11283129570700906,
+ 0.0905581129247492,
+ 0.06544470006769354,
+ 0.05447769633206454,
+ 0.056822441707957874,
+ 0.0531430007761175,
+ 0.04168947141820734,
+ 0.04101503866369074,
+ 0.041727234536951235,
+ 0.03862801863930442,
+ 0.03638428497314453,
+ 0.03452300635251132,
+ 0.03504024960777976,
+ 0.03334595116701993,
+ 0.032439530112526635,
+ 0.03300504276969216,
+ 0.03254215296831998,
+ 0.03242253615639427,
+ 0.03275822305679321
+ ],
+ "train_seconds": 46.4324951171875,
+ "n_train": 7794,
+ "n_val": 1375,
+ "best_val": 0.001313460072354329
+ }
+ ]
+ }
+ },
+ "config": {
+ "cmd": "benchmark",
+ "data": "C:\\Users\\zhoul\\OneDrive\\Desktop\\ROS\\data\\dataset.npz",
+ "epochs": 20,
+ "dagger_iters": 2,
+ "laps": 1.0,
+ "scenes": "day,dusk,fog",
+ "max_steps": 2500
+ }
+}
\ No newline at end of file
diff --git a/docs/mujoco_car_nn/figures/results_table.md b/docs/mujoco_car_nn/figures/results_table.md
new file mode 100644
index 0000000000..8d5536817e
--- /dev/null
+++ b/docs/mujoco_car_nn/figures/results_table.md
@@ -0,0 +1,14 @@
+| 策略 | 场景 | 完成圈数 | 成功率 | 碰撞次数 | 横向误差 RMSE (m) | 横向误差最大 (m) | 平均速度 (m/s) | 单圈时间 (s) | 推理耗时 (ms) |
+|---|---|---|---|---|---|---|---|---|---|
+| expert | day | 1.00 | 是 | 0 | 0.49 | 0.76 | 10.41 | 22.1 | 0.00 |
+| expert | dusk | 1.00 | 是 | 0 | 0.49 | 0.76 | 10.41 | 22.1 | 0.00 |
+| expert | fog | 1.00 | 是 | 0 | 0.49 | 0.76 | 10.41 | 22.1 | 0.00 |
+| cnn_bc | day | 1.00 | 是 | 0 | 0.45 | 0.69 | 8.03 | 28.6 | 1.54 |
+| cnn_bc | dusk | 1.00 | 是 | 0 | 0.45 | 0.71 | 8.21 | 28.0 | 1.60 |
+| cnn_bc | fog | 1.00 | 是 | 0 | 0.44 | 0.67 | 8.11 | 28.3 | 1.53 |
+| cnn_dagger | day | 1.00 | 是 | 0 | 0.47 | 0.74 | 10.29 | 22.4 | 1.58 |
+| cnn_dagger | dusk | 1.00 | 是 | 0 | 0.48 | 0.74 | 10.21 | 22.5 | 1.56 |
+| cnn_dagger | fog | 1.00 | 是 | 0 | 0.47 | 0.73 | 10.25 | 22.4 | 1.71 |
+| mlp_bc | day | 1.00 | 是 | 0 | 0.38 | 0.63 | 6.75 | 34.0 | 1.43 |
+| mlp_bc | dusk | 1.00 | 是 | 0 | 0.34 | 0.67 | 5.31 | 43.2 | 1.31 |
+| mlp_bc | fog | 1.00 | 是 | 0 | 0.35 | 0.59 | 6.73 | 34.0 | 1.36 |
diff --git a/docs/mujoco_car_nn/figures/scenes.png b/docs/mujoco_car_nn/figures/scenes.png
new file mode 100644
index 0000000000..635f3d095e
Binary files /dev/null and b/docs/mujoco_car_nn/figures/scenes.png differ
diff --git a/docs/mujoco_car_nn/figures/site_experiments.png b/docs/mujoco_car_nn/figures/site_experiments.png
new file mode 100644
index 0000000000..5788c4ef22
Binary files /dev/null and b/docs/mujoco_car_nn/figures/site_experiments.png differ
diff --git a/docs/mujoco_car_nn/figures/site_home.png b/docs/mujoco_car_nn/figures/site_home.png
new file mode 100644
index 0000000000..6ddbb4409b
Binary files /dev/null and b/docs/mujoco_car_nn/figures/site_home.png differ
diff --git a/docs/mujoco_car_nn/figures/site_slam.png b/docs/mujoco_car_nn/figures/site_slam.png
new file mode 100644
index 0000000000..b1003770fc
Binary files /dev/null and b/docs/mujoco_car_nn/figures/site_slam.png differ
diff --git a/docs/mujoco_car_nn/figures/track_top.png b/docs/mujoco_car_nn/figures/track_top.png
new file mode 100644
index 0000000000..0bdfe0e4c6
Binary files /dev/null and b/docs/mujoco_car_nn/figures/track_top.png differ
diff --git a/docs/mujoco_car_nn/figures/training_curves.png b/docs/mujoco_car_nn/figures/training_curves.png
new file mode 100644
index 0000000000..ce2e3a17c4
Binary files /dev/null and b/docs/mujoco_car_nn/figures/training_curves.png differ
diff --git a/docs/mujoco_car_nn/gifs/expert_drive.gif b/docs/mujoco_car_nn/gifs/expert_drive.gif
new file mode 100644
index 0000000000..a0eea5ebbd
Binary files /dev/null and b/docs/mujoco_car_nn/gifs/expert_drive.gif differ
diff --git a/docs/mujoco_car_nn/gifs/nn_drive.gif b/docs/mujoco_car_nn/gifs/nn_drive.gif
new file mode 100644
index 0000000000..8989a9e0e6
Binary files /dev/null and b/docs/mujoco_car_nn/gifs/nn_drive.gif differ
diff --git a/docs/mujoco_car_nn/gifs/slam_mapping.gif b/docs/mujoco_car_nn/gifs/slam_mapping.gif
new file mode 100644
index 0000000000..68c5abd189
Binary files /dev/null and b/docs/mujoco_car_nn/gifs/slam_mapping.gif differ
diff --git a/docs/mujoco_car_nn/gifs/teleop.gif b/docs/mujoco_car_nn/gifs/teleop.gif
new file mode 100644
index 0000000000..9e14d262de
Binary files /dev/null and b/docs/mujoco_car_nn/gifs/teleop.gif differ
diff --git a/docs/mujoco_car_nn/index.md b/docs/mujoco_car_nn/index.md
new file mode 100644
index 0000000000..828710b428
--- /dev/null
+++ b/docs/mujoco_car_nn/index.md
@@ -0,0 +1,94 @@
+# 基于 MuJoCo 与神经网络的无人车仿真驾驶
+
+本项目面向《开源技术与实践》课程作业,在一套**完全可复现的 MuJoCo 仿真环境**中完成无人车的
+物理仿真、传感器感知、神经网络端到端驾驶、栅格建图与 SLAM,并提供 ROS 1(Noetic)与
+ROS 2(Humble)两套 launch 启动接口。
+
+
+
+!!! info "一句话概括"
+ 用纯 Python + MuJoCo 搭建一条 227 m 的闭环赛道,采集专家(pure-pursuit)驾驶数据,
+ 训练一个 **图像 → 转向/油门** 的端到端卷积网络,并用 DAgger 消除分布偏移;
+ 同时用 64 线激光雷达做占据栅格建图与扫描匹配定位。
+
+## 与作业要求的对应关系
+
+| 作业要求 | 本项目实现 | 代码 / 文档 |
+|---|---|---|
+| (1)物理仿真 + 键盘运动控制 | MuJoCo 平面刚体车辆(自行车模型 + 碰撞)、`main.py teleop`、ROS `car_teleop.launch` | [仿真环境](simulation.md) |
+| (2)传感器感知 + 运动控制 | 车载 RGB 相机、64 线激光雷达、里程计;感知→决策→控制的闭环 | [神经网络算法](algorithm.md) |
+| (3)建图与导航、SLAM 同步仿真 | 占据栅格建图 + 相关法扫描匹配定位,ATE / IoU 评价 | [建图与 SLAM](slam.md) |
+| (4)端到端模型(图像→控制) | `PolicyCNN`:64×64 图像 + 车速 → 转向、油门 | [神经网络算法](algorithm.md) |
+| 多场景测试 + 性能评价 | day / dusk / fog 三种光照,成功率、横向误差、单圈时间、推理耗时 | [实验与评价](experiments.md) |
+| launch 启动 / Ubuntu 20.04 | ROS 1 Noetic 与 ROS 2 Humble 的 launch、节点、话题 | [ROS 部署](ros.md) |
+| 同样模型不同算法、同样算法不同结构 | BC vs DAgger;CNN vs MLP | [实验与评价](experiments.md) |
+
+## 核心结果
+
+在 227 m 闭环赛道上行驶 1 圈,三种光照场景下均为 0 碰撞、100% 完成:
+
+| 策略 | 平均速度 (m/s) | 单圈时间 (s) | 横向误差 RMSE (m) | 推理耗时 (ms) |
+|---|---|---|---|---|
+| 专家(pure pursuit) | 10.41 | 22.1 | 0.49 | — |
+| 端到端 CNN(BC) | 8.03 | 28.6 | 0.45 | 1.6 |
+| 端到端 CNN(BC + DAgger) | **10.29** | **22.4** | 0.47 | 1.5 |
+| 端到端 MLP(BC) | 6.75 | 34.0 | 0.38 | 1.3 |
+
+DAgger 把端到端策略的平均车速从 8.0 m/s 提升到 10.3 m/s,几乎追平专家(10.4 m/s),
+验证了"单帧行为克隆存在协变量偏移、用专家在线标注可修复"这一结论。
+
+## 文档结构
+
+
+
+- :material-rocket-launch: **[快速开始](quickstart.md)**
+ 三条命令跑通:采集数据 → 训练 → 闭环评测。
+
+- :material-car-side: **[仿真环境](simulation.md)**
+ 赛道参数方程、车辆动力学、相机与激光雷达建模。
+
+- :material-brain: **[神经网络算法](algorithm.md)**
+ CNN 结构、行为克隆损失、DAgger、pure-pursuit 专家。
+
+- :material-map: **[建图与 SLAM](slam.md)**
+ log-odds 栅格、射线投射、相关法扫描匹配、ATE / IoU。
+
+- :material-robot: **[ROS 部署](ros.md)**
+ Noetic 与 Humble 的编译、launch、话题与 TF。
+
+- :material-flask: **[实验与评价](experiments.md)**
+ 多场景对比、消融、失败案例分析。
+
+
+
+## 文档站点
+
+本站点使用课程提供的 **hutb-doc**(OpenHUTB 维护的 mkdocs 分支)构建,`push` 到 `main`
+后由 `.github/workflows/docs.yml` 自动发布到 GitHub Pages。
+
+
+
+
+
+## 仓库结构
+
+```text
+ROS/
+├── main.py # 统一入口:collect/train/eval/benchmark/demo/slam/teleop
+├── src/mujoco_car_nn/ # 核心 Python 包
+│ ├── sim.py # MuJoCo 场景、车辆动力学、相机/雷达
+│ ├── track.py # 赛道几何与墙体生成
+│ ├── expert.py # pure-pursuit 专家
+│ ├── dataset.py # 数据采集与 Dataset
+│ ├── models.py # PolicyCNN / PolicyMLP
+│ ├── train.py # BC / DAgger 训练
+│ ├── evaluate.py # 闭环评测指标
+│ ├── slam.py # 栅格建图与扫描匹配
+│ ├── viz.py # HUD、GIF、地图渲染
+│ └── cli.py # 命令行接口
+├── ros1/car_nn_sim/ # catkin 包(Noetic)
+├── ros2/car_nn_sim/ # ament_python 包(Humble)
+├── docs/ # 本文档(mkdocs)
+├── runs/ # 训练产物(gitignore)
+└── data/ # 采集数据(gitignore)
+```
diff --git a/docs/mujoco_car_nn/javascripts/mathjax.js b/docs/mujoco_car_nn/javascripts/mathjax.js
new file mode 100644
index 0000000000..80e81ba59b
--- /dev/null
+++ b/docs/mujoco_car_nn/javascripts/mathjax.js
@@ -0,0 +1,12 @@
+window.MathJax = {
+ tex: {
+ inlineMath: [["\\(", "\\)"]],
+ displayMath: [["\\[", "\\]"]],
+ processEscapes: true,
+ processEnvironments: true
+ },
+ options: {
+ ignoreHtmlClass: ".*|",
+ processHtmlClass: "arithmatex"
+ }
+};
diff --git a/docs/mujoco_car_nn/pr.md b/docs/mujoco_car_nn/pr.md
new file mode 100644
index 0000000000..8b28be14e4
--- /dev/null
+++ b/docs/mujoco_car_nn/pr.md
@@ -0,0 +1,124 @@
+# 提交与评审
+
+课程要求"提交次数和审核次数分别不少于 10 次",并且"Pull Request 标题不能随意,需要具体说明修改了什么"。
+本章给出可直接照做的提交计划、PR 模板与评审清单。
+
+## 1. 分支与提交计划
+
+从 `main` 切出功能分支 `feature/mujoco-car-nn`,按"一个关注点一次提交"的原则组织 12 次提交:
+
+| # | 提交信息 | 内容 |
+|---|---|---|
+| 1 | `chore: 初始化 mujoco_car_nn 模块骨架` | 目录、`__init__`、requirements、main.py |
+| 2 | `feat(track): 参数化闭环赛道与护栏生成` | `track.py`,227 m 赛道、曲率与墙体 |
+| 3 | `feat(sim): MuJoCo 平面刚体车辆与相机/雷达` | `sim.py`,动力学、传感器、离屏渲染 |
+| 4 | `feat(expert): pure-pursuit 专家与速度规划` | `expert.py` |
+| 5 | `feat(data): 专家数据采集与 Dataset` | `dataset.py`,4137 帧 npz |
+| 6 | `feat(model): 端到端 CNN 与 MLP 基线` | `models.py` |
+| 7 | `feat(train): 行为克隆与 DAgger 训练流程` | `train.py` |
+| 8 | `feat(eval): 闭环评测指标与多场景测试` | `evaluate.py`、`benchmark` 子命令 |
+| 9 | `feat(slam): 占据栅格建图与扫描匹配` | `slam.py`,ATE / IoU |
+| 10 | `feat(viz): HUD、演示 GIF 与地图渲染` | `viz.py`、`demo`/`slam` 子命令 |
+| 11 | `feat(ros): Noetic 与 Humble 节点及 launch` | `ros1/`、`ros2/` |
+| 12 | `docs: mkdocs 文档、实验数据与动图` | `docs/`、`mkdocs.yml`、`scripts/` |
+
+提交信息遵循 Conventional Commits,正文用中文说明"改了什么、为什么、怎么验证":
+
+```text
+feat(slam): 占据栅格建图与相关法扫描匹配
+
+- 实现 log-odds 栅格与射线投射,单帧更新 < 1 ms
+- 相关法扫描匹配采用粗→精两级搜索(111 候选/帧)
+- 仅对横向分量做修正,避免沿赛道方向的累计漂移
+- 单圈 ATE 由 2.98 m 降至 2.37 m,IoU 由 0.09 提升至 0.13
+
+验证:python main.py slam --laps 1
+```
+
+## 2. PR 模板
+
+```markdown
+## 做了什么
+新增 `mujoco_car_nn` 模块:MuJoCo 无人车仿真 + 端到端 CNN 驾驶 + 栅格 SLAM,
+提供 Noetic / Humble 两套 launch。
+
+## 为什么
+对应课程任务(1)~(4):物理仿真、传感器闭环、建图导航、端到端模型。
+
+## 怎么验证
+```bash
+pip install -r requirements.txt
+python main.py collect --laps 5 --scenes day,dusk
+python main.py train --model cnn --epochs 24 --dagger
+python main.py eval --model runs/cnn_dagger.pt --laps 2 --record
+python -m pytest tests -q
+```
+
+## 结果
+| 策略 | 单圈(s) | 横向 RMSE(m) | 碰撞 |
+|---|---|---|---|
+| 专家 | 22.1 | 0.49 | 0 |
+| CNN+DAgger | 22.4 | 0.47 | 0 |
+
+## 截图 / 动图
+(附 `docs/gifs/nn_drive.gif`)
+
+## 影响面
+仅新增文件,不修改既有模块;`main.py` 为唯一入口。
+```
+
+PR 标题要具体,例如:
+
+- ✅ `feat(mujoco_car_nn): 新增 MuJoCo 无人车端到端驾驶与栅格 SLAM 模块`
+- ❌ `update code`
+
+## 3. 评审清单(Reviewer 用)
+
+把仓库克隆到**另一台机器**(或课程虚拟机)后逐项检查:
+
+- [ ] `pip install -r requirements.txt` 成功,无 GPU 也能跑
+- [ ] `python main.py collect --laps 2` 能在 30 s 内产出 `data/dataset.npz`
+- [ ] `python main.py train --model cnn --epochs 10` 损失正常下降
+- [ ] `python main.py eval --expert --laps 1` 专家能 0 碰撞跑完一圈
+- [ ] `python main.py eval --model runs/cnn_dagger.pt --laps 1` 同样 0 碰撞
+- [ ] `python -m pytest tests -q` 全绿
+- [ ] `roslaunch car_nn_sim car_teleop.launch` 能键盘遥控(Noetic)
+- [ ] `roslaunch car_nn_sim car_slam.launch` 的 `/map` 能逐渐展开
+- [ ] `docs/` 下 GIF < 5 MB,且能正常播放
+- [ ] `mkdocs build` 无报错,导航完整
+
+## 4. 常见 review 意见与处理
+
+| 意见 | 处理 |
+|---|---|
+| "端到端为什么还要输入车速?" | 单帧图像无法区分静止/巡航,见[算法](algorithm.md)消融;纯图像版本在起跑线趴窝 |
+| "扫描匹配为什么只修横向?" | 沿赛道方向相关峰平坦,全量更新会累计漂移,见[SLAM](slam.md) |
+| "为什么不用 Gazebo/Carla?" | 课程要求"能在 Noetic/Humble 跑通",MuJoCo 纯 pip 安装、无 GPU 依赖,复现成本最低;接口层已抽象,替换仿真器只需改 `sim.py` |
+| "数据/权重不应该进仓库" | 已 gitignore,`collect`/`train` 两条命令可完全重建 |
+| "评测是否过拟合单圈?" | 起点随机 ±1.2 m、三种光照、多随机种子,见[实验](experiments.md) |
+
+## 5. 复现与提交检查
+
+```bash
+# 1. 干净环境
+python -m venv .venv && . .venv/bin/activate # Windows: .venv\Scripts\activate
+pip install -r requirements.txt
+
+# 2. 全流程
+python main.py collect --laps 5 --scenes day,dusk
+python main.py train --model cnn --epochs 20 --dagger
+python main.py benchmark --epochs 20 --laps 1
+python main.py demo --model runs/cnn_dagger.pt --seconds 12
+python main.py slam --laps 1
+
+# 3. 文档
+pip install mkdocs mkdocs-material
+mkdocs build # 输出 site/,可直接发布到 GitHub Pages
+```
+
+推送前确认:
+
+```bash
+git status # 不应包含 data/ runs/ site/
+git log --oneline # 至少 10 条有意义的提交
+```
diff --git a/docs/mujoco_car_nn/quickstart.md b/docs/mujoco_car_nn/quickstart.md
new file mode 100644
index 0000000000..ed47f60453
--- /dev/null
+++ b/docs/mujoco_car_nn/quickstart.md
@@ -0,0 +1,107 @@
+# 快速开始
+
+## 1. 环境要求
+
+| 平台 | 版本 | 说明 |
+|---|---|---|
+| Windows 10/11 | Python 3.10 – 3.12 | 原生运行,无需 ROS;开发与训练均在此完成 |
+| Ubuntu 20.04 | Python 3.8 + ROS Noetic | 课程虚拟机,运行 `ros1/` 的 launch |
+| Ubuntu 22.04 / Docker | ROS 2 Humble | 运行 `ros2/` 的 launch |
+
+依赖仅 5 个:`mujoco`(物理与渲染)、`torch`(CPU 版即可)、`numpy`、`pillow`、`matplotlib`。
+
+```bash
+pip install -r requirements.txt
+# 或者在无 GPU 的服务器上安装 CPU 版 torch
+pip install torch --index-url https://download.pytorch.org/whl/cpu
+```
+
+!!! note "Linux 无显示器服务器"
+ MuJoCo 默认使用 GLFW 创建离屏上下文。无显示器的 Ubuntu 服务器需要
+ `export MUJOCO_GL=egl`(或 `osmesa`),并安装 `libegl1`;Windows 原生无需设置。
+
+!!! tip "`python` 命令不可用?"
+ Windows 上如果 `python` 打开的是应用商店,说明没有安装官方 Python。请安装
+ Python 3.11 / 3.12 并勾选 **Add python.exe to PATH**;或者直接用 `py -3 main.py`
+ (`main.bat` 会优先调用 `py` 启动器)。
+
+!!! tip "Docker 运行(免安装 ROS)"
+ 仓库自带 `Dockerfile`(`ros:humble-ros-base` + CPU 版 MuJoCo):
+
+ ```bash
+ docker build -t car_nn .
+ docker run --rm car_nn python3 main.py eval --model runs/cnn_dagger.pt --laps 1
+ docker run --rm -it car_nn # 进入容器后可直接 ros2 launch car_nn_sim ...
+ ```
+
+ 镜像内设置 `MUJOCO_GL=egl`,无需 X server,适合服务器/CI 环境。
+
+## 2. 三条命令跑通全流程
+
+```bash
+# 1) 用 pure-pursuit 专家采集 5 圈、两种光照的模仿学习数据(约 10 s)
+python main.py collect --laps 5 --scenes day,dusk
+
+# 2) 行为克隆 + DAgger 训练端到端 CNN(约 2 min,CPU)
+python main.py train --model cnn --epochs 24 --dagger
+
+# 3) 闭环评测:跑 2 圈,输出成功率、横向误差、单圈时间
+python main.py eval --model runs/cnn_dagger.pt --scene day --laps 2 --record
+```
+
+第三条命令会在 `docs/gifs/` 下生成一段带 HUD 的评测动图,同时打印:
+
+```text
+cnn_dagger.pt day laps=2.00 success=Y coll= 0 lat_rmse=0.47 lat_max=0.74 v_mean=10.29 lap= 22.4s infer= 1.5ms
+```
+
+## 3. 全部命令
+
+| 命令 | 作用 | 主要参数 |
+|---|---|---|
+| `python main.py collect` | 专家驾驶采集 (image, lidar, state, action) | `--laps --scenes --seed --out` |
+| `python main.py train` | 行为克隆;加 `--dagger` 启用 DAgger | `--model cnn/mlp --epochs --dagger-iters` |
+| `python main.py eval` | 闭环评测,可录制 GIF | `--model --scene --laps --record --json` |
+| `python main.py benchmark` | 结构/算法对比实验,生成表格与图表 | `--epochs --laps --scenes` |
+| `python main.py demo` | 录制带 HUD 与第一视角小窗的演示 GIF | `--model --seconds --fps` |
+| `python main.py slam` | 建图 + 扫描匹配,输出 ATE 与地图 GIF | `--laps --every --json` |
+| `python main.py teleop` | 键盘遥控(MuJoCo 原生窗口) | `--scene` |
+
+## 4. 脚本入口约定
+
+作业要求"每个模块必须支持 launch 启动,入口以 main 开头"。本仓库提供:
+
+| 文件 | 平台 | 用法 |
+|---|---|---|
+| `main.py` | 全平台 | `python main.py ` |
+| `main.bat` | Windows | `main.bat ` |
+| `main.sh` | Ubuntu | `./main.sh ` |
+
+ROS 侧则通过 `roslaunch car_nn_sim car_nn.launch` / `ros2 launch car_nn_sim car_nn.launch.py`
+启动,详见 [ROS 部署](ros.md)。
+
+## 5. 目录约定
+
+```text
+data/ 采集得到的 npz 数据集(自动生成,约 4.6 MB / 4000 帧)
+runs/ 模型权重与训练历史(自动生成)
+docs/ mkdocs 文档、figures/ 实验结果图、gifs/ 演示动图
+site/ mkdocs build 输出(GitHub Pages 发布目录)
+```
+
+数据与权重体积较大,已加入 `.gitignore`;任何一台机器都可以用上面三条命令重新生成,
+保证仓库里只保存文本与少量图片。
+
+## 6. 常见问题
+
+??? question "`mujoco` 报 `GLFW error` / 窗口无法创建"
+ Windows 上确认没有在无桌面会话(如 ssh)中运行;Ubuntu 服务器设置
+ `MUJOCO_GL=egl`。本项目所有评测与录制都走离屏渲染,不需要打开窗口。
+
+??? question "训练很慢"
+ CPU 上 4000 帧、20 epoch 约 40 s。若安装了 CUDA 版 torch,把 `--device cuda`
+ 传入 `TrainConfig` 即可,代码默认 `cpu` 以保证任何机器可复现。
+
+??? question "换了赛道会不会失效"
+ 不会。`track.py` 中赛道由参数方程生成,改 `R0/AMP2/AMP3` 即可换形状;
+ 相机、雷达、专家、网络输入均自动跟随,重新 `collect` 一次即可。
diff --git a/docs/mujoco_car_nn/report.docx b/docs/mujoco_car_nn/report.docx
new file mode 100644
index 0000000000..c900e68a59
Binary files /dev/null and b/docs/mujoco_car_nn/report.docx differ
diff --git a/docs/mujoco_car_nn/ros.md b/docs/mujoco_car_nn/ros.md
new file mode 100644
index 0000000000..7b701f021e
--- /dev/null
+++ b/docs/mujoco_car_nn/ros.md
@@ -0,0 +1,154 @@
+# ROS 部署
+
+同一套 MuJoCo 仿真与策略推理被封装为 ROS 1(Noetic)和 ROS 2(Humble)两个包,
+满足"所有程序必须支持 launch 启动,且在 Ubuntu 20.04 ROS Noetic / Humble 上运行通过"的要求。
+
+## 1. 工作空间布局
+
+```text
+catkin_ws/src/
+├── car_nn_sim/ # 本仓库 ros1/car_nn_sim
+└── ROS/ # 本仓库根目录(提供 mujoco_car_nn Python 包)
+```
+
+节点通过相对路径 `../../../src` 把仓库的 `src/` 加入 `sys.path`,因此**无需 pip install**,
+克隆即可用:
+
+```python
+_REPO_SRC = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "..", "src"))
+sys.path.insert(0, _REPO_SRC)
+```
+
+若把仓库放在别处,设置环境变量 `PYTHONPATH=/path/to/ROS/src:$PYTHONPATH` 亦可。
+
+## 2. ROS 1(Noetic)
+
+### 2.1 依赖
+
+```bash
+sudo apt install ros-noetic-teleop-twist-keyboard ros-noetic-tf2-ros
+pip3 install mujoco torch numpy pillow
+```
+
+### 2.2 编译
+
+```bash
+cd ~/catkin_ws/src
+ln -s /path/to/ROS ros_car_nn
+cd ~/catkin_ws
+catkin_make
+source devel/setup.bash
+```
+
+### 2.3 启动
+
+| 命令 | 功能 |
+|---|---|
+| `roslaunch car_nn_sim car_teleop.launch` | 键盘遥控(W/A/S/D) |
+| `roslaunch car_nn_sim car_nn.launch policy:=/abs/path/cnn_dagger.pt` | 神经网络自动驾驶 |
+| `roslaunch car_nn_sim car_slam.launch` | 建图 + 定位(发布 `/map` 与 TF) |
+
+参数:
+
+| 参数 | 默认 | 说明 |
+|---|---|---|
+| `~scene` | `day` | `day` / `dusk` / `fog` |
+| `~policy` | 空 | 权重路径;为空则等待 `/car/cmd_vel` |
+| `~rate` | 20.0 | 控制频率 Hz |
+| `~offset` | 0.0 | 初始横向偏移(m) |
+
+### 2.4 话题
+
+| 话题 | 类型 | 方向 | 说明 |
+|---|---|---|---|
+| `/car/camera/image_raw` | `sensor_msgs/Image` | 发布 | 64×64 rgb8 第一视角 |
+| `/car/scan` | `sensor_msgs/LaserScan` | 发布 | 360°,64 束,量程 20 m |
+| `/car/odom` | `nav_msgs/Odometry` | 发布 | 真值位姿 + 速度 |
+| `/car/cmd_vel` | `geometry_msgs/Twist` | 订阅 | `linear.x` m/s → 油门,`angular.z` → 转向 |
+| `/map` | `nav_msgs/OccupancyGrid` | 发布 | 占据栅格(latched) |
+| TF `map→base_link` | `tf2` | 发布 | SLAM 估计位姿 |
+
+## 3. ROS 2(Humble)
+
+```bash
+mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
+ln -s /path/to/ROS/ros2/car_nn_sim car_nn_sim
+cd ~/ros2_ws
+colcon build --packages-select car_nn_sim
+source install/setup.bash
+```
+
+启动:
+
+```bash
+ros2 launch car_nn_sim car_teleop.launch.py
+ros2 launch car_nn_sim car_nn.launch.py policy:=/abs/path/cnn_dagger.pt scene:=dusk
+ros2 launch car_nn_sim car_slam.launch.py
+```
+
+ROS 2 节点用 `create_timer(1/rate)` 驱动仿真,话题名与 ROS 1 完全一致,便于对比。
+
+### 3.1 Docker(免安装 ROS)
+
+```bash
+docker build -t car_nn .
+docker run --rm -it car_nn
+# 容器内:
+source /opt/ros/humble/setup.bash && source /workspace/ros2_ws/install/setup.bash
+ros2 launch car_nn_sim car_teleop.launch.py
+```
+
+镜像基于 `ros:humble-ros-base`,安装 CPU 版 `torch`/`mujoco` 并设置 `MUJOCO_GL=egl`,
+无显示器也能离屏渲染;`colcon build` 已在构建阶段完成。
+
+## 4. 在 RViz 中观察
+
+1. `Fixed Frame` 设为 `map`;
+2. 添加 `LaserScan`(topic `/car/scan`)、`Image`(`/car/camera/image_raw`)、
+ `Map`(`/map`)、`Odometry`(`/car/odom`);
+3. 建图时可以看到红色雷达点沿护栏铺开、灰色未知区域逐渐被白色自由空间填满。
+
+## 5. 无 ROS 的等价运行
+
+课程检查时如果没有 ROS 环境,直接用 `main.py` 即可得到同样的功能与指标:
+
+```bash
+python main.py eval --model runs/cnn_dagger.pt --laps 2 --record
+python main.py slam --laps 1
+```
+
+ROS 节点的**运行时冒烟测试**(不需要安装 ROS):
+
+```bash
+python -m pytest tests/test_ros_nodes.py -q
+```
+
+该测试用 `rospy`/`rclpy` 桩模块真实实例化节点、跑 3 个控制周期,校验
+图像字节数、雷达 64 束、`odom` frame 以及 `cmd_vel` 能驱动车辆加速,
+可以在没有 ROS 的 Windows 上提前发现消息字段错误。
+
+## 6. 文档构建
+
+文档使用课程提供的 **hutb-doc**(OpenHUTB 维护的 mkdocs 分支)构建,已实测兼容:
+
+```bash
+pip install hutb-doc mkdocs-material pymdown-extensions
+mkdocs build --strict # 输出 site/,可直接发布 GitHub Pages
+```
+
+`docs/report.docx` 由 `scripts/build_report.py` 调用 pandoc 生成(目录、章节编号、
+公式、表格、图片齐全),作用等价于 Markdown2Anything 的 Markdown → Word 导出。
+
+## 7. 常见问题
+
+??? question "`ImportError: No module named mujoco_car_nn`"
+ 确认 `catkin_ws/src/ROS` 软链接存在;节点是按 `ros1/car_nn_sim/scripts/../../../src`
+ 推算仓库位置的。若目录结构不同,设置 `PYTHONPATH`。
+
+??? question "虚拟机里 MuJoCo 报 GL 错误"
+ 课程虚拟机是 Ubuntu 20.04,装有 Mesa。若离屏渲染失败,安装 `libosmesa6` 并
+ `export MUJOCO_GL=osmesa`;有桌面会话时默认 GLFW 即可。
+
+??? question "launch 后车子不动"
+ `car_teleop.launch` 需要把焦点放在 teleop 终端上按键;`car_nn.launch` 需要
+ `policy` 参数指向存在的 `.pt` 文件,否则节点只发布传感器、等待 `cmd_vel`。
diff --git a/docs/mujoco_car_nn/simulation.md b/docs/mujoco_car_nn/simulation.md
new file mode 100644
index 0000000000..523cce192c
--- /dev/null
+++ b/docs/mujoco_car_nn/simulation.md
@@ -0,0 +1,217 @@
+# 仿真环境
+
+仿真在 **MuJoCo 4.0** 中离屏运行,不依赖 Gazebo / Carla / GPU,Windows 与 Ubuntu 均可直接跑通。
+整个场景在启动时由 Python 动态生成 XML(约 275 个几何体),因此修改赛道、光照、车辆参数
+都只需要改一处常量。
+
+## 1. 场景组成
+
+| 元素 | 实现 | 说明 |
+|---|---|---|
+| 地面 | `plane` + 程序化棋盘纹理 | 240 m × 240 m |
+| 赛道 | `mesh` 四边形条带(沿中心线扫掠) | 宽 9 m,长 227 m |
+| 车道线 | 白色 `mesh` 边线 + 虚线中心线 | 与中心线同源,绝不"错位" |
+| 护栏 | 约 230 个 `box`,高 0.7 m | 真实碰撞 + 激光雷达反射面 |
+| 车辆 | 平面刚体(2 滑块 + 1 铰链)+ 4 轮几何 | 1500 kg |
+| 光照 | 平行光 + 天空盒 + 雾 | day / dusk / fog 三套参数 |
+
+
+
+*图 1:赛道俯视图(MuJoCo 离屏渲染)。红色为护栏,白色为车道线,灰色为可行驶区域。*
+
+## 2. 赛道几何
+
+中心线采用极坐标下的光滑闭合曲线,保证曲率连续、无自交、最小转弯半径可解析控制:
+
+\[
+r(\theta) = R_0 + A\cos 2\theta + B\sin 3\theta,\qquad \theta\in[0,2\pi)
+\]
+
+\[
+x(\theta)=r\cos\theta,\qquad y(\theta)=r\sin\theta
+\]
+
+取 \(R_0=34,\ A=7,\ B=3.5\),得到周长 **227.1 m**、最小曲率半径 **15.5 m** 的赛道;
+道路半宽 \(W/2=4.5\) m,因此弯道内侧仍有 11 m 的余量,车辆不会"转不过弯"。
+
+曲率由离散转角估计并做 5 点滑动平均:
+
+\[
+\kappa_i=\frac{\Delta\theta_i}{\Delta s_i},\qquad
+\Delta\theta_i=\operatorname{atan2}\big(t_i\times t_{i+1},\ t_i\cdot t_{i+1}\big)
+\]
+
+其中 \(t_i\) 为第 \(i\) 个采样点的单位切向量,\(\Delta s_i\) 为弧长增量。
+中心线按 0.25 m 等弧长重采样(908 个点),左右法向偏移即可生成路面、边线与护栏。
+
+!!! note "为什么不用贴图路面"
+ 贴图方案需要手工对齐纹理坐标,一旦赛道改动就会出现"车在草地上跑"的问题。
+ 本项目把路面、边线、护栏全部由同一条中心线**扫掠**生成,几何天然一致。
+
+## 3. 车辆动力学
+
+车辆被约束在水平面内运动:两个移动关节 \((x,y)\) 加一个绕 \(z\) 的铰链 \(\psi\),
+刚体只保留 3 个自由度,但**碰撞、接触力、质量、转动惯量仍由 MuJoCo 求解**。
+这样可以避免四轮悬挂带来的数值刚性,同时保留"撞墙会减速并被弹开"的真实行为。
+
+状态量 \(\mathbf{s}=(x,y,\psi,v_x,v_y,\omega)\),其中 \(v_x,v_y\) 为车体系速度,\(\omega=\dot\psi\)。
+
+**转向与横摆**(自行车模型 + 横摆角速度伺服):
+
+\[
+\delta = \delta_{\max}\,u_{\text{steer}},\qquad
+\omega_{\text{des}}=\frac{v_x\tan\delta}{L}
+\]
+
+\[
+|\omega_{\text{des}}|\le \frac{a_{\text{lat,max}}}{\max(|v_x|,0.6)},\qquad
+\tau_z = \operatorname{clip}\Big(I_z K_\omega(\omega_{\text{des}}-\omega),\ \pm\tau_{\max}\Big)
+\]
+
+其中 \(L=2.7\) m 为轴距,\(\delta_{\max}=0.55\) rad,\(a_{\text{lat,max}}=7\ \text{m/s}^2\),
+\(I_z=\frac{1}{12}m(L^2+W^2)\approx 2600\ \text{kg·m}^2\),\(K_\omega=12\)。
+
+**纵向**:
+
+\[
+F_x = m\,a_{\text{cmd}} - C_d\,v_x|v_x| - f_r\,m g\,\operatorname{sgn}(v_x)
+\]
+
+\[
+a_{\text{cmd}}=
+\begin{cases}
+u_{\text{thr}}\,a_{\max}, & u_{\text{thr}}\ge 0\\
+u_{\text{thr}}\,a_{\text{brake}}, & u_{\text{thr}}<0
+\end{cases}
+\]
+
+并限制 \(v_x\le v_{\max}=16\) m/s、刹车不产生倒车(\(v_x\le0.05\) 时制动力置零)。
+
+**横向**:用轮胎侧向阻尼近似侧偏力,并饱和到附着极限:
+
+\[
+F_y=\operatorname{clip}\big(-c_y m v_y,\ \pm \mu m g\big),\qquad c_y=9,\ \mu=1.0
+\]
+
+最后把车体系力变换到世界系,写入广义力:
+
+\[
+\begin{bmatrix}F_x^{w}\\F_y^{w}\end{bmatrix}
+=\begin{bmatrix}\cos\psi&-\sin\psi\\ \sin\psi&\cos\psi\end{bmatrix}
+\begin{bmatrix}F_x\\F_y\end{bmatrix},\qquad
+\mathbf{q}_{\text{frc}}=[F_x^w,\ F_y^w,\ \tau_z]^{\top}
+\]
+
+MuJoCo 以 100 Hz 积分(`timestep=0.01`,每控制步 5 个子步),策略以 20 Hz 输出动作。
+
+
+
+*图 2:第三人称跟车视角。车辆、护栏、车道线、路面均由参数化几何生成。*
+
+## 4. 传感器建模
+
+### 4.1 车载相机
+
+相机安装在车头保险杠上方 \((1.45,\,0,\,0.85)\) m(相对车体中心),垂直视场角 \(70^\circ\),
+略微下俯 \(6^\circ\),输出 **64×64 RGB** 供网络使用;同时可渲染 320×180 用于演示。
+
+相机朝向用四元数精确给出:MuJoCo 相机沿自身 \(-z\) 方向观察、\(+y\) 为上,
+因此车体系下相机坐标轴为
+
+\[
+R_c=\begin{bmatrix}
+0&0&-1\\ -1&0&0\\ 0&1&0
+\end{bmatrix}
+\]
+
+即"右 = 车身 \(-y\),上 = 车身 \(+z\),后 = 车身 \(-x\)"。
+
+
+
+*图 3:64×64 网络输入的放大显示(最近邻插值)。*
+
+### 4.2 激光雷达
+
+64 束、水平 360°、量程 20 m,安装高度 0.6 m(低于 0.7 m 护栏,保证能打到)。
+每一束调用 MuJoCo 的射线求交:
+
+```python
+dist = mujoco.mj_ray(model, data, origin, direction,
+ geomgroup=[0, 1, 0, 0, 0, 0], # 只与护栏求交
+ flg_static=True, bodyexclude=-1, geomid=geomid)
+```
+
+单帧 64 束耗时约 **0.24 ms**,完全满足 20 Hz 实时。雷达数据用于
+[建图与 SLAM](slam.md)。
+
+### 4.3 里程计
+
+仿真器给出真值速度 \(v_x\) 与横摆角速度 \(\omega\),SLAM 节点对其乘以
+\((1+\mathcal{N}(0,\sigma^2))\) 的乘性噪声(默认 \(\sigma=1.5\%\))模拟轮速计/IMU,
+用于验证扫描匹配能否抑制里程计漂移。
+
+## 5. 多场景光照
+
+通过修改平行光的 `diffuse/ambient` 与雾浓度实现三种场景,用于检验网络对光照的鲁棒性:
+
+| 场景 | 光照颜色 | 环境光 | 雾浓度 | 用途 |
+|---|---|---|---|---|
+| day | (1.00, 0.98, 0.92) | 0.45 | 0.05 | 训练 / 主实验 |
+| dusk | (1.00, 0.72, 0.45) | 0.30 | 0.18 | 训练 / 泛化测试 |
+| fog | (0.85, 0.88, 0.92) | 0.50 | 0.45 | **零样本**测试 |
+
+
+
+*图 4:day / dusk / fog 三种光照下的同一路段。*
+
+## 6. 仿真操作步骤
+
+### 6.1 Windows 原生(推荐用于开发)
+
+```powershell
+cd ROS
+python main.py demo --model runs/cnn_dagger.pt --seconds 14 --fps 10
+```
+
+执行后终端会打印 GIF 路径;用浏览器或图片查看器打开即可看到
+"第三人称 + 第一视角小窗 + HUD"的演示。若想自己开:
+
+```powershell
+python main.py teleop --scene day
+```
+
+会弹出 MuJoCo 原生窗口,按 `W/S` 加速/刹车、`A/D` 转向、空格停车、`Q` 退出。
+
+### 6.2 Ubuntu 20.04 + ROS Noetic
+
+```bash
+cd ~/catkin_ws/src && ln -s /path/to/ROS ros_car_nn
+cd ~/catkin_ws && catkin_make && source devel/setup.bash
+roslaunch car_nn_sim car_teleop.launch # 键盘遥控
+roslaunch car_nn_sim car_nn.launch policy:=/path/to/runs/cnn_dagger.pt
+roslaunch car_nn_sim car_slam.launch # 建图
+```
+
+详细话题、TF 与 RViz 配置见 [ROS 部署](ros.md)。
+
+### 6.3 运行截图说明
+
+| 步骤 | 观察到的现象 |
+|---|---|
+| 启动 `demo` | 车辆从起点出发,HUD 显示 speed / steer / lateral |
+| 进入弯道 | 网络提前打方向,车辆保持在车道中心 ±0.5 m |
+| 撞墙测试 | 车辆被护栏弹回、速度骤降,碰撞计数 +1(评测中统计) |
+| `slam` | 右侧地图随行驶逐渐展开,红色点为当前帧雷达点 |
+
+## 7. 性能
+
+| 项目 | 数值 | 测试环境 |
+|---|---|---|
+| 物理步进 | 100 Hz(5 子步 × 20 Hz 控制) | i7 / Ryzen 笔记本 CPU |
+| 64×64 相机渲染 | 0.7 ms / 帧 | 同上 |
+| 64 束雷达 | 0.24 ms / 帧 | 同上 |
+| 单圈仿真(227 m) | 22 s 仿真时间 ≈ 1.2 s 墙钟 | 同上 |
+| 端到端推理 | 1.5 ms / 帧(CPU,单线程) | torch 2.x |
+
+仿真速度约为实时的 **15–20 倍**,因此一次完整的 20 epoch 训练 + 12 次闭环评测可在
+10 分钟内完成。
diff --git a/docs/mujoco_car_nn/slam.md b/docs/mujoco_car_nn/slam.md
new file mode 100644
index 0000000000..0684189de6
--- /dev/null
+++ b/docs/mujoco_car_nn/slam.md
@@ -0,0 +1,132 @@
+# 建图与 SLAM
+
+作业任务(3)要求在虚拟环境中完成建图与导航,并实现"导航 + SLAM"同步仿真。
+本项目在 MuJoCo 中一边驾驶一边用 64 束激光雷达构建 **2-D 占据栅格地图**,
+同时用**相关法扫描匹配**抑制里程计漂移,并给出定量评价。
+
+
+
+
+
+## 1. 系统结构
+
+```text
+ ┌────────────┐ /car/scan (64 束) ┌──────────────┐
+ │ MuJoCo │ ────────────────────► │ 栅格建图 │──► /map (OccupancyGrid)
+ │ 仿真节点 │ │ + 扫描匹配 │
+ │ │ ────────────────────► │ │──► TF map→base_link
+ └────────────┘ /car/odom (v, ω) └──────────────┘
+```
+
+无 ROS 时同样的算法由 `python main.py slam` 驱动,输出动图与定量指标。
+
+## 2. 占据栅格地图
+
+地图为 \(0.25\ \text{m}\) 分辨率的栅格(440×360,覆盖 110 m × 90 m),每个单元保存
+**对数几率** \(l_{t}(m)\):
+
+\[
+l_t(m)=\log\frac{p(m\mid z_{1:t})}{1-p(m\mid z_{1:t})},\qquad
+l_t(m)=l_{t-1}(m)+\underbrace{\log\frac{p(m\mid z_t)}{1-p(m\mid z_t)}}_{\text{逆观测模型}}
+\]
+
+实现中命中取 \(+0.85\)、穿过取 \(-0.35\),并截断到 \([-6,6]\) 防止过度自信:
+
+\[
+l_t(m)\leftarrow\operatorname{clip}\big(l_{t-1}(m)+\Delta l,\ -6,\ 6\big)
+\]
+
+概率还原:\(p(m)=\sigma(l)=\dfrac{1}{1+e^{-l}}\)。
+
+### 2.1 射线投射
+
+对第 \(k\) 束雷达(量程 \(r_k\)、角度 \(\theta_k\)),沿射线均匀取 12 个点标记为**自由**,
+在端点处标记为**占据**(3×3 膨胀,抵消离散化误差):
+
+\[
+\mathbf{p}_{j}=\mathbf{o}+r_k\,\eta_j
+\begin{bmatrix}\cos(\psi+\theta_k)\\ \sin(\psi+\theta_k)\end{bmatrix},
+\qquad \eta_j\in[0.1,0.9]
+\]
+
+代码用 `np.add.at` 做散射累加,一帧 64 束的更新耗时 < 1 ms。
+
+## 3. 扫描匹配定位
+
+里程计只用 \((v,\omega)\) 积分,误差随时间累积。扫描匹配在每个控制周期把当前帧点云
+与已有地图做**相关对齐**,搜索使地图占据概率最大的位姿:
+
+\[
+\hat{\mathbf{x}}=\arg\max_{\mathbf{x}\in\mathcal{S}(\mathbf{x}_{\text{prior}})}
+\frac{1}{|\mathcal{P}|}\sum_{\mathbf{p}\in\mathcal{P}} p\big(m(\mathbf{x}\oplus\mathbf{p})\big)
+\]
+
+其中 \(\mathcal{S}\) 为以里程计预测 \(\mathbf{x}_{\text{prior}}\) 为中心的搜索窗
+(\(\pm0.35\ \text{m}\)、\(\pm0.06\ \text{rad}\)),\(\mathcal{P}\) 为当前帧 64 个端点,
+\(m(\cdot)\) 取栅格占据概率。采用"粗→精"两级搜索(5×5×3 → 3×3×3,共 111 个候选),
+单帧约 1.5 ms。
+
+未知栅格取 \(p=0.5\),对得分为零贡献,因此位姿不会因为"指向空白区域"而获得虚假高分。
+
+### 3.1 为什么只修正横向与航向
+
+单帧点云对**沿道路方向**的位移几乎不敏感(两侧护栏近似平移不变),相关峰沿切向非常平坦。
+若把匹配结果全量用于位置更新,沿切向的微弱偏置会逐步累积,实验中发现 200 步后
+ATE 可达 15 m 以上。
+
+因此实现中采用**分量投影**:只取匹配位移在车辆左法向上的分量作为横向修正,
+纵向仍信任轮速里程计:
+
+\[
+\Delta s=\big(\hat{\mathbf{p}}-\mathbf{p}_{\text{prior}}\big)\cdot
+\mathbf{n},\qquad \mathbf{n}=[-\sin\psi,\ \cos\psi]^{\top}
+\]
+
+\[
+\mathbf{p}\leftarrow\mathbf{p}_{\text{prior}}+\alpha\,\Delta s\,\mathbf{n},\qquad \alpha=0.7
+\]
+
+这一取舍在实验中把 ATE 从 15 m 降到 **2.4 m**(单圈),并把地图 IoU 从 0.05 提升到 0.13。
+
+## 4. 评价指标
+
+**绝对轨迹误差(ATE)**:
+
+\[
+\text{ATE}=\sqrt{\frac{1}{T}\sum_{t=1}^{T}\big\|\hat{\mathbf{p}}_t-\mathbf{p}_t\big\|_2^2}
+\]
+
+由于地图与真值同坐标系、起点重合,无需做 Umeyama 对齐。
+
+**地图 IoU**:把护栏矩形按 0.25 m 栅格光栅化为真值占据集 \(G\),估计占据集
+\(P=\{m: l(m)>1\}\),则
+
+\[
+\text{IoU}=\frac{|G\cap P|}{|G\cup P|}
+\]
+
+## 5. 实验结果
+
+单圈(227 m)、里程计噪声 1.5%:
+
+| 指标 | 纯里程计 | 里程计 + 扫描匹配 |
+|---|---|---|
+| ATE RMSE (m) | 2.98 | **2.37** |
+| ATE 最大 (m) | 4.37 | 3.36 |
+| 地图 IoU | 0.09 | **0.13** |
+| 占据栅格数 | 5.2 k | 5.5 k |
+
+
+
+*图 1:单圈建图结果。白色为自由空间,黑色为护栏(占据),灰色为未知区域,青线为估计轨迹。*
+
+!!! note "误差来源分析"
+ 剩余 2.4 m 误差主要来自**沿赛道方向**的累计漂移:轮速计存在 1.5% 的乘性误差,
+ 而扫描匹配按设计不修正切向位置。若要进一步降低,可引入回环检测(如 Scan Context)
+ 或图优化(g2o / GTSAM),这也是本项目的后续工作。
+
+## 6. 与导航的结合
+
+建图结果可以直接作为导航代价地图:ROS 节点把 `/map` 发布为标准 `nav_msgs/OccupancyGrid`,
+`map_server` / `costmap_2d` / `Nav2` 可直接订阅。端到端策略本身完成"感知—规划—控制",
+地图则用于监控、回放与后续全局路径规划,二者在同一仿真时基下同步运行。
diff --git a/main.py b/main.py
new file mode 100644
index 0000000000..f55819bb81
--- /dev/null
+++ b/main.py
@@ -0,0 +1,14 @@
+"""Entry point: ``python main.py ``.
+
+Commands: collect / train / eval / benchmark / demo / slam / teleop.
+"""
+
+import os
+import sys
+
+sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), "src"))
+
+from mujoco_car_nn.cli import main # noqa: E402
+
+if __name__ == "__main__":
+ main()
diff --git a/ros1/car_nn_sim/CMakeLists.txt b/ros1/car_nn_sim/CMakeLists.txt
new file mode 100644
index 0000000000..dfd7093bca
--- /dev/null
+++ b/ros1/car_nn_sim/CMakeLists.txt
@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(car_nn_sim)
+
+find_package(catkin REQUIRED)
+
+catkin_python_setup()
+
+catkin_package()
+
+catkin_install_python(PROGRAMS
+ scripts/sim_node.py
+ scripts/slam_node.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/ros1/car_nn_sim/launch/car_nn.launch b/ros1/car_nn_sim/launch/car_nn.launch
new file mode 100644
index 0000000000..ed0d9e8b92
--- /dev/null
+++ b/ros1/car_nn_sim/launch/car_nn.launch
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/ros1/car_nn_sim/launch/car_slam.launch b/ros1/car_nn_sim/launch/car_slam.launch
new file mode 100644
index 0000000000..529ba62f4c
--- /dev/null
+++ b/ros1/car_nn_sim/launch/car_slam.launch
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/ros1/car_nn_sim/launch/car_teleop.launch b/ros1/car_nn_sim/launch/car_teleop.launch
new file mode 100644
index 0000000000..72ec572d03
--- /dev/null
+++ b/ros1/car_nn_sim/launch/car_teleop.launch
@@ -0,0 +1,17 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/ros1/car_nn_sim/package.xml b/ros1/car_nn_sim/package.xml
new file mode 100644
index 0000000000..f8f1dc6e3f
--- /dev/null
+++ b/ros1/car_nn_sim/package.xml
@@ -0,0 +1,22 @@
+
+
+ car_nn_sim
+ 0.1.0
+ MuJoCo car + neural-network driving and 2-D SLAM demo (ROS 1, Ubuntu 20.04 / Noetic).
+ zhoul
+ MIT
+
+ catkin
+
+ rospy
+ std_msgs
+ sensor_msgs
+ nav_msgs
+ geometry_msgs
+ tf2_ros
+ teleop_twist_keyboard
+
+
+ catkin
+
+
diff --git a/ros1/car_nn_sim/scripts/sim_node.py b/ros1/car_nn_sim/scripts/sim_node.py
new file mode 100644
index 0000000000..e2fb7a6ed2
--- /dev/null
+++ b/ros1/car_nn_sim/scripts/sim_node.py
@@ -0,0 +1,130 @@
+#!/usr/bin/env python3
+"""ROS 1 simulation node for the MuJoCo neural-network car.
+
+Publishes ``/car/camera/image_raw``, ``/car/scan`` and ``/car/odom`` and
+consumes ``/car/cmd_vel`` (linear.x in m/s -> throttle, angular.z in
+[-1, 1] -> normalised steering). When the ``~policy`` parameter points at
+a checkpoint the node drives itself with the neural network instead of
+waiting for teleoperation commands.
+"""
+
+import os
+import sys
+
+import numpy as np
+import rospy
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import Image, LaserScan
+
+# Make the repository's python package importable when running from the
+# catkin workspace (repo_root/src/mujoco_car_nn).
+_REPO_SRC = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "..", "src"))
+if _REPO_SRC not in sys.path:
+ sys.path.insert(0, _REPO_SRC)
+
+from mujoco_car_nn.sim import LIDAR_RANGE, CarSim, SimConfig # noqa: E402
+from mujoco_car_nn.track import build_track # noqa: E402
+
+
+def to_image_msg(img: np.ndarray, stamp) -> Image:
+ msg = Image()
+ msg.header.stamp = stamp
+ msg.header.frame_id = "camera_link"
+ msg.height, msg.width = img.shape[:2]
+ msg.encoding = "rgb8"
+ msg.is_bigendian = 0
+ msg.step = msg.width * 3
+ msg.data = img.tobytes()
+ return msg
+
+
+def to_scan_msg(ranges: np.ndarray, stamp) -> LaserScan:
+ msg = LaserScan()
+ msg.header.stamp = stamp
+ msg.header.frame_id = "laser_link"
+ msg.angle_min = -np.pi
+ msg.angle_max = np.pi
+ msg.angle_increment = 2 * np.pi / len(ranges)
+ msg.range_min = 0.05
+ msg.range_max = LIDAR_RANGE
+ msg.ranges = ranges.astype(np.float32).tolist()
+ return msg
+
+
+def to_odom_msg(st, stamp) -> Odometry:
+ msg = Odometry()
+ msg.header.stamp = stamp
+ msg.header.frame_id = "odom"
+ msg.child_frame_id = "base_link"
+ msg.pose.pose.position.x = st.x
+ msg.pose.pose.position.y = st.y
+ msg.pose.pose.orientation.z = float(np.sin(st.yaw / 2))
+ msg.pose.pose.orientation.w = float(np.cos(st.yaw / 2))
+ msg.twist.twist.linear.x = st.speed
+ msg.twist.twist.angular.z = st.yaw_rate
+ return msg
+
+
+class CarSimNode:
+ """Simulation node: sensors out, ``/car/cmd_vel`` or policy in."""
+
+ def __init__(self) -> None:
+ rospy.init_node("car_nn_sim")
+ self.scene = rospy.get_param("~scene", "day")
+ policy_path = rospy.get_param("~policy", "")
+ self.offset = float(rospy.get_param("~offset", 0.0))
+ self.rate = float(rospy.get_param("~rate", 20.0))
+
+ track = build_track()
+ self.sim = CarSim(
+ track, SimConfig(scene=self.scene, seed=int(rospy.get_param("~seed", 0)), randomize_start=False)
+ )
+ self.st = self.sim.reset(self.offset)
+ self.policy = _load_policy(policy_path) if policy_path else None
+ if self.policy is not None:
+ rospy.loginfo("driving with policy %s", policy_path)
+
+ self.pub_img = rospy.Publisher("/car/camera/image_raw", Image, queue_size=1)
+ self.pub_scan = rospy.Publisher("/car/scan", LaserScan, queue_size=1)
+ self.pub_odom = rospy.Publisher("/car/odom", Odometry, queue_size=1)
+ self.cmd = [0.0, 0.0]
+ rospy.Subscriber("/car/cmd_vel", Twist, self.on_cmd, queue_size=1)
+
+ def on_cmd(self, msg: Twist) -> None:
+ self.cmd[0] = float(np.clip(msg.angular.z, -1.0, 1.0))
+ self.cmd[1] = float(np.clip(msg.linear.x / 8.0, -1.0, 1.0))
+
+ def step(self) -> None:
+ stamp = rospy.Time.now()
+ img = self.sim.camera_image()
+ scan = self.sim.lidar()
+ self.pub_img.publish(to_image_msg(img, stamp))
+ self.pub_scan.publish(to_scan_msg(scan, stamp))
+ self.pub_odom.publish(to_odom_msg(self.st, stamp))
+
+ action = self.policy(img, self.st.speed) if self.policy is not None else np.array(self.cmd, dtype=np.float32)
+ self.st = self.sim.step(action)
+
+ def spin(self) -> None:
+ rate = rospy.Rate(self.rate)
+ while not rospy.is_shutdown():
+ self.step()
+ rate.sleep()
+
+
+def _load_policy(path: str):
+ from mujoco_car_nn.cli import load_policy
+
+ return load_policy(path)
+
+
+def main() -> None:
+ CarSimNode().spin()
+
+
+if __name__ == "__main__":
+ try:
+ main()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/ros1/car_nn_sim/scripts/slam_node.py b/ros1/car_nn_sim/scripts/slam_node.py
new file mode 100644
index 0000000000..9e04af23ee
--- /dev/null
+++ b/ros1/car_nn_sim/scripts/slam_node.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+"""ROS 1 SLAM node: lidar + odometry -> occupancy grid + map->base_link TF."""
+
+import os
+import sys
+
+import numpy as np
+import rospy
+from nav_msgs.msg import OccupancyGrid, Odometry
+from sensor_msgs.msg import LaserScan
+from tf2_ros import TransformBroadcaster
+from geometry_msgs.msg import TransformStamped
+
+_REPO_SRC = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "..", "src"))
+if _REPO_SRC not in sys.path:
+ sys.path.insert(0, _REPO_SRC)
+
+from mujoco_car_nn.slam import GridMap, scan_match # noqa: E402
+from mujoco_car_nn.track import build_track # noqa: E402
+
+
+class SlamNode:
+ def __init__(self) -> None:
+ rospy.init_node("car_slam")
+ self.grid = GridMap()
+ self.pose = None
+ self.publish_rate = rospy.get_param("~publish_rate", 1.0)
+ self.pub_map = rospy.Publisher("/map", OccupancyGrid, queue_size=1, latch=True)
+ self.tf = TransformBroadcaster()
+ self.sub_scan = rospy.Subscriber("/car/scan", LaserScan, self.on_scan, queue_size=1)
+ rospy.Timer(rospy.Duration(1.0 / self.publish_rate), self.publish_map)
+
+ def on_scan(self, msg: LaserScan) -> None:
+ ranges = np.asarray(msg.ranges, dtype=np.float32)
+ angles = msg.angle_min + np.arange(len(ranges)) * msg.angle_increment
+ if self.pose is None:
+ self.pose = (0.0, 0.0, 0.0)
+ matched, _ = scan_match(np.column_stack([ranges * np.cos(angles), ranges * np.sin(angles)]), self.grid, self.pose)
+ self.pose = matched
+ self.grid.update(self.pose, ranges, angles)
+ self._broadcast()
+
+ def _broadcast(self) -> None:
+ t = TransformStamped()
+ t.header.stamp = rospy.Time.now()
+ t.header.frame_id = "map"
+ t.child_frame_id = "base_link"
+ t.transform.translation.x = float(self.pose[0])
+ t.transform.translation.y = float(self.pose[1])
+ t.transform.rotation.z = float(np.sin(self.pose[2] / 2))
+ t.transform.rotation.w = float(np.cos(self.pose[2] / 2))
+ self.tf.sendTransform(t)
+
+ def publish_map(self, _event=None) -> None:
+ msg = OccupancyGrid()
+ msg.header.stamp = rospy.Time.now()
+ msg.header.frame_id = "map"
+ msg.info.resolution = self.grid.resolution
+ msg.info.width = self.grid.width
+ msg.info.height = self.grid.height
+ msg.info.origin.position.x = self.grid.x_min
+ msg.info.origin.position.y = self.grid.y_min
+ prob = self.grid.probability()
+ data = np.full(prob.shape, -1, dtype=np.int8)
+ data[prob > 0.6] = 100
+ data[prob < 0.4] = 0
+ msg.data = data.ravel().tolist()
+ self.pub_map.publish(msg)
+
+
+if __name__ == "__main__":
+ try:
+ node = SlamNode()
+ rospy.spin()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/ros1/car_nn_sim/setup.py b/ros1/car_nn_sim/setup.py
new file mode 100644
index 0000000000..971b3a66fd
--- /dev/null
+++ b/ros1/car_nn_sim/setup.py
@@ -0,0 +1,6 @@
+from distutils.core import setup
+
+from catkin_pkg.python_setup import generate_distutils_setup
+
+setup_args = generate_distutils_setup(packages=[], package_dir={"": "src"})
+setup(**setup_args)
diff --git a/ros2/car_nn_sim/car_nn_sim/__init__.py b/ros2/car_nn_sim/car_nn_sim/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/ros2/car_nn_sim/car_nn_sim/sim_node.py b/ros2/car_nn_sim/car_nn_sim/sim_node.py
new file mode 100644
index 0000000000..229e3a4f9a
--- /dev/null
+++ b/ros2/car_nn_sim/car_nn_sim/sim_node.py
@@ -0,0 +1,109 @@
+#!/usr/bin/env python3
+"""ROS 2 (Humble) simulation node for the MuJoCo neural-network car."""
+
+import os
+import sys
+
+import numpy as np
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import Image, LaserScan
+
+_REPO_SRC = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "..", "..", "src"))
+if _REPO_SRC not in sys.path:
+ sys.path.insert(0, _REPO_SRC)
+
+from mujoco_car_nn.sim import LIDAR_RANGE, CarSim, SimConfig # noqa: E402
+from mujoco_car_nn.track import build_track # noqa: E402
+
+
+class CarSimNode(Node):
+ def __init__(self) -> None:
+ super().__init__("car_nn_sim")
+ self.declare_parameter("scene", "day")
+ self.declare_parameter("policy", "")
+ self.declare_parameter("offset", 0.0)
+ self.declare_parameter("rate", 20.0)
+
+ scene = self.get_parameter("scene").value
+ policy_path = self.get_parameter("policy").value
+ rate = float(self.get_parameter("rate").value)
+ offset = float(self.get_parameter("offset").value)
+
+ track = build_track()
+ self.sim = CarSim(track, SimConfig(scene=scene, seed=0, randomize_start=False))
+ self.st = self.sim.reset(offset)
+ self.policy = None
+ if policy_path:
+ from mujoco_car_nn.cli import load_policy
+
+ self.policy = load_policy(policy_path)
+ self.get_logger().info(f"driving with policy: {policy_path}")
+
+ self.pub_img = self.create_publisher(Image, "/car/camera/image_raw", 1)
+ self.pub_scan = self.create_publisher(LaserScan, "/car/scan", 1)
+ self.pub_odom = self.create_publisher(Odometry, "/car/odom", 1)
+ self.create_subscription(Twist, "/car/cmd_vel", self.on_cmd, 1)
+ self.cmd = [0.0, 0.0]
+ self.timer = self.create_timer(1.0 / rate, self.step)
+
+ def on_cmd(self, msg: Twist) -> None:
+ self.cmd[0] = float(np.clip(msg.angular.z, -1.0, 1.0))
+ self.cmd[1] = float(np.clip(msg.linear.x / 8.0, -1.0, 1.0))
+
+ def step(self) -> None:
+ stamp = self.get_clock().now().to_msg()
+ img = self.sim.camera_image()
+ scan = self.sim.lidar()
+
+ msg = Image()
+ msg.header.stamp = stamp
+ msg.header.frame_id = "camera_link"
+ msg.height, msg.width = img.shape[:2]
+ msg.encoding = "rgb8"
+ msg.step = msg.width * 3
+ msg.data = img.tobytes()
+ self.pub_img.publish(msg)
+
+ scan_msg = LaserScan()
+ scan_msg.header.stamp = stamp
+ scan_msg.header.frame_id = "laser_link"
+ scan_msg.angle_min = -np.pi
+ scan_msg.angle_max = np.pi
+ scan_msg.angle_increment = 2 * np.pi / len(scan)
+ scan_msg.range_min = 0.05
+ scan_msg.range_max = LIDAR_RANGE
+ scan_msg.ranges = scan.astype(np.float32).tolist()
+ self.pub_scan.publish(scan_msg)
+
+ odom = Odometry()
+ odom.header.stamp = stamp
+ odom.header.frame_id = "odom"
+ odom.child_frame_id = "base_link"
+ odom.pose.pose.position.x = self.st.x
+ odom.pose.pose.position.y = self.st.y
+ odom.pose.pose.orientation.z = float(np.sin(self.st.yaw / 2))
+ odom.pose.pose.orientation.w = float(np.cos(self.st.yaw / 2))
+ odom.twist.twist.linear.x = self.st.speed
+ odom.twist.twist.angular.z = self.st.yaw_rate
+ self.pub_odom.publish(odom)
+
+ action = self.policy(img, self.st.speed) if self.policy is not None else np.array(self.cmd, dtype=np.float32)
+ self.st = self.sim.step(action)
+
+
+def main(args=None) -> None:
+ rclpy.init(args=args)
+ node = CarSimNode()
+ try:
+ rclpy.spin(node)
+ finally:
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/ros2/car_nn_sim/car_nn_sim/slam_node.py b/ros2/car_nn_sim/car_nn_sim/slam_node.py
new file mode 100644
index 0000000000..47b75d07ac
--- /dev/null
+++ b/ros2/car_nn_sim/car_nn_sim/slam_node.py
@@ -0,0 +1,81 @@
+#!/usr/bin/env python3
+"""ROS 2 SLAM node: lidar + odometry -> occupancy grid + map->base_link TF."""
+
+import os
+import sys
+
+import numpy as np
+
+import rclpy
+from geometry_msgs.msg import TransformStamped
+from nav_msgs.msg import OccupancyGrid
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile
+from sensor_msgs.msg import LaserScan
+from tf2_ros import TransformBroadcaster
+
+_REPO_SRC = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "..", "..", "src"))
+if _REPO_SRC not in sys.path:
+ sys.path.insert(0, _REPO_SRC)
+
+from mujoco_car_nn.slam import GridMap, scan_match # noqa: E402
+
+
+class SlamNode(Node):
+ def __init__(self) -> None:
+ super().__init__("car_slam")
+ self.grid = GridMap()
+ self.pose = (0.0, 0.0, 0.0)
+ self.tf = TransformBroadcaster(self)
+ qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
+ self.pub_map = self.create_publisher(OccupancyGrid, "/map", qos)
+ self.create_subscription(LaserScan, "/car/scan", self.on_scan, 1)
+ self.create_timer(1.0, self.publish_map)
+
+ def on_scan(self, msg: LaserScan) -> None:
+ ranges = np.asarray(msg.ranges, dtype=np.float32)
+ angles = msg.angle_min + np.arange(len(ranges)) * msg.angle_increment
+ local = np.column_stack([ranges * np.cos(angles), ranges * np.sin(angles)])
+ matched, _ = scan_match(local, self.grid, self.pose)
+ self.pose = matched
+ self.grid.update(self.pose, ranges, angles)
+
+ t = TransformStamped()
+ t.header.stamp = self.get_clock().now().to_msg()
+ t.header.frame_id = "map"
+ t.child_frame_id = "base_link"
+ t.transform.translation.x = float(self.pose[0])
+ t.transform.translation.y = float(self.pose[1])
+ t.transform.rotation.z = float(np.sin(self.pose[2] / 2))
+ t.transform.rotation.w = float(np.cos(self.pose[2] / 2))
+ self.tf.sendTransform(t)
+
+ def publish_map(self) -> None:
+ msg = OccupancyGrid()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.header.frame_id = "map"
+ msg.info.resolution = self.grid.resolution
+ msg.info.width = self.grid.width
+ msg.info.height = self.grid.height
+ msg.info.origin.position.x = self.grid.x_min
+ msg.info.origin.position.y = self.grid.y_min
+ prob = self.grid.probability()
+ data = np.full(prob.shape, -1, dtype=np.int8)
+ data[prob > 0.6] = 100
+ data[prob < 0.4] = 0
+ msg.data = data.ravel().tolist()
+ self.pub_map.publish(msg)
+
+
+def main(args=None) -> None:
+ rclpy.init(args=args)
+ node = SlamNode()
+ try:
+ rclpy.spin(node)
+ finally:
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/ros2/car_nn_sim/launch/car_nn.launch.py b/ros2/car_nn_sim/launch/car_nn.launch.py
new file mode 100644
index 0000000000..3f64c99c32
--- /dev/null
+++ b/ros2/car_nn_sim/launch/car_nn.launch.py
@@ -0,0 +1,30 @@
+"""Launch file for neural-network driving (ROS 2 Humble)."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ scene = LaunchConfiguration("scene")
+ policy = LaunchConfiguration("policy")
+ rate = LaunchConfiguration("rate")
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument("scene", default_value="day", description="day | dusk | fog"),
+ DeclareLaunchArgument(
+ "policy",
+ default_value="",
+ description="path to a .pt checkpoint; empty means teleoperation",
+ ),
+ DeclareLaunchArgument("rate", default_value="20.0"),
+ Node(
+ package="car_nn_sim",
+ executable="sim_node",
+ name="car_nn_sim",
+ output="screen",
+ parameters=[{"scene": scene, "policy": policy, "rate": rate}],
+ ),
+ ]
+ )
diff --git a/ros2/car_nn_sim/launch/car_slam.launch.py b/ros2/car_nn_sim/launch/car_slam.launch.py
new file mode 100644
index 0000000000..b77c7dbeb6
--- /dev/null
+++ b/ros2/car_nn_sim/launch/car_slam.launch.py
@@ -0,0 +1,25 @@
+"""Launch file for SLAM mapping (ROS 2 Humble)."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ scene = LaunchConfiguration("scene")
+ policy = LaunchConfiguration("policy")
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument("scene", default_value="day"),
+ DeclareLaunchArgument("policy", default_value=""),
+ Node(
+ package="car_nn_sim",
+ executable="sim_node",
+ name="car_nn_sim",
+ output="screen",
+ parameters=[{"scene": scene, "policy": policy, "rate": 20.0}],
+ ),
+ Node(package="car_nn_sim", executable="slam_node", name="car_slam", output="screen"),
+ ]
+ )
diff --git a/ros2/car_nn_sim/launch/car_teleop.launch.py b/ros2/car_nn_sim/launch/car_teleop.launch.py
new file mode 100644
index 0000000000..06d757b7f5
--- /dev/null
+++ b/ros2/car_nn_sim/launch/car_teleop.launch.py
@@ -0,0 +1,29 @@
+"""Launch file for teleoperation (ROS 2 Humble)."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ scene = LaunchConfiguration("scene")
+ return LaunchDescription(
+ [
+ DeclareLaunchArgument("scene", default_value="day"),
+ Node(
+ package="car_nn_sim",
+ executable="sim_node",
+ name="car_nn_sim",
+ output="screen",
+ parameters=[{"scene": scene, "policy": "", "rate": 20.0}],
+ ),
+ Node(
+ package="teleop_twist_keyboard",
+ executable="teleop_twist_keyboard",
+ name="teleop",
+ output="screen",
+ remappings=[("/cmd_vel", "/car/cmd_vel")],
+ ),
+ ]
+ )
diff --git a/ros2/car_nn_sim/package.xml b/ros2/car_nn_sim/package.xml
new file mode 100644
index 0000000000..2a03c2960a
--- /dev/null
+++ b/ros2/car_nn_sim/package.xml
@@ -0,0 +1,23 @@
+
+
+
+ car_nn_sim
+ 0.1.0
+ MuJoCo car + neural-network driving and 2-D SLAM demo (ROS 2 Humble).
+ zhoul
+ MIT
+
+ rclpy
+ std_msgs
+ sensor_msgs
+ nav_msgs
+ geometry_msgs
+ tf2_ros
+ teleop_twist_keyboard
+ launch
+ launch_ros
+
+
+ ament_python
+
+
diff --git a/ros2/car_nn_sim/resource/car_nn_sim b/ros2/car_nn_sim/resource/car_nn_sim
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/ros2/car_nn_sim/rviz/car.rviz b/ros2/car_nn_sim/rviz/car.rviz
new file mode 100644
index 0000000000..9257f72213
--- /dev/null
+++ b/ros2/car_nn_sim/rviz/car.rviz
@@ -0,0 +1,102 @@
+Panels:
+ - Class: rviz_common/Displays
+ Name: Displays
+ Property Tree Widget:
+ Expanded: ~
+ Splitter Ratio: 0.5
+ Tree Height: 520
+ - Class: rviz_common/Views
+ Name: Views
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Class: rviz_default_plugins/Grid
+ Name: Grid
+ Enabled: true
+ Cell Size: 5
+ Plane Cell Count: 20
+ Color: 160; 160; 160
+ Alpha: 0.4
+ - Class: rviz_default_plugins/TF
+ Name: TF
+ Enabled: true
+ Show Names: false
+ Marker Scale: 2
+ - Class: rviz_default_plugins/Map
+ Name: Map
+ Enabled: true
+ Topic:
+ Value: /map
+ Depth: 1
+ Durability Policy: Transient Local
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Alpha: 1
+ Color Scheme: map
+ - Class: rviz_default_plugins/LaserScan
+ Name: LaserScan
+ Enabled: true
+ Topic:
+ Value: /car/scan
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Best Effort
+ Style: Points
+ Size (m): 0.15
+ Color Transformer: FlatColor
+ Color: 255; 60; 40
+ Alpha: 1
+ - Class: rviz_default_plugins/Odometry
+ Name: Odometry
+ Enabled: true
+ Topic:
+ Value: /car/odom
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Best Effort
+ Keep: 1
+ Shape:
+ Color: 0; 170; 255
+ Shaft Length: 1.5
+ Shaft Radius: 0.1
+ Head Length: 0.4
+ Head Radius: 0.2
+ Covariance:
+ Value: false
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 60
+ Focal Point:
+ X: 20
+ Y: 5
+ Z: 0
+ Name: Current View
+ Pitch: 1.2
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 0.0
+ Saved: ~
+Window Geometry:
+ Height: 860
+ Width: 1200
+ X: 60
+ Y: 40
diff --git a/ros2/car_nn_sim/setup.cfg b/ros2/car_nn_sim/setup.cfg
new file mode 100644
index 0000000000..c0bde8e471
--- /dev/null
+++ b/ros2/car_nn_sim/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/car_nn_sim
+[install]
+install_scripts=$base/lib/car_nn_sim
diff --git a/ros2/car_nn_sim/setup.py b/ros2/car_nn_sim/setup.py
new file mode 100644
index 0000000000..17dfdcb6e1
--- /dev/null
+++ b/ros2/car_nn_sim/setup.py
@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = "car_nn_sim"
+
+setup(
+ name=package_name,
+ version="0.1.0",
+ packages=[package_name],
+ data_files=[
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
+ ("share/" + package_name + "/launch", ["launch/car_nn.launch.py", "launch/car_slam.launch.py", "launch/car_teleop.launch.py"]),
+ ],
+ install_requires=["setuptools"],
+ zip_safe=True,
+ maintainer="zhoul",
+ maintainer_email="zhoul@example.com",
+ description="MuJoCo car + neural network driving demo",
+ license="MIT",
+ entry_points={
+ "console_scripts": [
+ "sim_node = car_nn_sim.sim_node:main",
+ "slam_node = car_nn_sim.slam_node:main",
+ ],
+ },
+)
diff --git a/runs/cnn_dagger.pt b/runs/cnn_dagger.pt
new file mode 100644
index 0000000000..19d76ff801
Binary files /dev/null and b/runs/cnn_dagger.pt differ
diff --git a/src/mujoco_car_nn/README.md b/src/mujoco_car_nn/README.md
new file mode 100644
index 0000000000..267f7bbb78
--- /dev/null
+++ b/src/mujoco_car_nn/README.md
@@ -0,0 +1,61 @@
+# MuJoCo 无人车神经网络驾驶(开源技术与实践)
+
+一套可在 **Windows / Ubuntu 20.04** 上直接跑通的无人车仿真课程作业:MuJoCo 物理仿真、
+64 线激光雷达、端到端卷积网络驾驶(行为克隆 + DAgger)、占据栅格建图与扫描匹配 SLAM,
+并提供 ROS 1(Noetic)与 ROS 2(Humble)两套 launch。
+
+
+
+## 快速开始
+
+```bash
+pip install -r requirements.txt
+
+python main.py collect --laps 5 --scenes day,dusk # 专家采集数据(约 10 s)
+python main.py train --model cnn --epochs 24 --dagger # 训练(约 2 min,CPU)
+python main.py eval --model runs/cnn_dagger.pt --laps 2 --record
+python main.py slam --laps 1 # 建图 + 定位
+```
+
+Windows 可双击 `main.bat`,Ubuntu 用 `./main.sh`。ROS:
+
+```bash
+roslaunch car_nn_sim car_nn.launch policy:=/abs/path/runs/cnn_dagger.pt
+ros2 launch car_nn_sim car_nn.launch.py policy:=/abs/path/runs/cnn_dagger.pt
+```
+
+## 结果
+
+| 策略 | 平均速度 (m/s) | 单圈 (s) | 横向 RMSE (m) | 碰撞 |
+|---|---|---|---|---|
+| 专家 pure-pursuit | 10.41 | 22.1 | 0.49 | 0 |
+| 端到端 CNN(BC) | 8.03 | 28.6 | 0.45 | 0 |
+| 端到端 CNN(BC+DAgger) | **10.29** | **22.4** | 0.47 | 0 |
+| 端到端 MLP(BC) | 6.75 | 34.0 | 0.38 | 0 |
+
+三种光照(day/dusk/fog)下均 0 碰撞、100% 完成;fog 为零样本场景。
+
+## 文档
+
+完整文档(仿真原理、公式、代码解析、实验、ROS 部署)位于 `docs/`,使用 mkdocs 构建:
+
+```bash
+pip install mkdocs mkdocs-material
+mkdocs serve # http://127.0.0.1:8000
+mkdocs build # 生成 site/,可发布 GitHub Pages
+```
+
+## 目录
+
+```text
+main.py # 入口
+src/mujoco_car_nn/ # 仿真、数据、模型、训练、评测、SLAM
+ros1/ ros2/ # ROS 包与 launch
+docs/ # mkdocs 文档与图片/GIF
+scripts/ # 生成图表、消融、演示的辅助脚本
+tests/ # 冒烟测试
+```
+
+## 许可证
+
+MIT
diff --git a/src/mujoco_car_nn/__init__.py b/src/mujoco_car_nn/__init__.py
new file mode 100644
index 0000000000..d991bd9eb9
--- /dev/null
+++ b/src/mujoco_car_nn/__init__.py
@@ -0,0 +1,8 @@
+"""MuJoCo car + neural-network driving module for OpenHUTB.
+
+End-to-end imitation learning, a modular perception/planning/control
+pipeline, 2-D lidar SLAM and ROS 1 / ROS 2 wrappers around a MuJoCo
+racing scene.
+"""
+
+__version__ = "0.1.0"
diff --git a/src/mujoco_car_nn/cli.py b/src/mujoco_car_nn/cli.py
new file mode 100644
index 0000000000..ebbb4b816b
--- /dev/null
+++ b/src/mujoco_car_nn/cli.py
@@ -0,0 +1,478 @@
+"""Command line interface: collect / train / eval / benchmark / demo / slam."""
+
+from __future__ import annotations
+
+import argparse
+import json
+import os
+import sys
+import time
+
+import numpy as np
+
+from .dataset import CollectConfig, collect_dataset, load_dataset, save_dataset
+from .evaluate import EvalResult, rollout
+from .expert import PurePursuitExpert
+from .models import PolicyRunner, build_model
+from .sim import IMAGE_SIZE, CarSim, SimConfig
+from .slam import GridMap, map_iou, run_slam
+from .track import build_track
+from .train import TrainConfig, dagger_train, train_policy
+from .viz import MapView, composite_demo, render_map, save_gif
+
+ROOT = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+DATA_DIR = os.path.join(ROOT, "data")
+RUN_DIR = os.path.join(ROOT, "runs")
+FIG_DIR = os.path.join(ROOT, "docs", "figures")
+GIF_DIR = os.path.join(ROOT, "docs", "gifs")
+
+
+def _ensure(*paths: str) -> None:
+ for p in paths:
+ os.makedirs(p, exist_ok=True)
+
+
+# ----------------------------------------------------------------------
+# data
+# ----------------------------------------------------------------------
+def cmd_collect(args) -> None:
+ track = build_track()
+ scenes = tuple(args.scenes.split(","))
+ cfg = CollectConfig(laps=args.laps, scenes=scenes, seed=args.seed)
+ t0 = time.time()
+
+ def progress(steps, laps):
+ print(f"\r collecting: {steps} steps, {laps:.2f} laps", end="", flush=True)
+
+ data = collect_dataset(track, cfg, progress=progress)
+ print()
+ out = args.out or os.path.join(DATA_DIR, "dataset.npz")
+ save_dataset(out, data)
+ print(f"saved {len(data['images'])} samples ({len(data['scenes'])} frames, scenes={scenes}) -> {out}")
+ print(f"size {os.path.getsize(out) / 1e6:.1f} MB, took {time.time() - t0:.1f} s")
+
+
+# ----------------------------------------------------------------------
+# training
+# ----------------------------------------------------------------------
+def _load_data(path: str) -> dict:
+ if not os.path.exists(path):
+ raise SystemExit(f"dataset not found: {path}. Run `python main.py collect` first.")
+ return load_dataset(path)
+
+
+def cmd_train(args) -> None:
+ track = build_track()
+ data = _load_data(args.data)
+ cfg = TrainConfig(model=args.model, epochs=args.epochs, seed=args.seed)
+ _ensure(RUN_DIR)
+
+ if args.dagger:
+ net, data, logs = dagger_train(track, data, cfg, iterations=args.dagger_iters, verbose=True)
+ suffix = "dagger"
+ else:
+ net, logs = train_policy(data, cfg, verbose=True)
+ suffix = "bc"
+
+ import torch
+
+ out = args.out or os.path.join(RUN_DIR, f"{args.model}_{suffix}.pt")
+ torch.save({"model": args.model, "state_dict": net.state_dict(), "config": vars(cfg)}, out)
+ hist_path = os.path.splitext(out)[0] + "_history.json"
+ with open(hist_path, "w", encoding="utf-8") as fh:
+ json.dump(logs, fh, indent=2, default=float)
+ print(f"model saved to {out}")
+ print(f"history saved to {hist_path}")
+
+
+def load_policy(path: str) -> PolicyRunner:
+ import torch
+
+ ckpt = torch.load(path, map_location="cpu", weights_only=False)
+ net = build_model(ckpt["model"], state_dim=int(ckpt.get("state_dim", 1)))
+ net.load_state_dict(ckpt["state_dict"])
+ return PolicyRunner(net)
+
+
+# ----------------------------------------------------------------------
+# evaluation
+# ----------------------------------------------------------------------
+def _print_result(res: EvalResult) -> None:
+ print(
+ f"{res.policy:14s} {res.scene:5s} laps={res.laps_completed:4.2f} "
+ f"success={'Y' if res.success else 'N'} coll={res.collisions:2d} "
+ f"lat_rmse={res.lateral_rmse:4.2f} lat_max={res.lateral_max:4.2f} "
+ f"v_mean={res.speed_mean:5.2f} lap={res.lap_time:5.1f}s infer={res.inference_ms:4.1f}ms"
+ )
+
+
+def cmd_eval(args) -> None:
+ track = build_track()
+ policy = None if args.expert else load_policy(args.model)
+ name = "expert" if args.expert else os.path.basename(args.model)
+ res, frames = rollout(
+ policy,
+ track,
+ scene=args.scene,
+ laps=args.laps,
+ seed=args.seed,
+ policy_name=name,
+ record=args.record,
+ max_steps=args.max_steps,
+ )
+ _print_result(res)
+ if args.record and frames:
+ _ensure(GIF_DIR)
+ path = args.gif or os.path.join(GIF_DIR, f"eval_{name}_{args.scene}.gif")
+ size = save_gif(frames, path, fps=10, colors=64)
+ print(f"gif saved to {path} ({size / 1e6:.2f} MB, {len(frames)} frames)")
+ if args.json:
+ with open(args.json, "w", encoding="utf-8") as fh:
+ json.dump(res.to_dict(), fh, indent=2)
+
+
+# ----------------------------------------------------------------------
+# benchmark: architecture and algorithm comparison
+# ----------------------------------------------------------------------
+def cmd_benchmark(args) -> None:
+ import torch
+
+ _ensure(FIG_DIR, RUN_DIR, DATA_DIR)
+ track = build_track()
+ data = _load_data(args.data)
+
+ scenarios = {
+ "expert": None,
+ "cnn_bc": None,
+ "cnn_dagger": None,
+ "mlp_bc": None,
+ }
+ histories = {}
+
+ print("\n[1/4] behaviour cloning (CNN)")
+ cnn, hist_cnn = train_policy(data, TrainConfig(model="cnn", epochs=args.epochs), verbose=False)
+ torch.save({"model": "cnn", "state_dict": cnn.state_dict()}, os.path.join(RUN_DIR, "cnn_bc.pt"))
+ histories["cnn_bc"] = hist_cnn
+ scenarios["cnn_bc"] = PolicyRunner(cnn)
+
+ print("[2/4] behaviour cloning (MLP)")
+ mlp, hist_mlp = train_policy(data, TrainConfig(model="mlp", epochs=args.epochs), verbose=False)
+ torch.save({"model": "mlp", "state_dict": mlp.state_dict()}, os.path.join(RUN_DIR, "mlp_bc.pt"))
+ histories["mlp_bc"] = hist_mlp
+ scenarios["mlp_bc"] = PolicyRunner(mlp)
+
+ print("[3/4] DAgger (CNN + expert relabelling)")
+ cnn_d, data_d, logs_d = dagger_train(
+ track, data, TrainConfig(model="cnn", epochs=args.epochs), iterations=args.dagger_iters, verbose=False
+ )
+ torch.save({"model": "cnn", "state_dict": cnn_d.state_dict()}, os.path.join(RUN_DIR, "cnn_dagger.pt"))
+ histories["cnn_dagger"] = logs_d
+ scenarios["cnn_dagger"] = PolicyRunner(cnn_d)
+
+ print("[4/4] closed-loop evaluation")
+ results = []
+ for name, policy in scenarios.items():
+ for scene in args.scenes.split(","):
+ res, _ = rollout(
+ policy,
+ track,
+ scene=scene,
+ laps=args.laps,
+ seed=42,
+ policy_name=name,
+ max_steps=args.max_steps,
+ )
+ _print_result(res)
+ results.append(res.to_dict())
+
+ payload = {"results": results, "histories": histories, "config": {k: v for k, v in vars(args).items() if k != "func"}}
+ with open(os.path.join(FIG_DIR, "results.json"), "w", encoding="utf-8") as fh:
+ json.dump(payload, fh, indent=2, default=float)
+
+ write_markdown_table(results, os.path.join(FIG_DIR, "results_table.md"))
+ plot_histories(histories, os.path.join(FIG_DIR, "training_curves.png"))
+ plot_comparison(results, os.path.join(FIG_DIR, "comparison.png"))
+ print(f"\nsaved tables and figures to {FIG_DIR}")
+
+
+def write_markdown_table(results: list[dict], path: str) -> None:
+ header = (
+ "| 策略 | 场景 | 完成圈数 | 成功率 | 碰撞次数 | 横向误差 RMSE (m) | 横向误差最大 (m) | "
+ "平均速度 (m/s) | 单圈时间 (s) | 推理耗时 (ms) |\n"
+ "|---|---|---|---|---|---|---|---|---|---|\n"
+ )
+ rows = []
+ for r in results:
+ rows.append(
+ f"| {r['policy']} | {r['scene']} | {r['laps_completed']:.2f} | {'是' if r['success'] else '否'} | "
+ f"{r['collisions']} | {r['lateral_rmse']:.2f} | {r['lateral_max']:.2f} | {r['speed_mean']:.2f} | "
+ f"{r['lap_time']:.1f} | {r['inference_ms']:.2f} |"
+ )
+ with open(path, "w", encoding="utf-8") as fh:
+ fh.write(header + "\n".join(rows) + "\n")
+
+
+def plot_histories(histories: dict, path: str) -> None:
+ import matplotlib
+
+ matplotlib.use("Agg")
+ import matplotlib.pyplot as plt
+
+ fig, axes = plt.subplots(1, 2, figsize=(10, 3.6))
+ for name, hist in histories.items():
+ if name == "cnn_dagger":
+ h = hist["dagger"][-1] if isinstance(hist, dict) else None
+ if h is None:
+ continue
+ axes[0].plot(h["train_loss"], label="cnn_dagger(train)")
+ axes[1].plot(h["val_loss"], label="cnn_dagger(val)")
+ else:
+ axes[0].plot(hist["train_loss"], label=f"{name}(train)")
+ axes[1].plot(hist["val_loss"], label=f"{name}(val)")
+ for ax, title in zip(axes, ("training loss", "validation loss")):
+ ax.set_title(title)
+ ax.set_xlabel("epoch")
+ ax.set_yscale("log")
+ ax.grid(alpha=0.3)
+ ax.legend(fontsize=8)
+ fig.tight_layout()
+ fig.savefig(path, dpi=140)
+ plt.close(fig)
+
+
+def plot_comparison(results: list[dict], path: str) -> None:
+ import matplotlib
+
+ matplotlib.use("Agg")
+ import matplotlib.pyplot as plt
+
+ scenes = sorted({r["scene"] for r in results})
+ policies = [p for p in ("expert", "cnn_bc", "cnn_dagger", "mlp_bc") if any(r["policy"] == p for r in results)]
+ colors = {"expert": "#4c4c4c", "cnn_bc": "#1f77b4", "cnn_dagger": "#2ca02c", "mlp_bc": "#d62728"}
+
+ fig, axes = plt.subplots(1, 2, figsize=(11, 3.8))
+ width = 0.8 / max(len(policies), 1)
+ for i, policy in enumerate(policies):
+ vals = [next((r["lateral_rmse"] for r in results if r["policy"] == policy and r["scene"] == s), 0) for s in scenes]
+ axes[0].bar(np.arange(len(scenes)) + i * width, vals, width, label=policy, color=colors.get(policy))
+ vals2 = [next((r["laps_completed"] for r in results if r["policy"] == policy and r["scene"] == s), 0) for s in scenes]
+ axes[1].bar(np.arange(len(scenes)) + i * width, vals2, width, label=policy, color=colors.get(policy))
+ axes[0].set_xticks(np.arange(len(scenes)) + 0.4 - width / 2, scenes)
+ axes[1].set_xticks(np.arange(len(scenes)) + 0.4 - width / 2, scenes)
+ axes[0].set_ylabel("lateral RMSE (m)")
+ axes[1].set_ylabel("laps completed")
+ axes[0].set_title("tracking accuracy")
+ axes[1].set_title("completion")
+ for ax in axes:
+ ax.grid(axis="y", alpha=0.3)
+ ax.legend(fontsize=8)
+ fig.tight_layout()
+ fig.savefig(path, dpi=140)
+ plt.close(fig)
+
+
+# ----------------------------------------------------------------------
+# demo GIF
+# ----------------------------------------------------------------------
+def cmd_demo(args) -> None:
+ track = build_track()
+ policy = None if args.expert else load_policy(args.model)
+ sim = CarSim(track, SimConfig(scene=args.scene, seed=args.seed, randomize_start=False))
+ expert = PurePursuitExpert(track)
+ st = sim.reset(args.offset)
+ _ensure(GIF_DIR)
+
+ frames = []
+ fps = args.fps
+ every = max(1, int(round(20 / fps)))
+ steps = int(args.seconds * 20)
+ for i in range(steps):
+ if policy is None:
+ action = expert.act(st)
+ else:
+ action = policy(sim.camera_image(), st.speed)
+ if i % every == 0:
+ chase = sim.render_chase(640, 360)
+ ego = sim.render_front(320, 180)
+ lines = [
+ f"speed {st.speed:5.2f} m/s",
+ f"steer {action[0]:+.2f} thr {action[1]:+.2f}",
+ f"lateral {st.lateral:+.2f} m",
+ f"scene {args.scene}",
+ ]
+ frames.append(composite_demo(chase, ego, lines))
+ st = sim.step(np.clip(action, -1, 1))
+ path = args.out or os.path.join(GIF_DIR, f"demo_{'expert' if policy is None else 'nn'}_{args.scene}.gif")
+ size = save_gif(frames, path, fps=fps, colors=64)
+ print(f"gif saved to {path} ({size / 1e6:.2f} MB, {len(frames)} frames)")
+
+
+# ----------------------------------------------------------------------
+# SLAM demo
+# ----------------------------------------------------------------------
+def cmd_slam(args) -> None:
+ track = build_track()
+ policy = None if args.expert else load_policy(args.model)
+ grid_holder = {}
+
+ def render(grid, traj, pose, st):
+ view = MapView(resolution=grid.resolution, origin=(grid.x_min, grid.y_min), width=grid.width, height=grid.height, scale=1)
+ img = render_map(
+ grid.log_odds,
+ view,
+ [tuple(p) for p in traj],
+ (pose[0], pose[1], pose[2]),
+ title=f"occupancy grid | steps {len(traj)} | scene {args.scene}",
+ )
+ return img
+
+ metrics, frames, grid = run_slam(
+ track,
+ policy=policy,
+ scene=args.scene,
+ laps=args.laps,
+ max_steps=args.max_steps,
+ record_every=args.every,
+ render=render,
+ )
+ print(
+ f"SLAM: {metrics['steps']} steps, {metrics['laps']:.2f} laps, "
+ f"ATE RMSE {metrics['ate_rmse']:.2f} m, max {metrics['ate_max']:.2f} m, "
+ f"map IoU {map_iou(grid, track):.3f}, free/occupied cells {metrics['map_cells']}"
+ )
+ _ensure(GIF_DIR)
+ if frames:
+ path = args.out or os.path.join(GIF_DIR, "slam_mapping.gif")
+ size = save_gif(frames, path, fps=args.fps, colors=64)
+ print(f"gif saved to {path} ({size / 1e6:.2f} MB, {len(frames)} frames)")
+ if args.json:
+ payload = {k: v for k, v in metrics.items() if k not in ("trajectory", "estimated")}
+ payload["map_iou"] = map_iou(grid, track)
+ with open(args.json, "w", encoding="utf-8") as fh:
+ json.dump(payload, fh, indent=2, default=float)
+
+
+# ----------------------------------------------------------------------
+# keyboard teleoperation
+# ----------------------------------------------------------------------
+def cmd_teleop(args) -> None:
+ import mujoco.viewer
+
+ track = build_track()
+ sim = CarSim(track, SimConfig(scene=args.scene, seed=0, randomize_start=False, allow_reverse=True))
+ sim.reset(0.0)
+ steer, throttle = 0.0, 0.0
+ print("keyboard: A/D steer, W/S throttle, Q quit (focus the viewer window)")
+ with mujoco.viewer.launch_passive(sim.model, sim.data) as viewer:
+ while viewer.is_running():
+ try:
+ import msvcrt
+
+ if msvcrt.kbhit():
+ key = msvcrt.getch().decode(errors="ignore").lower()
+ if key == "a":
+ steer = max(-1.0, steer - 0.15)
+ elif key == "d":
+ steer = min(1.0, steer + 0.15)
+ elif key == "w":
+ throttle = min(1.0, throttle + 0.2)
+ elif key == "s":
+ throttle = max(-1.0, throttle - 0.2)
+ elif key == "q":
+ break
+ elif key == " ":
+ steer, throttle = 0.0, 0.0
+ except ImportError: # Linux / macOS
+ import select
+ import termios
+ import tty
+
+ if select.select([sys.stdin], [], [], 0)[0]:
+ key = sys.stdin.read(1).lower()
+ steer = max(-1.0, steer - 0.15) if key == "a" else min(1.0, steer + 0.15) if key == "d" else steer
+ throttle = min(1.0, throttle + 0.2) if key == "w" else max(-1.0, throttle - 0.2) if key == "s" else throttle
+ if key == "q":
+ break
+ sim.step([steer, throttle], substeps=1)
+ viewer.sync()
+ time.sleep(0.005)
+ sim.close()
+
+
+def build_parser() -> argparse.ArgumentParser:
+ p = argparse.ArgumentParser(prog="mujoco_car_nn", description="MuJoCo car + neural network driving demo")
+ sub = p.add_subparsers(dest="cmd", required=True)
+
+ c = sub.add_parser("collect", help="collect imitation-learning data with the expert")
+ c.add_argument("--laps", type=float, default=5.0)
+ c.add_argument("--scenes", type=str, default="day")
+ c.add_argument("--seed", type=int, default=0)
+ c.add_argument("--out", type=str, default="")
+ c.set_defaults(func=cmd_collect)
+
+ t = sub.add_parser("train", help="behaviour cloning / DAgger")
+ t.add_argument("--model", type=str, default="cnn", choices=["cnn", "mlp"])
+ t.add_argument("--data", type=str, default=os.path.join(DATA_DIR, "dataset.npz"))
+ t.add_argument("--epochs", type=int, default=24)
+ t.add_argument("--seed", type=int, default=0)
+ t.add_argument("--dagger", action="store_true")
+ t.add_argument("--dagger-iters", type=int, default=2)
+ t.add_argument("--out", type=str, default="")
+ t.set_defaults(func=cmd_train)
+
+ e = sub.add_parser("eval", help="closed-loop evaluation")
+ e.add_argument("--model", type=str, default=os.path.join(RUN_DIR, "cnn_dagger.pt"))
+ e.add_argument("--expert", action="store_true")
+ e.add_argument("--scene", type=str, default="day", choices=["day", "dusk", "fog"])
+ e.add_argument("--laps", type=float, default=2.0)
+ e.add_argument("--seed", type=int, default=0)
+ e.add_argument("--max-steps", type=int, default=3000)
+ e.add_argument("--record", action="store_true")
+ e.add_argument("--gif", type=str, default="")
+ e.add_argument("--json", type=str, default="")
+ e.set_defaults(func=cmd_eval)
+
+ b = sub.add_parser("benchmark", help="architecture / algorithm comparison")
+ b.add_argument("--data", type=str, default=os.path.join(DATA_DIR, "dataset.npz"))
+ b.add_argument("--epochs", type=int, default=20)
+ b.add_argument("--dagger-iters", type=int, default=2)
+ b.add_argument("--laps", type=float, default=1.0)
+ b.add_argument("--scenes", type=str, default="day,dusk,fog")
+ b.add_argument("--max-steps", type=int, default=2500)
+ b.set_defaults(func=cmd_benchmark)
+
+ d = sub.add_parser("demo", help="record a driving GIF")
+ d.add_argument("--model", type=str, default=os.path.join(RUN_DIR, "cnn_dagger.pt"))
+ d.add_argument("--expert", action="store_true")
+ d.add_argument("--scene", type=str, default="day")
+ d.add_argument("--seconds", type=float, default=14.0)
+ d.add_argument("--fps", type=int, default=10)
+ d.add_argument("--seed", type=int, default=0)
+ d.add_argument("--offset", type=float, default=0.0)
+ d.add_argument("--out", type=str, default="")
+ d.set_defaults(func=cmd_demo)
+
+ s = sub.add_parser("slam", help="occupancy mapping + scan matching demo")
+ s.add_argument("--model", type=str, default=os.path.join(RUN_DIR, "cnn_dagger.pt"))
+ s.add_argument("--expert", action="store_true")
+ s.add_argument("--scene", type=str, default="day")
+ s.add_argument("--laps", type=float, default=1.0)
+ s.add_argument("--max-steps", type=int, default=2000)
+ s.add_argument("--every", type=int, default=8)
+ s.add_argument("--fps", type=int, default=10)
+ s.add_argument("--out", type=str, default="")
+ s.add_argument("--json", type=str, default="")
+ s.set_defaults(func=cmd_slam)
+
+ k = sub.add_parser("teleop", help="keyboard teleoperation in the MuJoCo viewer")
+ k.add_argument("--scene", type=str, default="day")
+ k.set_defaults(func=cmd_teleop)
+
+ return p
+
+
+def main(argv=None) -> None:
+ parser = build_parser()
+ args = parser.parse_args(argv)
+ _ensure(DATA_DIR, RUN_DIR)
+ args.func(args)
diff --git a/src/mujoco_car_nn/dataset.py b/src/mujoco_car_nn/dataset.py
new file mode 100644
index 0000000000..ad1111a8e3
--- /dev/null
+++ b/src/mujoco_car_nn/dataset.py
@@ -0,0 +1,122 @@
+"""Dataset collection (expert driving) and torch datasets."""
+
+from __future__ import annotations
+
+import os
+from dataclasses import dataclass, field
+
+import numpy as np
+import torch
+from torch.utils.data import Dataset
+
+from .expert import ExpertConfig, PurePursuitExpert
+from .sim import IMAGE_SIZE, CarSim, SimConfig
+
+
+@dataclass
+class CollectConfig:
+ laps: float = 6.0
+ scenes: tuple[str, ...] = ("day",)
+ seed: int = 0
+ control_dt: float = 0.05
+ perturb_prob: float = 0.12
+ perturb_amp: float = 0.18
+ randomize_start: bool = True
+ offset_range: float = 1.2
+ max_steps: int = 6000
+
+
+def collect_dataset(track, config: CollectConfig | None = None, progress=None) -> dict[str, np.ndarray]:
+ """Drive the expert around the track and record (image, lidar, state, action)."""
+ cfg = config or CollectConfig()
+ rng = np.random.default_rng(cfg.seed)
+ images, actions, states, scans, scenes = [], [], [], [], []
+
+ for scene in cfg.scenes:
+ sim = CarSim(
+ track,
+ SimConfig(scene=scene, seed=cfg.seed, randomize_start=cfg.randomize_start, offset_range=cfg.offset_range),
+ )
+ expert = PurePursuitExpert(track)
+ st = sim.reset()
+ prev_s = st.s
+ travelled = 0.0
+ steps = 0
+ while travelled < cfg.laps * track.length and steps < cfg.max_steps:
+ action = expert.act(st)
+ images.append(sim.camera_image())
+ scans.append(sim.lidar())
+ states.append(st.as_vector())
+ actions.append(action)
+ scenes.append(scene)
+
+ executed = action.copy()
+ if rng.random() < cfg.perturb_prob:
+ executed[0] = float(np.clip(executed[0] + rng.normal(0, cfg.perturb_amp), -1, 1))
+ if rng.random() < cfg.perturb_prob * 0.5:
+ executed[1] = float(np.clip(executed[1] + rng.normal(0, 0.25), -1, 1))
+ st = sim.step(executed)
+ travelled += (st.s - prev_s) % track.length
+ prev_s = st.s
+ steps += 1
+ if progress and steps % 200 == 0:
+ progress(steps, travelled / track.length)
+ sim.close()
+
+ return {
+ "images": np.asarray(images, dtype=np.uint8),
+ "actions": np.asarray(actions, dtype=np.float32),
+ "states": np.asarray(states, dtype=np.float32),
+ "scans": np.asarray(scans, dtype=np.float32),
+ "scenes": np.asarray(scenes),
+ }
+
+
+def save_dataset(path: str, data: dict[str, np.ndarray]) -> None:
+ os.makedirs(os.path.dirname(os.path.abspath(path)), exist_ok=True)
+ np.savez_compressed(path, **data)
+
+
+class DrivingDataset(Dataset):
+ """Image + speed + action dataset with light photometric augmentation."""
+
+ def __init__(
+ self,
+ images: np.ndarray,
+ actions: np.ndarray,
+ states: np.ndarray | None = None,
+ augment: bool = False,
+ seed: int = 0,
+ ):
+ self.images = images
+ self.actions = actions.astype(np.float32)
+ self.states = None if states is None else states.astype(np.float32)[:, :1]
+ self.augment = augment
+ self.rng = np.random.default_rng(seed)
+
+ def __len__(self) -> int:
+ return len(self.images)
+
+ def __getitem__(self, idx: int):
+ img = self.images[idx].astype(np.float32)
+ if self.augment:
+ img = img * self.rng.uniform(0.85, 1.15)
+ img = np.clip(img + self.rng.uniform(-12, 12), 0, 255)
+ x = torch.from_numpy(img).permute(2, 0, 1).div_(127.5).sub_(1.0)
+ y = torch.from_numpy(self.actions[idx])
+ if self.states is None:
+ s = torch.zeros(1)
+ else:
+ s = torch.from_numpy(self.states[idx])
+ return x, s, y
+
+
+def load_dataset(path: str) -> dict[str, np.ndarray]:
+ return dict(np.load(path, allow_pickle=True))
+
+
+def split_indices(n: int, val_ratio: float = 0.15, seed: int = 0):
+ rng = np.random.default_rng(seed)
+ idx = rng.permutation(n)
+ n_val = max(1, int(n * val_ratio))
+ return idx[n_val:], idx[:n_val]
diff --git a/src/mujoco_car_nn/evaluate.py b/src/mujoco_car_nn/evaluate.py
new file mode 100644
index 0000000000..31dec1a4c6
--- /dev/null
+++ b/src/mujoco_car_nn/evaluate.py
@@ -0,0 +1,112 @@
+"""Closed-loop evaluation of driving policies."""
+
+from __future__ import annotations
+
+import time
+from dataclasses import asdict, dataclass, field
+from typing import Callable
+
+import numpy as np
+
+from .expert import PurePursuitExpert
+from .sim import CarSim, SimConfig
+
+
+@dataclass
+class EvalResult:
+ policy: str = ""
+ scene: str = "day"
+ laps_target: float = 2.0
+ laps_completed: float = 0.0
+ success: bool = False
+ collisions: int = 0
+ lateral_rmse: float = 0.0
+ lateral_max: float = 0.0
+ speed_mean: float = 0.0
+ speed_max: float = 0.0
+ sim_seconds: float = 0.0
+ lap_time: float = 0.0
+ steps: int = 0
+ inference_ms: float = 0.0
+
+ def to_dict(self) -> dict:
+ return asdict(self)
+
+
+def rollout(
+ policy: Callable[[np.ndarray], np.ndarray] | None,
+ track,
+ scene: str = "day",
+ laps: float = 2.0,
+ max_steps: int = 3000,
+ seed: int = 0,
+ randomize_start: bool = True,
+ record: bool = False,
+ record_every: int = 4,
+ policy_name: str = "policy",
+ on_step: Callable | None = None,
+) -> tuple[EvalResult, list]:
+ """Run ``policy`` in closed loop and compute driving metrics.
+
+ ``policy`` takes the 64x64 ego image and returns ``[steer, throttle]``.
+ When it is ``None`` the pure-pursuit expert drives (used as a reference).
+ """
+ sim = CarSim(track, SimConfig(scene=scene, seed=seed, randomize_start=randomize_start))
+ expert = PurePursuitExpert(track)
+ st = sim.reset()
+
+ prev_s = st.s
+ travelled = 0.0
+ steps = 0
+ collisions = 0
+ contact = False
+ lat, speeds, frames = [], [], []
+ inference = []
+ t0 = time.time()
+
+ while travelled < laps * track.length and steps < max_steps:
+ if policy is None:
+ action = expert.act(st)
+ else:
+ t1 = time.perf_counter()
+ action = policy(sim.camera_image(), st.speed)
+ inference.append((time.perf_counter() - t1) * 1000.0)
+
+ st = sim.step(np.clip(action, -1, 1))
+ travelled += (st.s - prev_s) % track.length
+ prev_s = st.s
+ lat.append(st.lateral)
+ speeds.append(st.speed)
+ steps += 1
+
+ if sim.data.ncon > 0 and not contact:
+ collisions += 1
+ contact = sim.data.ncon > 0
+
+ if record and steps % record_every == 0:
+ frames.append(sim.render_chase(480, 270))
+ if on_step is not None:
+ on_step(sim, st)
+ if st.speed < 0.05 and steps > 200:
+ break
+
+ sim_seconds = steps * 0.05
+ laps_completed = travelled / track.length
+ res = EvalResult(
+ policy=policy_name,
+ scene=scene,
+ laps_target=laps,
+ laps_completed=laps_completed,
+ success=laps_completed >= laps - 0.02 and collisions == 0,
+ collisions=collisions,
+ lateral_rmse=float(np.sqrt(np.mean(np.square(lat)))) if lat else 0.0,
+ lateral_max=float(np.max(np.abs(lat))) if lat else 0.0,
+ speed_mean=float(np.mean(speeds)) if speeds else 0.0,
+ speed_max=float(np.max(speeds)) if speeds else 0.0,
+ sim_seconds=sim_seconds,
+ lap_time=sim_seconds / max(laps_completed, 1e-6),
+ steps=steps,
+ inference_ms=float(np.median(inference)) if inference else 0.0,
+ )
+ sim.close()
+ return res, frames
diff --git a/src/mujoco_car_nn/expert.py b/src/mujoco_car_nn/expert.py
new file mode 100644
index 0000000000..4ec273d739
--- /dev/null
+++ b/src/mujoco_car_nn/expert.py
@@ -0,0 +1,69 @@
+"""Pure-pursuit expert used to generate the imitation-learning dataset.
+
+The expert follows the track centre line with a speed profile derived
+from the upcoming curvature::
+
+ v_target = sqrt(a_lat / |kappa|)
+
+and converts the pure-pursuit curvature into a normalised steering
+command::
+
+ delta = atan(2 * L * sin(alpha) / L_d)
+ steer = delta / delta_max
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+from .sim import MAX_STEER, MAX_SPEED, WHEELBASE, CarState
+from .track import Track, ROAD_HALF_WIDTH
+
+
+@dataclass
+class ExpertConfig:
+ lookahead_base: float = 4.0
+ lookahead_gain: float = 0.55
+ max_lookahead: float = 11.0
+ max_speed: float = 12.0
+ min_speed: float = 4.0
+ lat_accel: float = 5.2
+ speed_kp: float = 0.45
+ speed_ff: float = 0.30
+
+
+class PurePursuitExpert:
+ def __init__(self, track: Track, config: ExpertConfig | None = None):
+ self.track = track
+ self.cfg = config or ExpertConfig()
+
+ def act(self, st: CarState) -> np.ndarray:
+ cfg = self.cfg
+ s = st.s
+ speed = max(st.speed, 0.0)
+ lookahead = float(np.clip(cfg.lookahead_base + cfg.lookahead_gain * speed, 3.0, cfg.max_lookahead))
+
+ target = self.track.point_at(s + lookahead)
+ dx = target[0] - st.x
+ dy = target[1] - st.y
+ c, sn = np.cos(st.yaw), np.sin(st.yaw)
+ x_f = c * dx + sn * dy
+ y_f = -sn * dx + c * dy
+ alpha = float(np.arctan2(y_f, max(x_f, 1e-3)))
+ curvature = 2.0 * np.sin(alpha) / max(lookahead, 1e-3)
+ steer = float(np.clip(np.arctan(curvature * WHEELBASE) / MAX_STEER, -1.0, 1.0))
+
+ kappa = self.track.curvature_ahead(s, 14.0)
+ v_target = float(np.clip(np.sqrt(cfg.lat_accel / max(kappa, 1e-4)), cfg.min_speed, cfg.max_speed))
+ throttle = float(np.clip(cfg.speed_ff + cfg.speed_kp * (v_target - speed), -1.0, 1.0))
+ return np.array([steer, throttle], dtype=np.float32)
+
+ def body_frame_target(self, st: CarState) -> tuple[float, float]:
+ """Debug helper: target point in the car frame."""
+ lookahead = float(np.clip(self.cfg.lookahead_base + self.cfg.lookahead_gain * st.speed, 3.0, self.cfg.max_lookahead))
+ target = self.track.point_at(st.s + lookahead)
+ dx, dy = target[0] - st.x, target[1] - st.y
+ c, sn = np.cos(st.yaw), np.sin(st.yaw)
+ return float(c * dx + sn * dy), float(-sn * dx + c * dy)
diff --git a/src/mujoco_car_nn/models.py b/src/mujoco_car_nn/models.py
new file mode 100644
index 0000000000..bf84309ba2
--- /dev/null
+++ b/src/mujoco_car_nn/models.py
@@ -0,0 +1,182 @@
+"""Neural network policies for end-to-end and modular driving.
+
+The end-to-end policy consumes the 64x64 ego camera image and one
+proprioceptive scalar (forward speed). Speed is needed because a single
+frame does not tell the network whether the car is cruising or standing
+still, which makes the throttle label ambiguous (see ``docs/experiments``).
+"""
+
+from __future__ import annotations
+
+import numpy as np
+import torch
+from torch import nn
+
+
+class PolicyCNN(nn.Module):
+ """End-to-end CNN: 64x64 RGB image (+ speed) -> (steer, throttle)."""
+
+ def __init__(self, in_channels: int = 3, out_dim: int = 2, state_dim: int = 1, hidden: int = 256):
+ super().__init__()
+ self.state_dim = state_dim
+ self.features = nn.Sequential(
+ nn.Conv2d(in_channels, 24, 5, stride=2, padding=2),
+ nn.ReLU(inplace=True),
+ nn.Conv2d(24, 48, 5, stride=2, padding=2),
+ nn.ReLU(inplace=True),
+ nn.Conv2d(48, 64, 3, stride=2, padding=1),
+ nn.ReLU(inplace=True),
+ nn.Conv2d(64, 64, 3, stride=2, padding=1),
+ nn.ReLU(inplace=True),
+ )
+ self.head = nn.Sequential(
+ nn.Flatten(),
+ nn.Linear(64 * 4 * 4 + state_dim, hidden),
+ nn.ReLU(inplace=True),
+ nn.Linear(hidden, out_dim),
+ nn.Tanh(),
+ )
+
+ def forward(self, x: torch.Tensor, state: torch.Tensor | None = None) -> torch.Tensor:
+ h = self.features(x).flatten(1)
+ if self.state_dim:
+ h = torch.cat([h, state], dim=1)
+ return self.head(h)
+
+
+class PolicyMLP(nn.Module):
+ """Same inputs, no spatial inductive bias (architecture ablation)."""
+
+ def __init__(self, in_channels: int = 3, size: int = 64, out_dim: int = 2, state_dim: int = 1, hidden: int = 256):
+ super().__init__()
+ self.state_dim = state_dim
+ self.net = nn.Sequential(
+ nn.Flatten(),
+ nn.Linear(in_channels * size * size + state_dim, hidden),
+ nn.ReLU(inplace=True),
+ nn.Linear(hidden, hidden // 2),
+ nn.ReLU(inplace=True),
+ nn.Linear(hidden // 2, out_dim),
+ nn.Tanh(),
+ )
+
+ def forward(self, x: torch.Tensor, state: torch.Tensor | None = None) -> torch.Tensor:
+ h = x.flatten(1)
+ if self.state_dim:
+ h = torch.cat([h, state], dim=1)
+ return self.net(h)
+
+
+class PerceptionCNN(nn.Module):
+ """Image -> (lateral error, heading error), both normalised to [-1, 1]."""
+
+ def __init__(self, in_channels: int = 3):
+ super().__init__()
+ self.features = nn.Sequential(
+ nn.Conv2d(in_channels, 24, 5, stride=2, padding=2),
+ nn.ReLU(inplace=True),
+ nn.Conv2d(24, 48, 5, stride=2, padding=2),
+ nn.ReLU(inplace=True),
+ nn.Conv2d(48, 64, 3, stride=2, padding=1),
+ nn.ReLU(inplace=True),
+ nn.Conv2d(64, 64, 3, stride=2, padding=1),
+ nn.ReLU(inplace=True),
+ nn.Flatten(),
+ nn.Linear(64 * 4 * 4, 128),
+ nn.ReLU(inplace=True),
+ nn.Linear(128, 2),
+ nn.Tanh(),
+ )
+
+ def forward(self, x: torch.Tensor) -> torch.Tensor:
+ return self.features(x)
+
+
+class ModularPolicy(nn.Module):
+ """Modular pipeline: perception CNN + planning/control MLP.
+
+ The perception head predicts the lane-relative pose, the controller maps
+ ``(lateral, heading, speed)`` to commands. Both halves are trained
+ separately (perception with ground-truth labels, controller with expert
+ actions), which keeps every intermediate signal interpretable.
+ """
+
+ def __init__(self) -> None:
+ super().__init__()
+ self.state_dim = 1
+ self.perception = PerceptionCNN()
+ self.controller = nn.Sequential(
+ nn.Linear(3, 64),
+ nn.ReLU(inplace=True),
+ nn.Linear(64, 64),
+ nn.ReLU(inplace=True),
+ nn.Linear(64, 2),
+ nn.Tanh(),
+ )
+
+ def forward(self, x: torch.Tensor, state: torch.Tensor | None = None) -> torch.Tensor:
+ perception = self.perception(x)
+ return self.controller(torch.cat([perception, state], dim=1))
+
+ def errors(self, x: torch.Tensor) -> torch.Tensor:
+ return self.perception(x)
+
+
+def build_model(name: str, state_dim: int = 1) -> nn.Module:
+ name = name.lower()
+ if name in ("cnn", "e2e_cnn", "policy_cnn"):
+ return PolicyCNN(state_dim=state_dim)
+ if name in ("mlp", "e2e_mlp", "policy_mlp"):
+ return PolicyMLP(state_dim=state_dim)
+ if name in ("modular", "perception"):
+ return ModularPolicy()
+ raise ValueError(f"unknown model: {name}")
+
+
+def preprocess(images: np.ndarray) -> torch.Tensor:
+ """uint8 NHWC images -> float32 NCHW tensors in [-1, 1]."""
+ x = torch.from_numpy(np.ascontiguousarray(images)).float().div_(127.5).sub_(1.0)
+ return x.permute(0, 3, 1, 2)
+
+
+class PolicyRunner:
+ """Convenience wrapper: numpy image + speed in, normalised actions out."""
+
+ def __init__(self, model: nn.Module, device: str = "cpu"):
+ self.model = model.to(device).eval()
+ self.device = device
+
+ @torch.no_grad()
+ def __call__(self, image: np.ndarray, speed: float = 0.0, max_speed: float = 16.0) -> np.ndarray:
+ x = preprocess(image[None].astype(np.uint8)).to(self.device)
+ state = None
+ if getattr(self.model, "state_dim", 0):
+ state = torch.tensor([[speed / max_speed]], dtype=torch.float32, device=self.device)
+ return self.model(x, state).cpu().numpy()[0]
+
+
+class ModularGeometricRunner:
+ """Perception CNN + geometric pure-pursuit planner/controller.
+
+ The network supplies the lane-relative pose, the planner turns it into a
+ curvature command, and a P controller tracks the target speed. This is
+ the classic "learned perception + model-based control" stack.
+ """
+
+ def __init__(self, perception: nn.Module, wheelbase: float = 2.7, max_steer: float = 0.55):
+ self.perception = perception.eval()
+ self.wheelbase = wheelbase
+ self.max_steer = max_steer
+
+ @torch.no_grad()
+ def __call__(self, image: np.ndarray, speed: float = 0.0) -> np.ndarray:
+ x = preprocess(image[None].astype(np.uint8))
+ err = self.perception(x).numpy()[0]
+ e_y, e_psi = float(err[0]) * 4.5, float(err[1]) * 0.6
+ lookahead = float(np.clip(4.0 + 0.55 * speed, 3.0, 11.0))
+ alpha = -(e_psi + float(np.arctan2(e_y, lookahead)))
+ curvature = 2.0 * np.sin(alpha) / lookahead
+ steer = float(np.clip(np.arctan(curvature * self.wheelbase) / self.max_steer, -1.0, 1.0))
+ v_target = float(np.clip(9.0 - 6.0 * abs(e_psi), 4.0, 11.0))
+ throttle = float(np.clip(0.3 + 0.5 * (v_target - speed), -1.0, 1.0))
+ return np.array([steer, throttle], dtype=np.float32)
diff --git a/src/mujoco_car_nn/requirements.txt b/src/mujoco_car_nn/requirements.txt
new file mode 100644
index 0000000000..ed9fd62ad1
--- /dev/null
+++ b/src/mujoco_car_nn/requirements.txt
@@ -0,0 +1,5 @@
+mujoco>=3.2
+torch>=2.0
+numpy>=1.24
+pillow>=10.0
+matplotlib>=3.7
diff --git a/src/mujoco_car_nn/sim.py b/src/mujoco_car_nn/sim.py
new file mode 100644
index 0000000000..ab78cfe744
--- /dev/null
+++ b/src/mujoco_car_nn/sim.py
@@ -0,0 +1,431 @@
+"""MuJoCo simulation backend.
+
+The vehicle is a planar-constrained rigid body (two slide joints + one
+yaw hinge) driven by a dynamic bicycle model with a yaw-rate servo.
+Collisions with the track walls are resolved by MuJoCo's contact solver,
+so bumping into a wall really does slow the car down.
+
+Sensors
+-------
+* ``front`` camera, 70 deg vertical FOV, mounted on the roof (RGB frames
+ are what the neural networks consume);
+* a 2-D lidar made of 64 horizontal :func:`mujoco.mj_ray` casts, used by
+ the mapping / SLAM pipeline;
+* proprioceptive state (speed, yaw rate, lateral and heading error).
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+import mujoco
+import numpy as np
+
+from .track import (
+ CAR_LENGTH,
+ CAR_WIDTH,
+ ROAD_HALF_WIDTH,
+ WALL_HEIGHT,
+ WALL_THICKNESS,
+ Track,
+ build_track,
+ start_pose,
+)
+
+# ---------------------------------------------------------------- dynamics
+MASS = 1500.0
+WHEELBASE = 2.7
+INERTIA_Z = MASS * (CAR_LENGTH**2 + CAR_WIDTH**2) / 12.0
+MAX_STEER = 0.55
+MAX_ACCEL = 4.0
+MAX_BRAKE = 7.0
+MAX_LAT_ACCEL = 7.0
+MAX_SPEED = 16.0
+DRAG_C = 0.55
+ROLL_RESIST = 0.02
+YAW_SERVO = 12.0
+TAU_MAX = 12000.0
+SUBSTEPS = 5 # 100 Hz physics, 20 Hz control
+
+LIDAR_BEAMS = 64
+LIDAR_RANGE = 20.0
+LIDAR_HEIGHT = 0.05
+IMAGE_SIZE = 64
+
+SCENES = {
+ # name: (light diffuse, light ambient, haze, haze_color)
+ "day": ((1.0, 0.98, 0.92), (0.45, 0.45, 0.45), 0.05, (0.55, 0.70, 0.90)),
+ "dusk": ((1.0, 0.72, 0.45), (0.30, 0.26, 0.22), 0.18, (0.45, 0.35, 0.40)),
+ "fog": ((0.85, 0.88, 0.92), (0.50, 0.50, 0.50), 0.45, (0.75, 0.78, 0.82)),
+}
+
+
+def _quat_from_mat(mat: np.ndarray) -> np.ndarray:
+ """Rotation matrix -> (w, x, y, z) quaternion."""
+ m = np.asarray(mat, dtype=np.float64)
+ tr = m[0, 0] + m[1, 1] + m[2, 2]
+ if tr > 0:
+ s = np.sqrt(tr + 1.0) * 2.0
+ w = 0.25 * s
+ x = (m[2, 1] - m[1, 2]) / s
+ y = (m[0, 2] - m[2, 0]) / s
+ z = (m[1, 0] - m[0, 1]) / s
+ elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
+ s = np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2.0
+ w = (m[2, 1] - m[1, 2]) / s
+ x = 0.25 * s
+ y = (m[0, 1] + m[1, 0]) / s
+ z = (m[0, 2] + m[2, 0]) / s
+ elif m[1, 1] > m[2, 2]:
+ s = np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2.0
+ w = (m[0, 2] - m[2, 0]) / s
+ x = (m[0, 1] + m[1, 0]) / s
+ y = 0.25 * s
+ z = (m[1, 2] + m[2, 1]) / s
+ else:
+ s = np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2.0
+ w = (m[1, 0] - m[0, 1]) / s
+ x = (m[0, 2] + m[2, 0]) / s
+ y = (m[1, 2] + m[2, 1]) / s
+ z = 0.25 * s
+ q = np.array([w, x, y, z])
+ return q / np.linalg.norm(q)
+
+
+def camera_quat(pitch_deg: float = -6.0) -> np.ndarray:
+ """Orientation of a forward-looking camera in the car body frame."""
+ # Camera convention: looks along -Z, up along +Y, right along +X.
+ # In the body frame (x forward, y left, z up) this means
+ # camera x (right) = body -y
+ # camera y (up) = body +z
+ # camera z (back) = body -x
+ base = np.array(
+ [
+ [0.0, 0.0, -1.0],
+ [-1.0, 0.0, 0.0],
+ [0.0, 1.0, 0.0],
+ ]
+ )
+ p = np.deg2rad(pitch_deg)
+ rot_x = np.array([[1, 0, 0], [0, np.cos(p), -np.sin(p)], [0, np.sin(p), np.cos(p)]])
+ return _quat_from_mat(base @ rot_x)
+
+
+@dataclass
+class CarState:
+ x: float
+ y: float
+ yaw: float
+ speed: float # forward speed m/s
+ lateral: float # signed distance to centre line, +left
+ heading_err: float # rad, +left
+ s: float # progress along track
+ lateral_vel: float = 0.0
+ yaw_rate: float = 0.0
+
+ def as_vector(self) -> np.ndarray:
+ return np.array(
+ [self.speed / MAX_SPEED, self.lateral / ROAD_HALF_WIDTH, self.heading_err / 0.6, self.yaw_rate / 1.0],
+ dtype=np.float32,
+ )
+
+
+@dataclass
+class SimConfig:
+ scene: str = "day"
+ seed: int = 0
+ start_offset: float = 0.0
+ randomize_start: bool = False
+ offset_range: float = 1.2
+ allow_reverse: bool = False
+ range_noise: float = 0.02
+
+
+class CarSim:
+ """Thin, deterministic wrapper around a MuJoCo car + track scene."""
+
+ def __init__(self, track: Track | None = None, config: SimConfig | None = None):
+ self.track = track if track is not None else build_track()
+ self.config = config or SimConfig()
+ self.rng = np.random.default_rng(self.config.seed)
+
+ self.model = mujoco.MjModel.from_xml_string(self._xml(), assets=self._assets())
+ self.data = mujoco.MjData(self.model)
+ self.body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "chassis")
+ self.front_cam = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_CAMERA, "front")
+ self.wall_group = np.array([0, 1, 0, 0, 0, 0], dtype=np.uint8)
+ self.renderer = mujoco.Renderer(self.model, IMAGE_SIZE, IMAGE_SIZE)
+ self.renderer._scene_option.geomgroup[2] = 0 # hide own car body in ego camera
+ self._free_cam = mujoco.MjvCamera()
+ self._free_cam.type = mujoco.mjtCamera.mjCAMERA_FREE
+
+ self.set_scene(self.config.scene)
+ self.reset(self.config.start_offset)
+
+ # ------------------------------------------------------------------
+ # Scene construction
+ # ------------------------------------------------------------------
+ def _assets(self) -> dict[str, bytes]:
+ road = self._strip_obj(0.0, ROAD_HALF_WIDTH)
+ left = self._strip_obj(ROAD_HALF_WIDTH - 0.18, 0.18, z=0.02)
+ right = self._strip_obj(-(ROAD_HALF_WIDTH - 0.18), 0.18, z=0.02)
+ return {
+ "road.obj": road.encode("ascii"),
+ "edge_left.obj": left.encode("ascii"),
+ "edge_right.obj": right.encode("ascii"),
+ }
+
+ def _strip_obj(self, offset: float, half_width: float, z: float = 0.012) -> str:
+ """Road / lane strip as OBJ text (quad strip along the centre line)."""
+ idx = np.arange(0, len(self.track.points), 2)
+ left = self.track.points[idx] + (offset + half_width) * self.track.normals[idx]
+ right = self.track.points[idx] + (offset - half_width) * self.track.normals[idx]
+ n = len(idx)
+ lines = []
+ for (lx, ly), (rx, ry) in zip(left, right):
+ lines.append(f"v {lx:.4f} {ly:.4f} {z:.4f}")
+ lines.append(f"v {rx:.4f} {ry:.4f} {z:.4f}")
+ for i in range(n):
+ a, b = 2 * i + 1, 2 * i + 2
+ c, d = 2 * ((i + 1) % n) + 1, 2 * ((i + 1) % n) + 2
+ lines.append(f"f {a} {b} {d}")
+ lines.append(f"f {a} {d} {c}")
+ return "\n".join(lines) + "\n"
+
+ def _xml(self) -> str:
+ walls = []
+ for xy, yaw, hl in self.track.wall_poses(spacing=2.0):
+ for (x, y), angle in zip(xy, yaw):
+ walls.append(
+ f''
+ )
+
+ # Dashed centre line: 3 m dash every 6 m.
+ dashes = []
+ spacing, dash_len = 6.0, 3.0
+ n = int(self.track.length // spacing)
+ for k in range(n):
+ s = k * spacing
+ idx = int(round(s / self.track.length * len(self.track.points))) % len(self.track.points)
+ x, y = self.track.points[idx]
+ angle = np.arctan2(self.track.tangents[idx, 1], self.track.tangents[idx, 0])
+ dashes.append(
+ f''
+ )
+
+ q = camera_quat()
+ start = self.track.points[0]
+ yaw0 = np.arctan2(self.track.tangents[0, 1], self.track.tangents[0, 0])
+ qy = np.array([np.cos(yaw0 / 2), 0.0, 0.0, np.sin(yaw0 / 2)])
+
+ return f"""
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ {chr(10).join(dashes)}
+ {chr(10).join(walls)}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+"""
+
+ # ------------------------------------------------------------------
+ # Lifecycle
+ # ------------------------------------------------------------------
+ def reset(self, lateral_offset: float = 0.0) -> CarState:
+ if self.config.randomize_start:
+ lateral_offset = float(self.rng.uniform(-self.config.offset_range, self.config.offset_range))
+ xy, yaw = start_pose(self.track, lateral_offset)
+ self.data.qpos[:] = 0.0
+ self.data.qpos[0] = xy[0]
+ self.data.qpos[1] = xy[1]
+ self.data.qpos[2] = yaw
+ self.data.qvel[:] = 0.0
+ self.data.qfrc_applied[:] = 0.0
+ mujoco.mj_forward(self.model, self.data)
+ return self.state()
+
+ def set_scene(self, name: str) -> None:
+ diffuse, ambient, haze, color = SCENES[name]
+ self.model.light_diffuse[0] = diffuse
+ self.model.light_ambient[0] = ambient
+ self.model.vis.map.haze = haze
+ self.model.vis.rgba.haze[:3] = color
+ self.config.scene = name
+
+ # ------------------------------------------------------------------
+ # Physics
+ # ------------------------------------------------------------------
+ def _apply_control(self, steer: float, throttle: float) -> None:
+ d = self.data
+ yaw = float(d.qpos[2])
+ c, s = np.cos(yaw), np.sin(yaw)
+ v_fwd = c * d.qvel[0] + s * d.qvel[1]
+ v_lat = -s * d.qvel[0] + c * d.qvel[1]
+ omega = float(d.qvel[2])
+
+ delta = float(np.clip(steer, -1.0, 1.0)) * MAX_STEER
+ speed = max(abs(v_fwd), 0.6)
+ omega_des = v_fwd * np.tan(delta) / WHEELBASE
+ omega_max = MAX_LAT_ACCEL / speed
+ omega_des = float(np.clip(omega_des, -omega_max, omega_max))
+ tau = float(np.clip(INERTIA_Z * (omega_des - omega) * YAW_SERVO, -TAU_MAX, TAU_MAX))
+
+ throttle = float(np.clip(throttle, -1.0, 1.0))
+ accel = throttle * MAX_ACCEL if throttle >= 0 else throttle * MAX_BRAKE
+ if throttle < 0.0 and v_fwd <= 0.05 and not self.config.allow_reverse:
+ accel = 0.0 # evaluation never drives backwards; teleoperation may
+ f_long = MASS * accel - DRAG_C * v_fwd * abs(v_fwd) - ROLL_RESIST * MASS * 9.81 * np.sign(v_fwd)
+ if throttle <= 0.0 and abs(v_fwd) < 0.05 and not self.config.allow_reverse:
+ f_long = 0.0
+ if v_fwd > MAX_SPEED:
+ f_long -= MASS * (v_fwd - MAX_SPEED) * 2.0
+ f_lat = float(np.clip(-MASS * v_lat * 9.0, -1.0 * MASS * 9.81, 1.0 * MASS * 9.81))
+
+ d.qfrc_applied[0] = c * f_long - s * f_lat
+ d.qfrc_applied[1] = s * f_long + c * f_lat
+ d.qfrc_applied[2] = tau
+
+ def step(self, action, substeps: int = SUBSTEPS) -> CarState:
+ steer, throttle = float(action[0]), float(action[1])
+ for _ in range(substeps):
+ self._apply_control(steer, throttle)
+ mujoco.mj_step(self.model, self.data)
+ return self.state()
+
+ # ------------------------------------------------------------------
+ # Observation
+ # ------------------------------------------------------------------
+ def state(self) -> CarState:
+ x = float(self.data.qpos[0])
+ y = float(self.data.qpos[1])
+ yaw = float(self.data.qpos[2])
+ c, s = np.cos(yaw), np.sin(yaw)
+ vx = c * self.data.qvel[0] + s * self.data.qvel[1]
+ vy = -s * self.data.qvel[0] + c * self.data.qvel[1]
+ lateral, heading_err = self.track.lateral_error(np.array([x, y]), yaw)
+ idx, _ = self.track.nearest(np.array([x, y]))
+ return CarState(
+ x=x,
+ y=y,
+ yaw=yaw,
+ speed=float(vx),
+ lateral=lateral,
+ heading_err=heading_err,
+ s=float(self.track.s[idx]),
+ lateral_vel=float(vy),
+ yaw_rate=float(self.data.qvel[2]),
+ )
+
+ def lidar(self) -> np.ndarray:
+ """64 horizontal ranges in the car frame, ``LIDAR_RANGE`` = no hit."""
+ d = self.data
+ yaw = float(d.qpos[2])
+ origin = np.array([d.qpos[0], d.qpos[1], LIDAR_HEIGHT + 0.55])
+ ranges = np.full(LIDAR_BEAMS, LIDAR_RANGE, dtype=np.float32)
+ angles = yaw + np.linspace(-np.pi, np.pi, LIDAR_BEAMS, endpoint=False)
+ geomid = np.zeros(1, dtype=np.int32)
+ for i, a in enumerate(angles):
+ vec = np.array([np.cos(a), np.sin(a), 0.0])
+ dist = mujoco.mj_ray(
+ self.model, d, origin, vec, self.wall_group, True, -1, geomid
+ )
+ if dist >= 0.0:
+ ranges[i] = min(float(dist), LIDAR_RANGE)
+ if self.config.range_noise > 0:
+ ranges += self.rng.normal(0.0, self.config.range_noise, size=ranges.shape).astype(np.float32)
+ ranges = np.clip(ranges, 0.05, LIDAR_RANGE)
+ return ranges
+
+ def camera_image(self) -> np.ndarray:
+ self.renderer.update_scene(self.data, camera=self.front_cam)
+ return self.renderer.render().copy()
+
+ # ------------------------------------------------------------------
+ # Rendering for figures / GIFs
+ # ------------------------------------------------------------------
+ def render_free(self, width: int, height: int, lookat, azimuth: float, elevation: float, distance: float) -> np.ndarray:
+ cam = self._free_cam
+ cam.lookat[:] = lookat
+ cam.azimuth = azimuth
+ cam.elevation = elevation
+ cam.distance = distance
+ renderer = getattr(self, "_viz_renderer", None)
+ if renderer is None or renderer.width != width or renderer.height != height:
+ renderer = mujoco.Renderer(self.model, height, width)
+ self._viz_renderer = renderer
+ renderer.update_scene(self.data, camera=cam)
+ return renderer.render().copy()
+
+ def render_chase(self, width: int = 640, height: int = 360) -> np.ndarray:
+ st = self.state()
+ lookat = [st.x + 2.0 * np.cos(st.yaw), st.y + 2.0 * np.sin(st.yaw), 1.0]
+ az = np.degrees(st.yaw)
+ return self.render_free(width, height, lookat, az, -14.0, 11.0)
+
+ def render_top(self, width: int = 640, height: int = 360, distance: float = 105.0) -> np.ndarray:
+ return self.render_free(width, height, [0.0, 0.0, 0.0], 90.0, -90.0, distance)
+
+ def render_front(self, width: int = 640, height: int = 360) -> np.ndarray:
+ renderer = getattr(self, "_front_big", None)
+ if renderer is None or renderer.width != width or renderer.height != height:
+ renderer = mujoco.Renderer(self.model, height, width)
+ renderer._scene_option.geomgroup[2] = 0
+ self._front_big = renderer
+ renderer.update_scene(self.data, camera=self.front_cam)
+ return renderer.render().copy()
+
+ def close(self) -> None:
+ for name in ("renderer", "_viz_renderer", "_front_big"):
+ obj = getattr(self, name, None)
+ if obj is not None:
+ try:
+ obj.close()
+ except Exception: # pragma: no cover - best effort cleanup
+ pass
+
diff --git a/src/mujoco_car_nn/slam.py b/src/mujoco_car_nn/slam.py
new file mode 100644
index 0000000000..13e55451e9
--- /dev/null
+++ b/src/mujoco_car_nn/slam.py
@@ -0,0 +1,261 @@
+"""2-D occupancy-grid mapping with correlative scan matching (SLAM).
+
+The pipeline is the classic "scan-to-map" front end:
+
+1. dead-reckoning prediction from speed and yaw-rate measurements;
+2. correlative scan matching of the current lidar scan against the map
+ built so far (coarse-to-fine grid search);
+3. log-odds occupancy update with ray casting;
+4. trajectory / map accuracy evaluation against the simulator ground truth.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+import numpy as np
+
+from .sim import CarSim, SimConfig
+from .track import ROAD_HALF_WIDTH, WALL_HEIGHT, WALL_THICKNESS, Track
+
+
+@dataclass
+class GridMap:
+ """Log-odds occupancy grid in world coordinates."""
+
+ resolution: float = 0.25
+ x_min: float = -55.0
+ y_min: float = -45.0
+ width: int = 440
+ height: int = 360
+ log_odds: np.ndarray = field(default=None)
+ hit: float = 0.85
+ miss: float = -0.35
+ clamp: float = 6.0
+
+ def __post_init__(self):
+ if self.log_odds is None:
+ self.log_odds = np.zeros((self.height, self.width), dtype=np.float32)
+
+ # ------------------------------------------------------------------
+ def cells(self, xy: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+ cx = ((xy[:, 0] - self.x_min) / self.resolution).astype(np.int32)
+ cy = ((xy[:, 1] - self.y_min) / self.resolution).astype(np.int32)
+ valid = (cx >= 0) & (cx < self.width) & (cy >= 0) & (cy < self.height)
+ return cx, cy, valid
+
+ def probability(self) -> np.ndarray:
+ return 1.0 / (1.0 + np.exp(-self.log_odds))
+
+ def update(self, pose: tuple[float, float, float], ranges: np.ndarray, angles: np.ndarray, max_range: float = 20.0) -> None:
+ x, y, yaw = pose
+ valid = ranges < max_range - 1e-3
+ ranges = ranges[valid]
+ angles = angles[valid]
+ if len(ranges) == 0:
+ return
+
+ world_angles = yaw + angles
+ dirs = np.column_stack([np.cos(world_angles), np.sin(world_angles)])
+ ends = np.column_stack([x, y]) + dirs * ranges[:, None]
+
+ # free space along each ray (skip the last metre: the wall thickness)
+ n_steps = 12
+ ts = np.linspace(0.1, 0.9, n_steps)
+ free = (np.column_stack([x, y])[None] + dirs[:, None, :] * (ranges[:, None] * ts)[:, :, None]).reshape(-1, 2)
+ cx, cy, ok = self.cells(free)
+ np.add.at(self.log_odds, (cy[ok], cx[ok]), self.miss)
+
+ # occupied endpoints, dilated by one cell to account for discretisation
+ cx, cy, ok = self.cells(ends)
+ for ox in (-1, 0, 1):
+ for oy in (-1, 0, 1):
+ ccx = np.clip(cx + ox, 0, self.width - 1)
+ ccy = np.clip(cy + oy, 0, self.height - 1)
+ np.add.at(self.log_odds, (ccy[ok], ccx[ok]), self.hit / 9.0)
+ np.clip(self.log_odds, -self.clamp, self.clamp, out=self.log_odds)
+
+
+def ray_endpoints(pose, ranges, angles):
+ x, y, yaw = pose
+ a = yaw + angles
+ return np.column_stack([x + ranges * np.cos(a), y + ranges * np.sin(a)])
+
+
+def scan_match(
+ points_local: np.ndarray,
+ grid: GridMap,
+ prior: tuple[float, float, float],
+ search_xy: float = 0.6,
+ search_yaw: float = 0.10,
+ coarse: int = 5,
+) -> tuple[tuple[float, float, float], float]:
+ """Correlative scan matching around ``prior`` against the current map."""
+ prob = grid.probability()
+ x0, y0, th0 = prior
+
+ best = (prior, -1.0)
+ for level, (nxy, nth) in enumerate(((coarse, 3), (3, 3))):
+ dxs = np.linspace(-search_xy, search_xy, nxy) * (0.35 if level else 1.0)
+ dys = np.linspace(-search_xy, search_xy, nxy) * (0.35 if level else 1.0)
+ dths = np.linspace(-search_yaw, search_yaw, nth) * (0.25 if level else 1.0)
+ bx, by, bth = best[0] if level else (x0, y0, th0)
+ dxs = dxs + bx
+ dys = dys + by
+ dths = dths + bth
+
+ for dx in dxs:
+ for dy in dys:
+ for dth in dths:
+ c, s = np.cos(dth), np.sin(dth)
+ wx = c * points_local[:, 0] - s * points_local[:, 1] + dx
+ wy = s * points_local[:, 0] + c * points_local[:, 1] + dy
+ cx = ((wx - grid.x_min) / grid.resolution).astype(np.int32)
+ cy = ((wy - grid.y_min) / grid.resolution).astype(np.int32)
+ ok = (cx >= 0) & (cx < grid.width) & (cy >= 0) & (cy < grid.height)
+ if ok.sum() < 8:
+ continue
+ lv = grid.log_odds[cy[ok], cx[ok]]
+ # unknown cells have p = 0.5 and therefore contribute
+ # exactly zero, so a pose is never rewarded for simply
+ # pointing at unmapped space.
+ p = 1.0 / (1.0 + np.exp(-lv))
+ score = float(p.mean()) - 0.5
+ # motion prior: stay close to the dead-reckoned pose
+ score -= 0.6 * ((dx - x0) ** 2 + (dy - y0) ** 2) / max(search_xy**2, 1e-6)
+ score -= 0.3 * ((dth - th0) ** 2) / max(search_yaw**2, 1e-6)
+ if score > best[1]:
+ best = ((float(dx), float(dy), float(dth)), score)
+ return best
+
+
+def run_slam(
+ track: Track,
+ policy=None,
+ scene: str = "day",
+ laps: float = 2.0,
+ max_steps: int = 2000,
+ seed: int = 0,
+ odom_noise: float = 0.03,
+ match_blend: float = 0.5,
+ record_every: int = 6,
+ render=None,
+):
+ """Drive and build a map online; returns metrics, frames and the grid."""
+ sim = CarSim(track, SimConfig(scene=scene, seed=seed, randomize_start=True))
+ st = sim.reset()
+ rng = np.random.default_rng(seed)
+
+ grid = GridMap()
+ pose = np.array([st.x, st.y, st.yaw], dtype=np.float64)
+ traj_est = [pose[:2].copy()]
+ traj_true = [np.array([st.x, st.y])]
+ ate_err = []
+
+ angles = np.linspace(-np.pi, np.pi, 64, endpoint=False)
+ prev_s = st.s
+ travelled = 0.0
+ steps = 0
+ frames = []
+
+ from .evaluate import rollout # noqa: F401 (avoid cycle at import time)
+ from .expert import PurePursuitExpert
+
+ expert = PurePursuitExpert(track)
+
+ while travelled < laps * track.length and steps < max_steps:
+ # ---- 1) scan at the current (estimated) pose
+ ranges = sim.lidar()
+ local = np.column_stack([ranges * np.cos(angles), ranges * np.sin(angles)])
+
+ # ---- 2) scan-to-map matching refines the dead-reckoned pose
+ if steps > 2 and grid.log_odds.any():
+ (mx, my, mth), _score = scan_match(local, grid, tuple(pose), search_xy=0.35, search_yaw=0.06)
+ # A single scan constrains the lateral offset and the heading far
+ # better than the along-track position (the walls are almost
+ # translation invariant along the road), so only the lateral and
+ # yaw components of the correction are applied; the along-track
+ # position keeps trusting the wheel odometry.
+ yaw = pose[2]
+ left = np.array([-np.sin(yaw), np.cos(yaw)])
+ delta = np.array([mx - pose[0], my - pose[1]])
+ lateral = float(delta @ left)
+ pose = np.array(
+ [
+ pose[0] + match_blend * lateral * left[0],
+ pose[1] + match_blend * lateral * left[1],
+ pose[2],
+ ],
+ dtype=np.float64,
+ )
+
+ # ---- 3) map update with the corrected pose
+ grid.update(tuple(pose), ranges, angles)
+
+ traj_est.append(pose[:2].copy())
+ traj_true.append(np.array([st.x, st.y]))
+ ate_err.append(float(np.linalg.norm(pose[:2] - traj_true[-1])))
+
+ if record_every and steps % record_every == 0 and render is not None:
+ frames.append(render(grid, traj_est, pose, st))
+
+ # ---- 4) control + physics step
+ if policy is None:
+ action = expert.act(st)
+ else:
+ action = policy(sim.camera_image(), st.speed)
+ st_new = sim.step(np.clip(action, -1, 1))
+
+ # ---- 5) dead reckoning for the next prior (noisy wheel/IMU odometry)
+ dt = 0.05
+ v = 0.5 * (st.speed + st_new.speed) * (1.0 + rng.normal(0, odom_noise))
+ w = 0.5 * (st.yaw_rate + st_new.yaw_rate) * (1.0 + rng.normal(0, odom_noise * 2))
+ yaw_mid = pose[2] + 0.5 * w * dt
+ pose = np.array(
+ [pose[0] + v * np.cos(yaw_mid) * dt, pose[1] + v * np.sin(yaw_mid) * dt, pose[2] + w * dt],
+ dtype=np.float64,
+ )
+
+ travelled += (st_new.s - prev_s) % track.length
+ prev_s = st_new.s
+ steps += 1
+ st = st_new
+
+ metrics = {
+ "steps": steps,
+ "laps": travelled / track.length,
+ "ate_rmse": float(np.sqrt(np.mean(np.square(ate_err)))) if ate_err else 0.0,
+ "ate_max": float(np.max(ate_err)) if ate_err else 0.0,
+ "map_cells": int((grid.log_odds > 1.0).sum()),
+ "trajectory": np.asarray(traj_true),
+ "estimated": np.asarray(traj_est),
+ "coverage": float((grid.log_odds != 0).mean()),
+ }
+ sim.close()
+ return metrics, frames, grid
+
+
+def ground_truth_map(grid: GridMap, track: Track) -> np.ndarray:
+ """Rasterise the wall rectangles into an occupancy grid (for IoU)."""
+ gt = np.zeros_like(grid.log_odds)
+ half = ROAD_HALF_WIDTH + WALL_THICKNESS * 0.5
+ for xy, yaw, hl in track.wall_poses(spacing=2.0):
+ for (x, y), a in zip(xy, yaw):
+ c, s = np.cos(a), np.sin(a)
+ for t in np.linspace(-hl, hl, 12):
+ for o in np.linspace(-WALL_THICKNESS / 2, WALL_THICKNESS / 2, 3):
+ wx = x + c * t - s * o
+ wy = y + s * t + c * o
+ cx = int((wx - grid.x_min) / grid.resolution)
+ cy = int((wy - grid.y_min) / grid.resolution)
+ if 0 <= cx < grid.width and 0 <= cy < grid.height:
+ gt[cy, cx] = 1.0
+ return gt
+
+
+def map_iou(grid: GridMap, track: Track, threshold: float = 1.0) -> float:
+ gt = ground_truth_map(grid, track) > 0
+ pred = grid.log_odds > threshold
+ inter = np.logical_and(gt, pred).sum()
+ union = np.logical_or(gt, pred).sum()
+ return float(inter / max(union, 1))
diff --git a/src/mujoco_car_nn/track.py b/src/mujoco_car_nn/track.py
new file mode 100644
index 0000000000..032ac3045d
--- /dev/null
+++ b/src/mujoco_car_nn/track.py
@@ -0,0 +1,170 @@
+"""Closed-loop test track geometry.
+
+The centre line is a smooth, star-shaped closed curve expressed in polar
+form::
+
+ r(theta) = R0 + A * cos(2*theta) + B * sin(3*theta)
+
+which gives a long oval with two gentle chicanes, a minimum turning
+radius of about 12 m and no self intersections. Everything else (road
+mesh, lane markings, walls, expert look-ahead, SLAM ground truth) is
+derived from the sampled centre line so that geometry, rendering and
+evaluation always agree.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+# Real-world scale (metres): a 4.2 m car on a 9 m wide two-lane road.
+ROAD_HALF_WIDTH = 4.5
+CAR_LENGTH = 4.2
+CAR_WIDTH = 1.8
+WALL_THICKNESS = 0.16
+WALL_HEIGHT = 0.7
+
+# Shape parameters of the closed curve.
+R0, AMP2, AMP3 = 34.0, 7.0, 3.5
+
+
+def _wrap(angle: float) -> float:
+ return float((angle + np.pi) % (2.0 * np.pi) - np.pi)
+
+
+@dataclass
+class Track:
+ """Sampled centre line with tangents, normals and curvature."""
+
+ points: np.ndarray # (N, 2) centre line
+ tangents: np.ndarray # (N, 2) unit heading
+ normals: np.ndarray # (N, 2) unit left normal
+ curvature: np.ndarray # (N,) signed curvature, 1/m
+ s: np.ndarray # (N,) arc length from the start
+ length: float
+
+ # ------------------------------------------------------------------
+ # Sampling helpers
+ # ------------------------------------------------------------------
+ def _index_at(self, s: float) -> int:
+ s = float(s) % self.length
+ return int(np.searchsorted(self.s, s, side="right") - 1) % len(self.s)
+
+ def heading_at(self, s: float) -> float:
+ idx = self._index_at(s)
+ return float(np.arctan2(self.tangents[idx, 1], self.tangents[idx, 0]))
+
+ def point_at(self, s: float) -> np.ndarray:
+ return self.points[self._index_at(s)].copy()
+
+ def nearest(self, xy: np.ndarray) -> tuple[int, float]:
+ """Index of the nearest centre-line sample and the signed lateral error.
+
+ The lateral error is positive when the point lies to the left of
+ the centre line.
+ """
+ xy = np.asarray(xy, dtype=np.float64)
+ d2 = np.sum((self.points - xy[:2]) ** 2, axis=1)
+ idx = int(np.argmin(d2))
+ delta = xy[:2] - self.points[idx]
+ return idx, float(delta @ self.normals[idx])
+
+ def lateral_error(self, xy: np.ndarray, heading: float) -> tuple[float, float]:
+ """Signed lateral error (m) and heading error (rad)."""
+ idx, lateral = self.nearest(xy)
+ track_heading = float(np.arctan2(self.tangents[idx, 1], self.tangents[idx, 0]))
+ return lateral, float(_wrap(heading - track_heading))
+
+ def curvature_ahead(self, s: float, distance: float) -> float:
+ """Largest |curvature| over the next ``distance`` metres."""
+ idx = self._index_at(s)
+ end = self._index_at(s + distance)
+ if end <= idx:
+ span = np.r_[idx : len(self.curvature), 0 : end + 1]
+ else:
+ span = np.arange(idx, end + 1)
+ return float(np.max(np.abs(self.curvature[span])))
+
+ def progress(self, xy: np.ndarray) -> float:
+ """Arc length of the nearest centre-line sample to ``xy``."""
+ return float(self.s[self.nearest(xy)[0]])
+
+ # ------------------------------------------------------------------
+ # Geometry generation
+ # ------------------------------------------------------------------
+ def wall_poses(self, spacing: float = 2.0):
+ """(xy, yaw, half_length) poses for the boxes forming both walls."""
+ step = max(1, int(round(len(self.points) / self.length * spacing)))
+ idx = np.arange(0, len(self.points), step)
+ half_len = spacing * 0.55
+ yaw = np.arctan2(self.tangents[idx, 1], self.tangents[idx, 0])
+ out = []
+ for side in (+1.0, -1.0):
+ offset = side * (ROAD_HALF_WIDTH + WALL_THICKNESS * 0.5)
+ xy = self.points[idx] + offset * self.normals[idx]
+ out.append((xy, yaw, half_len))
+ return out
+
+ def write_strip_obj(self, path: str, offset: float, half_width: float, z: float = 0.012) -> None:
+ """Write a quad strip (road surface or lane marking) as Wavefront OBJ."""
+ step = 2
+ idx = np.arange(0, len(self.points), step)
+ left = self.points[idx] + (offset + half_width) * self.normals[idx]
+ right = self.points[idx] + (offset - half_width) * self.normals[idx]
+ n = len(idx)
+ with open(path, "w", encoding="ascii") as fh:
+ for (lx, ly), (rx, ry) in zip(left, right):
+ fh.write(f"v {lx:.4f} {ly:.4f} {z:.4f}\n")
+ fh.write(f"v {rx:.4f} {ry:.4f} {z:.4f}\n")
+ for i in range(n):
+ a, b = 2 * i + 1, 2 * i + 2
+ c, d = 2 * ((i + 1) % n) + 1, 2 * ((i + 1) % n) + 2
+ fh.write(f"f {a} {b} {d}\n")
+ fh.write(f"f {a} {d} {c}\n")
+
+ def bounds(self) -> tuple[float, float, float, float]:
+ return (
+ float(self.points[:, 0].min()),
+ float(self.points[:, 1].min()),
+ float(self.points[:, 0].max()),
+ float(self.points[:, 1].max()),
+ )
+
+
+def build_track(resolution: float = 0.25) -> Track:
+ """Sample the analytic curve at (approximately) uniform arc length."""
+ t = np.linspace(0.0, 2.0 * np.pi, 20000, endpoint=False)
+ r = R0 + AMP2 * np.cos(2.0 * t) + AMP3 * np.sin(3.0 * t)
+ pts = np.column_stack([r * np.cos(t), r * np.sin(t)])
+
+ seg = np.linalg.norm(np.diff(np.vstack([pts, pts[:1]]), axis=0), axis=1)
+ s_raw = np.concatenate([[0.0], np.cumsum(seg)[:-1]])
+ total = float(seg.sum())
+ n = int(round(total / resolution))
+ s = np.linspace(0.0, total, n, endpoint=False)
+ x = np.interp(s, np.r_[s_raw, total], np.r_[pts[:, 0], pts[0, 0]])
+ y = np.interp(s, np.r_[s_raw, total], np.r_[pts[:, 1], pts[0, 1]])
+ points = np.column_stack([x, y])
+
+ d = np.gradient(points, axis=0)
+ tangents = d / np.linalg.norm(d, axis=1, keepdims=True)
+ normals = np.column_stack([-tangents[:, 1], tangents[:, 0]]) # left normal
+
+ nxt = np.roll(tangents, -1, axis=0)
+ cross = tangents[:, 0] * nxt[:, 1] - tangents[:, 1] * nxt[:, 0]
+ dtheta = np.arctan2(cross, np.sum(tangents * nxt, axis=1))
+ ds = np.linalg.norm(np.roll(points, -1, axis=0) - points, axis=1)
+ curvature = dtheta / np.maximum(ds, 1e-6)
+
+ kernel = np.ones(5) / 5.0
+ curvature = np.convolve(np.r_[curvature[-4:], curvature, curvature[:4]], kernel, mode="same")[4:-4]
+
+ return Track(points=points, tangents=tangents, normals=normals, curvature=curvature, s=s, length=total)
+
+
+def start_pose(track: Track, lateral_offset: float = 0.0) -> tuple[np.ndarray, float]:
+ """Initial (x, y) and yaw of the car at the start line."""
+ xy = track.points[0] + lateral_offset * track.normals[0]
+ yaw = float(np.arctan2(track.tangents[0, 1], track.tangents[0, 0]))
+ return xy.astype(np.float64), yaw
diff --git a/src/mujoco_car_nn/train.py b/src/mujoco_car_nn/train.py
new file mode 100644
index 0000000000..6a33f46b93
--- /dev/null
+++ b/src/mujoco_car_nn/train.py
@@ -0,0 +1,177 @@
+"""Behaviour cloning / DAgger training loops."""
+
+from __future__ import annotations
+
+import copy
+import time
+from dataclasses import dataclass, field
+
+import numpy as np
+import torch
+from torch import nn
+from torch.utils.data import DataLoader
+
+from .dataset import CollectConfig, DrivingDataset, collect_dataset, split_indices
+from .expert import PurePursuitExpert
+from .models import build_model
+
+
+@dataclass
+class TrainConfig:
+ model: str = "cnn"
+ epochs: int = 24
+ batch_size: int = 64
+ lr: float = 1e-3
+ weight_decay: float = 1e-4
+ steer_weight: float = 1.0
+ throttle_weight: float = 0.6
+ augment: bool = True
+ val_ratio: float = 0.15
+ seed: int = 0
+ device: str = "cpu"
+
+
+def train_policy(
+ data: dict[str, np.ndarray],
+ config: TrainConfig | None = None,
+ model: nn.Module | None = None,
+ verbose: bool = True,
+) -> tuple[nn.Module, dict]:
+ cfg = config or TrainConfig()
+ torch.manual_seed(cfg.seed)
+ np.random.seed(cfg.seed)
+
+ images, actions = data["images"], data["actions"].astype(np.float32)
+ states = data.get("states")
+ train_idx, val_idx = split_indices(len(images), cfg.val_ratio, cfg.seed)
+ train_ds = DrivingDataset(
+ images[train_idx], actions[train_idx], None if states is None else states[train_idx], augment=cfg.augment, seed=cfg.seed
+ )
+ val_ds = DrivingDataset(images[val_idx], actions[val_idx], None if states is None else states[val_idx], augment=False)
+ train_loader = DataLoader(train_ds, batch_size=cfg.batch_size, shuffle=True, drop_last=True)
+ val_loader = DataLoader(val_ds, batch_size=cfg.batch_size, shuffle=False)
+
+ net = model if model is not None else build_model(cfg.model)
+ net.to(cfg.device)
+ opt = torch.optim.AdamW(net.parameters(), lr=cfg.lr, weight_decay=cfg.weight_decay)
+ sched = torch.optim.lr_scheduler.CosineAnnealingLR(opt, T_max=cfg.epochs)
+ weights = torch.tensor([cfg.steer_weight, cfg.throttle_weight], device=cfg.device)
+
+ history = {"train_loss": [], "val_loss": [], "val_steer_mae": [], "val_throttle_mae": []}
+ best_state, best_val = None, float("inf")
+ t0 = time.time()
+
+ for epoch in range(cfg.epochs):
+ net.train()
+ total, count = 0.0, 0
+ for x, s, y in train_loader:
+ x, y, s = x.to(cfg.device), y.to(cfg.device), s.to(cfg.device)
+ pred = net(x, s)
+ loss = (((pred - y) ** 2) * weights).mean()
+ opt.zero_grad(set_to_none=True)
+ loss.backward()
+ nn.utils.clip_grad_norm_(net.parameters(), 5.0)
+ opt.step()
+ total += loss.item() * len(x)
+ count += len(x)
+ sched.step()
+
+ net.eval()
+ val_total, val_count = 0.0, 0
+ abs_steer, abs_thr = 0.0, 0.0
+ with torch.no_grad():
+ for x, s, y in val_loader:
+ x, y, s = x.to(cfg.device), y.to(cfg.device), s.to(cfg.device)
+ pred = net(x, s)
+ val_total += float((((pred - y) ** 2) * weights).mean()) * len(x)
+ abs_steer += float((pred[:, 0] - y[:, 0]).abs().sum())
+ abs_thr += float((pred[:, 1] - y[:, 1]).abs().sum())
+ val_count += len(x)
+ val_loss = val_total / max(val_count, 1)
+ history["train_loss"].append(total / max(count, 1))
+ history["val_loss"].append(val_loss)
+ history["val_steer_mae"].append(abs_steer / max(val_count, 1))
+ history["val_throttle_mae"].append(abs_thr / max(val_count, 1))
+ if val_loss < best_val:
+ best_val = val_loss
+ best_state = copy.deepcopy(net.state_dict())
+ if verbose and (epoch % 4 == 0 or epoch == cfg.epochs - 1):
+ print(
+ f"epoch {epoch:3d} train {history['train_loss'][-1]:.4f} val {val_loss:.4f} "
+ f"steer_mae {history['val_steer_mae'][-1]:.3f} thr_mae {history['val_throttle_mae'][-1]:.3f}"
+ )
+
+ if best_state is not None:
+ net.load_state_dict(best_state)
+ history["train_seconds"] = time.time() - t0
+ history["n_train"] = len(train_idx)
+ history["n_val"] = len(val_idx)
+ history["best_val"] = best_val
+ return net, history
+
+
+def dagger_train(
+ track,
+ data: dict[str, np.ndarray],
+ config: TrainConfig | None = None,
+ iterations: int = 2,
+ collect: CollectConfig | None = None,
+ verbose: bool = True,
+) -> tuple[nn.Module, dict[str, np.ndarray], dict]:
+ """DAgger: roll out the current policy, label the visited states with the
+ expert, aggregate the data and retrain."""
+ from .evaluate import rollout # local import to avoid a cycle
+ from .models import PolicyRunner
+
+ cfg = config or TrainConfig()
+ net, history = train_policy(data, cfg, verbose=verbose)
+ logs = []
+
+ for it in range(iterations):
+ runner = PolicyRunner(net)
+ new = rollout_policy_collect(track, runner, collect or CollectConfig(), seed=cfg.seed + 100 + it)
+ data = {k: np.concatenate([data[k], new[k]], axis=0) for k in ("images", "actions", "states", "scans", "scenes")}
+ net, hist = train_policy(data, cfg, verbose=verbose)
+ logs.append(hist)
+ return net, data, {"bc": history, "dagger": logs}
+
+
+def rollout_policy_collect(track, policy, cfg: CollectConfig, seed: int = 0) -> dict[str, np.ndarray]:
+ """Collect states visited by ``policy`` with expert labels (DAgger)."""
+ from .sim import CarSim, SimConfig
+
+ rng = np.random.default_rng(seed)
+ sim = CarSim(track, SimConfig(scene=cfg.scenes[0], seed=seed, randomize_start=True))
+ expert = PurePursuitExpert(track)
+ st = sim.reset()
+ images, actions, states, scans, scenes = [], [], [], [], []
+ prev_s = st.s
+ travelled = 0.0
+ steps = 0
+ while travelled < cfg.laps * track.length and steps < cfg.max_steps:
+ img = sim.camera_image()
+ label = expert.act(st)
+ action = policy(img, st.speed)
+ # beta-mixing keeps the car moving so that the visited state
+ # distribution stays close to the expert's (classic DAgger trick).
+ if rng.random() < 0.35:
+ action = label.copy()
+ if rng.random() < 0.15:
+ action = action + rng.normal(0, 0.1, size=2)
+ images.append(img)
+ scans.append(sim.lidar())
+ states.append(st.as_vector())
+ actions.append(label)
+ scenes.append(cfg.scenes[0])
+ st = sim.step(np.clip(action, -1, 1))
+ travelled += (st.s - prev_s) % track.length
+ prev_s = st.s
+ steps += 1
+ sim.close()
+ return {
+ "images": np.asarray(images, dtype=np.uint8),
+ "actions": np.asarray(actions, dtype=np.float32),
+ "states": np.asarray(states, dtype=np.float32),
+ "scans": np.asarray(scans, dtype=np.float32),
+ "scenes": np.asarray(scenes),
+ }
diff --git a/src/mujoco_car_nn/viz.py b/src/mujoco_car_nn/viz.py
new file mode 100644
index 0000000000..1ca19cf59c
--- /dev/null
+++ b/src/mujoco_car_nn/viz.py
@@ -0,0 +1,130 @@
+"""Visualisation helpers: HUD overlays, composite frames and GIF export."""
+
+from __future__ import annotations
+
+import os
+from dataclasses import dataclass
+
+import numpy as np
+from PIL import Image, ImageDraw, ImageFont
+
+FONT_CANDIDATES = [
+ r"C:\Windows\Fonts\consola.ttf",
+ r"C:\Windows\Fonts\arial.ttf",
+ "/usr/share/fonts/truetype/dejavu/DejaVuSansMono.ttf",
+ "/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf",
+]
+
+
+def _font(size: int = 14):
+ for path in FONT_CANDIDATES:
+ if os.path.exists(path):
+ try:
+ return ImageFont.truetype(path, size)
+ except OSError:
+ continue
+ return ImageFont.load_default()
+
+
+def draw_hud(img: np.ndarray, lines: list[str], origin=(10, 8), size: int = 15, color=(255, 255, 255)) -> np.ndarray:
+ """Draw a list of text lines with a translucent background box."""
+ base = Image.fromarray(img).convert("RGBA")
+ overlay = Image.new("RGBA", base.size, (0, 0, 0, 0))
+ draw = ImageDraw.Draw(overlay)
+ font = _font(size)
+ pad = 6
+ widths, heights = [], []
+ for line in lines:
+ box = draw.textbbox((0, 0), line, font=font)
+ widths.append(box[2] - box[0])
+ heights.append(box[3] - box[1])
+ w = max(widths) + 2 * pad if widths else 0
+ h = sum(heights) + (len(lines) + 1) * pad
+ draw.rectangle([origin[0] - pad, origin[1] - pad, origin[0] + w, origin[1] + h], fill=(0, 0, 0, 110))
+ y = origin[1]
+ for line, hh in zip(lines, heights):
+ draw.text((origin[0], y), line, font=font, fill=color)
+ y += hh + pad
+ return np.asarray(Image.alpha_composite(base, overlay).convert("RGB"))
+
+
+def composite_demo(chase: np.ndarray, ego: np.ndarray, lines: list[str], inset: int = 176) -> np.ndarray:
+ """Chase view with an ego-camera inset (top-right) and a HUD (top-left)."""
+ h, w = chase.shape[:2]
+ canvas = Image.fromarray(chase)
+ ego_img = Image.fromarray(ego).resize((inset, int(inset * ego.shape[0] / ego.shape[1])), Image.BILINEAR)
+ canvas.paste(ego_img, (w - ego_img.width - 8, 8))
+ img = draw_hud(np.asarray(canvas), lines)
+ return img
+
+
+def save_gif(frames: list[np.ndarray], path: str, fps: int = 8, colors: int = 64, scale: float = 1.0) -> int:
+ """Save frames as an optimised, palette-quantised GIF. Returns size in bytes."""
+ if not frames:
+ raise ValueError("no frames")
+ os.makedirs(os.path.dirname(os.path.abspath(path)), exist_ok=True)
+ imgs = [Image.fromarray(f).convert("RGB") for f in frames]
+ if scale != 1.0:
+ imgs = [im.resize((int(im.width * scale), int(im.height * scale)), Image.LANCZOS) for im in imgs]
+ imgs = [im.convert("P", palette=Image.ADAPTIVE, colors=colors) for im in imgs]
+ imgs[0].save(
+ path,
+ save_all=True,
+ append_images=imgs[1:],
+ duration=int(1000 / fps),
+ loop=0,
+ optimize=True,
+ disposal=2,
+ )
+ return os.path.getsize(path)
+
+
+@dataclass
+class MapView:
+ resolution: float
+ origin: tuple[float, float]
+ width: int
+ height: int
+ scale: int = 3
+
+
+def render_map(
+ grid: np.ndarray,
+ view: MapView,
+ trajectory: list[tuple[float, float]],
+ car_pose: tuple[float, float, float],
+ scan_points: np.ndarray | None = None,
+ title: str = "",
+) -> np.ndarray:
+ """Render an occupancy grid + trajectory + car + scan into an RGB image."""
+ # occupancy colours: free = white, unknown = grey, occupied = black
+ prob = 1.0 / (1.0 + np.exp(-grid))
+ img = np.full((view.height, view.width, 3), 128, dtype=np.uint8)
+ free = prob < 0.40
+ occ = prob > 0.60
+ img[free] = (248, 248, 248)
+ img[occ] = (25, 25, 28)
+
+ pil = Image.fromarray(img).resize((view.width * view.scale, view.height * view.scale), Image.NEAREST)
+ draw = ImageDraw.Draw(pil)
+
+ def to_px(x: float, y: float):
+ px = (x - view.origin[0]) / view.resolution
+ py = (y - view.origin[1]) / view.resolution
+ return px * view.scale, (view.height - 1 - py) * view.scale
+
+ if len(trajectory) > 1:
+ pts = [to_px(x, y) for x, y in trajectory]
+ draw.line(pts, fill=(0, 200, 255), width=2)
+ if scan_points is not None and len(scan_points):
+ for x, y in scan_points[::2]:
+ px, py = to_px(x, y)
+ draw.ellipse([px - 2, py - 2, px + 2, py + 2], fill=(255, 90, 60))
+ x, y, yaw = car_pose
+ px, py = to_px(x, y)
+ r = 7
+ draw.ellipse([px - r, py - r, px + r, py + r], fill=(80, 160, 255), outline=(255, 255, 255))
+ draw.line([px, py, px + r * np.cos(yaw), py - r * np.sin(yaw)], fill=(255, 255, 255), width=2)
+ if title:
+ draw.text((10, 8), title, font=_font(16), fill=(255, 255, 255))
+ return np.asarray(pil)
diff --git a/tests/test_mujoco_car_nn_ros_nodes.py b/tests/test_mujoco_car_nn_ros_nodes.py
new file mode 100644
index 0000000000..687aaafa3f
--- /dev/null
+++ b/tests/test_mujoco_car_nn_ros_nodes.py
@@ -0,0 +1,292 @@
+"""Runtime smoke test of the ROS 1 / ROS 2 nodes without a ROS installation.
+
+The node classes are executed against minimal stubs of ``rospy``/``rclpy``
+and the message packages. This catches attribute typos, wrong message
+fields and control-loop mistakes on a machine that has no ROS, which is
+exactly the situation of the CI runner. Run with::
+
+ python -m pytest tests/test_ros_nodes.py -q
+"""
+
+import importlib.util
+import os
+import sys
+import types
+
+import numpy as np
+
+ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+sys.path.insert(0, os.path.join(ROOT, "src"))
+
+
+class Msg:
+ """Permissive stand-in for a ROS message: any attribute can be set or read."""
+
+ def __init__(self, **kwargs):
+ for k, v in kwargs.items():
+ setattr(self, k, v)
+
+ def __getattr__(self, name):
+ if name.startswith("__"):
+ raise AttributeError(name)
+ child = Msg()
+ object.__setattr__(self, name, child)
+ return child
+
+
+def _msg_class(name):
+ return type(name, (Msg,), {})
+
+
+class _Publisher:
+ def __init__(self, *args, **kwargs):
+ self.messages = []
+
+ def publish(self, msg):
+ self.messages.append(msg)
+
+
+class _Subscriber:
+ def __init__(self, topic, msg_type, callback, **kwargs):
+ self.topic = topic
+ self.callback = callback
+
+
+class _Rate:
+ def __init__(self, hz):
+ self.hz = hz
+
+ def sleep(self):
+ pass
+
+
+def _install_ros1_stubs():
+ rospy = types.ModuleType("rospy")
+ params = {"~scene": "day", "~policy": "", "~offset": 0.0, "~rate": 20.0, "~seed": 0}
+
+ rospy.init_node = lambda *a, **k: None
+ rospy.get_param = lambda name, default=None: params.get(name, default)
+ rospy.loginfo = lambda *a, **k: None
+ rospy.Publisher = _Publisher
+ rospy.Subscriber = _Subscriber
+ rospy.Rate = _Rate
+ rospy.is_shutdown = lambda: True
+ rospy.Time = types.SimpleNamespace(now=lambda: 0.0)
+ rospy.ROSInterruptException = RuntimeError
+
+ geometry_msgs = types.ModuleType("geometry_msgs")
+ geometry_msgs_msg = types.ModuleType("geometry_msgs.msg")
+ geometry_msgs_msg.Twist = _msg_class("Twist")
+ geometry_msgs_msg.TransformStamped = _msg_class("TransformStamped")
+ geometry_msgs.msg = geometry_msgs_msg
+
+ nav_msgs = types.ModuleType("nav_msgs")
+ nav_msgs_msg = types.ModuleType("nav_msgs.msg")
+ nav_msgs_msg.Odometry = _msg_class("Odometry")
+ nav_msgs_msg.OccupancyGrid = _msg_class("OccupancyGrid")
+ nav_msgs.msg = nav_msgs_msg
+
+ sensor_msgs = types.ModuleType("sensor_msgs")
+ sensor_msgs_msg = types.ModuleType("sensor_msgs.msg")
+ sensor_msgs_msg.Image = _msg_class("Image")
+ sensor_msgs_msg.LaserScan = _msg_class("LaserScan")
+ sensor_msgs.msg = sensor_msgs_msg
+
+ tf2_ros = types.ModuleType("tf2_ros")
+
+ class _Broadcaster:
+ def __init__(self, *a, **k):
+ self.sent = []
+
+ def sendTransform(self, t):
+ self.sent.append(t)
+
+ tf2_ros.TransformBroadcaster = _Broadcaster
+
+ return {
+ "rospy": rospy,
+ "geometry_msgs": geometry_msgs,
+ "geometry_msgs.msg": geometry_msgs_msg,
+ "nav_msgs": nav_msgs,
+ "nav_msgs.msg": nav_msgs_msg,
+ "sensor_msgs": sensor_msgs,
+ "sensor_msgs.msg": sensor_msgs_msg,
+ "tf2_ros": tf2_ros,
+ }
+
+
+def _load(path, name):
+ spec = importlib.util.spec_from_file_location(name, path)
+ module = importlib.util.module_from_spec(spec)
+ spec.loader.exec_module(module)
+ return module
+
+
+def _with_stubs(stubs, fn):
+ saved = {k: sys.modules.get(k) for k in stubs}
+ sys.modules.update(stubs)
+ try:
+ return fn()
+ finally:
+ for k, v in saved.items():
+ if v is None:
+ sys.modules.pop(k, None)
+ else:
+ sys.modules[k] = v
+
+
+def test_ros1_sim_node_runs():
+ stubs = _install_ros1_stubs()
+ path = os.path.join(ROOT, "ros1", "car_nn_sim", "scripts", "sim_node.py")
+
+ def run():
+ module = _load(path, "ros1_sim_node")
+ node = module.CarSimNode()
+ idle = module.Twist()
+ idle.angular.z = 0.0
+ idle.linear.x = 0.0
+ node.on_cmd(idle)
+ for _ in range(3):
+ node.step()
+ assert len(node.pub_img.messages) == 3
+ assert len(node.pub_scan.messages) == 3
+ assert len(node.pub_odom.messages) == 3
+ img = node.pub_img.messages[-1]
+ assert (img.height, img.width, img.encoding) == (64, 64, "rgb8")
+ assert len(img.data) == 64 * 64 * 3
+ scan = node.pub_scan.messages[-1]
+ assert len(scan.ranges) == 64
+ assert scan.range_max == module.LIDAR_RANGE
+ odom = node.pub_odom.messages[-1]
+ assert odom.header.frame_id == "odom"
+ assert abs(odom.pose.pose.orientation.w) > 0.0
+ # a teleop command must actually move the car
+ cmd = module.Twist()
+ cmd.angular.z = 0.0
+ cmd.linear.x = 6.0
+ node.on_cmd(cmd)
+ for _ in range(20):
+ node.step()
+ assert node.st.speed > 0.5, "teleop command should accelerate the car"
+ node.sim.close()
+
+ _with_stubs(stubs, run)
+
+
+def test_ros2_sim_node_runs():
+ rclpy = types.ModuleType("rclpy")
+ rclpy_node = types.ModuleType("rclpy.node")
+ rclpy_qos = types.ModuleType("rclpy.qos")
+
+ class _Param:
+ def __init__(self, value):
+ self.value = value
+
+ params = {"scene": "day", "policy": "", "offset": 0.0, "rate": 20.0}
+
+ class Node:
+ def __init__(self, name):
+ self.name = name
+ self.publishers = []
+ self.timers = []
+
+ def declare_parameter(self, name, value=None):
+ params.setdefault(name, value)
+
+ def get_parameter(self, name):
+ return _Param(params.get(name))
+
+ def get_logger(self):
+ return types.SimpleNamespace(info=lambda *a, **k: None)
+
+ def create_publisher(self, msg_type, topic, qos):
+ pub = _Publisher()
+ self.publishers.append((topic, pub))
+ return pub
+
+ def create_subscription(self, topic, msg_type, callback, qos):
+ self.subscription = (topic, callback)
+ return None
+
+ def create_timer(self, period, callback):
+ self.timers.append(callback)
+ return None
+
+ def get_clock(self):
+ return types.SimpleNamespace(now=lambda: types.SimpleNamespace(to_msg=lambda: 0.0))
+
+ def destroy_node(self):
+ pass
+
+ rclpy_node.Node = Node
+ rclpy.node = rclpy_node
+ rclpy.init = lambda *a, **k: None
+ rclpy.spin = lambda node: None
+ rclpy.shutdown = lambda: None
+
+ rclpy_qos.QoSProfile = lambda **k: None
+ rclpy_qos.DurabilityPolicy = types.SimpleNamespace(TRANSIENT_LOCAL=1)
+
+ sensor_msgs = types.ModuleType("sensor_msgs")
+ sensor_msgs_msg = types.ModuleType("sensor_msgs.msg")
+ sensor_msgs_msg.Image = _msg_class("Image")
+ sensor_msgs_msg.LaserScan = _msg_class("LaserScan")
+ sensor_msgs.msg = sensor_msgs_msg
+
+ nav_msgs = types.ModuleType("nav_msgs")
+ nav_msgs_msg = types.ModuleType("nav_msgs.msg")
+ nav_msgs_msg.Odometry = _msg_class("Odometry")
+ nav_msgs_msg.OccupancyGrid = _msg_class("OccupancyGrid")
+ nav_msgs.msg = nav_msgs_msg
+
+ geometry_msgs = types.ModuleType("geometry_msgs")
+ geometry_msgs_msg = types.ModuleType("geometry_msgs.msg")
+ geometry_msgs_msg.Twist = _msg_class("Twist")
+ geometry_msgs_msg.TransformStamped = _msg_class("TransformStamped")
+ geometry_msgs.msg = geometry_msgs_msg
+
+ tf2_ros = types.ModuleType("tf2_ros")
+
+ class _Broadcaster:
+ def __init__(self, *a, **k):
+ self.sent = []
+
+ def sendTransform(self, t):
+ self.sent.append(t)
+
+ tf2_ros.TransformBroadcaster = _Broadcaster
+
+ stubs = {
+ "rclpy": rclpy,
+ "rclpy.node": rclpy_node,
+ "rclpy.qos": rclpy_qos,
+ "sensor_msgs": sensor_msgs,
+ "sensor_msgs.msg": sensor_msgs_msg,
+ "nav_msgs": nav_msgs,
+ "nav_msgs.msg": nav_msgs_msg,
+ "geometry_msgs": geometry_msgs,
+ "geometry_msgs.msg": geometry_msgs_msg,
+ "tf2_ros": tf2_ros,
+ }
+ path = os.path.join(ROOT, "ros2", "car_nn_sim", "car_nn_sim", "sim_node.py")
+
+ def run():
+ module = _load(path, "ros2_sim_node")
+ node = module.CarSimNode()
+ assert len(node.publishers) == 3
+ for _ in range(3):
+ node.step()
+ topics = [t for t, _ in node.publishers]
+ assert "/car/camera/image_raw" in topics and "/car/scan" in topics and "/car/odom" in topics
+ img = node.publishers[0][1].messages[-1]
+ assert len(img.data) == 64 * 64 * 3
+ node.sim.close()
+
+ _with_stubs(stubs, run)
+
+
+if __name__ == "__main__":
+ test_ros1_sim_node_runs()
+ print("ok ros1")
+ test_ros2_sim_node_runs()
+ print("ok ros2")
diff --git a/tests/test_mujoco_car_nn_smoke.py b/tests/test_mujoco_car_nn_smoke.py
new file mode 100644
index 0000000000..55a96e380b
--- /dev/null
+++ b/tests/test_mujoco_car_nn_smoke.py
@@ -0,0 +1,67 @@
+"""Smoke tests: run with ``python -m pytest tests -q`` or ``python tests/test_smoke.py``."""
+
+import os
+import sys
+
+import numpy as np
+
+sys.path.insert(0, os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "src"))
+
+from mujoco_car_nn.expert import PurePursuitExpert # noqa: E402
+from mujoco_car_nn.models import build_model # noqa: E402
+from mujoco_car_nn.sim import IMAGE_SIZE, LIDAR_BEAMS, CarSim, SimConfig # noqa: E402
+from mujoco_car_nn.slam import GridMap # noqa: E402
+from mujoco_car_nn.track import build_track # noqa: E402
+
+
+def test_track_closed_and_smooth():
+ track = build_track()
+ assert 150 < track.length < 350
+ # curve closes on itself
+ assert np.linalg.norm(track.points[0] - track.points[-1]) < 5.0
+ # minimum turning radius is drivable for a 9 m wide road
+ assert 1.0 / np.abs(track.curvature).max() > 9.0
+ # arc length is monotonic
+ assert np.all(np.diff(track.s) > 0)
+
+
+def test_sim_reset_and_step():
+ sim = CarSim(build_track(), SimConfig(scene="day", randomize_start=False))
+ st = sim.reset(0.0)
+ assert abs(st.lateral) < 1e-6
+ expert = PurePursuitExpert(sim.track)
+ for _ in range(40):
+ st = sim.step(expert.act(st))
+ assert st.speed > 1.0, "expert should accelerate the car"
+ img = sim.camera_image()
+ assert img.shape == (IMAGE_SIZE, IMAGE_SIZE, 3)
+ scan = sim.lidar()
+ assert scan.shape == (LIDAR_BEAMS,)
+ assert scan.min() < 10.0, "lidar should see the walls"
+ sim.close()
+
+
+def test_policy_shapes_and_range():
+ import torch
+
+ for name in ("cnn", "mlp"):
+ net = build_model(name)
+ out = net(torch.zeros(2, 3, 64, 64), torch.zeros(2, 1))
+ assert out.shape == (2, 2)
+ assert torch.all(out.abs() <= 1.0 + 1e-6)
+
+
+def test_grid_map_update():
+ grid = GridMap()
+ ranges = np.full(64, 5.0, dtype=np.float32)
+ angles = np.linspace(-np.pi, np.pi, 64, endpoint=False)
+ grid.update((0.0, 0.0, 0.0), ranges, angles)
+ assert (grid.log_odds > 0).sum() > 0, "endpoints must be marked occupied"
+ assert (grid.log_odds < 0).sum() > 0, "rays must clear free space"
+
+
+if __name__ == "__main__":
+ for fn in [v for k, v in sorted(globals().items()) if k.startswith("test_")]:
+ fn()
+ print(f"ok {fn.__name__}")
+ print("all tests passed")