Refactor perception and integrate costmap, planner, and control modules (#39)

* refactor perception and integrate costmap, planner, and control modules

* Update readme

* Change demo video to link

* add demo screenshot

* Add demo screenshot to README

* commit pre-commit changes

* Add demo video as github embed

* Update yamllint config

* fix cpp pre-commit hook issues

Author: bluebarryz

.gitignore

@@ -6,3 +6,6 @@ __pycache__
 watod-config.local.sh
 **/.DS_Store
 draft_*
+*copy*
+camera-info.txt
+src/gazebo/launch/MR24-DT-A00.00 - URDF/

README.md

@@ -8,3 +8,62 @@ These steps are to setup the repo to work on your own PC. We utilize docker to e
 1. This repo is supported on Linux Ubuntu >= 22.04, Windows (WSL), and MacOS. This is standard practice that roboticists can't get around. To setup, you can either setup an [Ubuntu Virtual Machine](https://ubuntu.com/tutorials/how-to-run-ubuntu-desktop-on-a-virtual-machine-using-virtualbox#1-overview), setting up [WSL](https://learn.microsoft.com/en-us/windows/wsl/install), or setting up your computer to [dual boot](https://opensource.com/article/18/5/dual-boot-linux). You can find online resources for all three approaches.
 2. Once inside Linux, [Download Docker Engine using the `apt` repository](https://docs.docker.com/engine/install/ubuntu/#install-using-the-repository)
 3. You're all set! You can visit the [WATO Wiki](https://wiki.watonomous.ca/autonomous_software_general/monorepo_infrastructure) for more documentation on the WATO infrastructure.
+
+## Demo
+https://github.com/user-attachments/assets/d4e821cd-8db7-4176-9e2e-1d9c62aab23d
+
+![Demo Screenshot](assets/demos/demo_nov30.png)
+
+## Simulation Environment
+
+We use Ignition Gazebo for physics simulation. The main world file is `robot_env.sdf`, which defines the entire simulation scene.
+
+### Robot Model (SDF)
+A basic model of the rover is defined directly in the SDF with a differential drive base. Key components:
+- **Chassis**: 2.0m x 1.0m x 0.5m box with inertial properties
+- **Wheels**: Four cylindrical wheels with revolute joints
+- **IMU**: Mounted on chassis, publishes to `/imu`
+- **RGBD Camera**: Simulated RealSense D435 mounted on the front
+
+### Depth Camera Simulation
+The depth camera is configured as an `rgbd_camera` sensor with realistic FOV, intrinsics, etc.
+
+It publishes:
+- `/camera/image` ([Image](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Image.html)) - RGB image (used by YOLO object detector)
+- `/camera/points` ([PointCloud2](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/PointCloud2.html)) - 3D point cloud (used by costmap)
+
+### Mars Environment (URDF)
+The environment is loaded from `mars_env.urdf`, which includes meshes for the mars terrain, mallet, and water bottle.
+
+The terrain has collision geometry so the rover can't drive through obstacles.
+
+### ROS2 Bridge
+The `ros_gz_bridge` translates Ignition messages to ROS2 topics, bridging `/cmd_vel`, `/imu`, camera streams, and TF transforms. This way, our ROS2 autonomy stack interface seamlessly with the Gazebo sim.
+
+## Autonomy Architecture
+
+### Sensor Simulation
+- **gps_sim**: Simulates GPS measurements by reading ground truth transforms from the simulator and injecting realistic noise and dropouts. Publishes [NavSatFix](https://docs.ros.org/en/kinetic/api/sensor_msgs/html/msg/NavSatFix.html) messages.
+- **imu_sim**: Adds sensor noise and bias to clean IMU data to mimic real-world sensors. Takes ground truth IMU and outputs noisy orientation and angular velocity.
+
+### Localization
+- **localization**: Runs an Extended Kalman Filter (EKF) to fuse (simulated) noisy GPS and IMU data into a smooth, filtered odometry estimate. Outputs robot pose for downstream modules.
+
+### Perception & Mapping
+- **costmap**: Converts RGBD camera point clouds into a local 2D occupancy grid centered on the robot. Marks obstacles and inflates them for safety margins.
+- **map_memory**: Stitches together local costmaps into a persistent global map as the rover explores. Only updates when the robot moves beyond a threshold distance to reduce computational load.
+- **yolo_inference**: Runs YOLOv8 object detection (ONNX runtime) on camera images to detect objects like bottles and mallets. Publishes annotated images with bounding boxes.
+
+### Planning & Control
+- **planner**: Implements A* search on the global occupancy grid. Takes the current robot pose and a goal point, outputs a collision-free path. Replans when new goals arrive.
+- **control**: Pure pursuit controller that tracks the planned path. Uses a PID loop (configurable Kp, Ki, Kd gains) to compute steering corrections based on cross-track error. Outputs angular velocity to steer toward the target waypoint while maintaining constant forward velocity, publishing [Twist](https://docs.ros.org/en/jade/api/geometry_msgs/html/msg/Twist.html) commands to `/cmd_vel`.
+
+### Data Flow
+The pipeline runs as follows:
+- Sensor sims generate noisy data
+- Localization fuses it into an estimate of the robot's position (odometry)
+- Costmap builds local obstacles
+- Map memory accumulates into global map
+- Planner finds path to goal
+- Control executes path
+- Motor commands actuate the rover

assets/demos/demo_nov30.mp4

@@ -15,10 +15,10 @@ RUN apt-get update && apt-get install -y --no-install-recommends curl \
 # Scan for rosdeps
 SHELL ["/bin/bash", "-o", "pipefail", "-c"]
 RUN apt-get -qq update && rosdep update && \
-    (rosdep install --from-paths . --ignore-src -r -s \
-        | grep 'apt-get install' \
+    (rosdep install --from-paths . --ignore-src -r -s || true) \
+        | (grep 'apt-get install' || true) \
         | awk '{print $3}' \
-        | sort  > /tmp/colcon_install_list || echo "# No additional dependencies needed" > /tmp/colcon_install_list)
+        | sort  > /tmp/colcon_install_list
 
 ################################# Dependencies ################################
 FROM ${BASE_IMAGE} AS dependencies

assets/demos/demo_nov30.png

@@ -10,22 +10,25 @@ RUN apt-get update && apt-get install -y --no-install-recommends curl \
  && rm -rf /var/lib/apt/lists/*
 
 # Copy in source code
-COPY src/robot/yolo_inference ./yolo_inference
-COPY src/robot/object_detection object_detection
+# COPY src/robot/yolo_inference ./yolo_inference
+# COPY src/robot/object_detection object_detection
 COPY src/robot/odometry_spoof odometry_spoof
+COPY src/robot/gps_sim gps_sim
+COPY src/robot/imu_sim imu_sim
+COPY src/robot/localization localization
+COPY src/robot/costmap costmap
+COPY src/robot/map_memory map_memory
+COPY src/robot/planner planner
+COPY src/robot/control control
 COPY src/robot/bringup_robot bringup_robot
-COPY src/robot/camera_fallback camera_fallback
-COPY src/robot/arcade_driver arcade_driver
-COPY src/robot/motor_speed_controller motor_speed_controller
-COPY src/wato_msgs/drivetrain_msgs drivetrain_msgs
 
 # Scan for rosdeps
 SHELL ["/bin/bash", "-o", "pipefail", "-c"]
 RUN apt-get -qq update && rosdep update && \
-    (rosdep install --from-paths . --ignore-src -r -s \
-    | grep 'apt-get install' \
-    | awk '{print $3}' \
-    | sort  > /tmp/colcon_install_list || echo "# No additional dependencies needed" > /tmp/colcon_install_list)
+    (rosdep install --from-paths . --ignore-src -r -s || true) \
+    | (grep 'apt-get install' || true) \
+        | awk '{print $3}' \
+    | sort  > /tmp/colcon_install_list || echo "# No additional dependencies needed" > /tmp/colcon_install_list
 
 ################################# Dependencies ################################
 FROM ${BASE_IMAGE} AS dependencies
@@ -50,8 +53,8 @@ RUN apt-get -qq update && \
     rm -rf /var/lib/apt/lists/*
 
 # Copy in source code from source stage
-WORKDIR ${AMENT_WS}/src
-COPY src/robot/object_detection object_detection
+# WORKDIR ${AMENT_WS}/src
+# COPY src/robot/object_detection object_detection
 WORKDIR ${AMENT_WS}
 COPY --from=source ${AMENT_WS}/src src
 

docker/gazebo/gazeboserver.Dockerfile

@@ -15,10 +15,10 @@ RUN apt-get update && apt-get install -y --no-install-recommends curl \
 # Scan for rosdeps
 SHELL ["/bin/bash", "-o", "pipefail", "-c"]
 RUN apt-get -qq update && rosdep update && \
-    (rosdep install --from-paths . --ignore-src -r -s \
-        | grep 'apt-get install' \
+    (rosdep install --from-paths . --ignore-src -r -s || true) \
+        | (grep 'apt-get install' || true) \
         | awk '{print $3}' \
-        | sort  > /tmp/colcon_install_list || echo "# No additional dependencies needed" > /tmp/colcon_install_list)
+        | sort  > /tmp/colcon_install_list
 
 ################################# Dependencies ################################
 FROM ${BASE_IMAGE} AS dependencies
@@ -36,7 +36,6 @@ RUN apt-get update && apt-get install -y --no-install-recommends lsb-release sof
     apt-add-repository universe && \
     rm -rf /var/lib/apt/lists/*
 
-# Install Dependencies
 # Install Dependencies
 RUN apt-get update && \
     apt-get install -y --no-install-recommends \

docker/robot/robot.Dockerfile

@@ -0,0 +1,65 @@
+commit 0bb5293d52599317a3e300c55897e34a48e5e289
+Author: Barry Zhang <45883553+bluebarryz@users.noreply.github.com>
+Date:   Wed Jan 7 19:46:46 2026 -0500
+
+    refactor perception and integrate costmap, planner, and control modules
+
+ .gitignore                                         |   3 +
+ docker/gazebo/gazeboserver.Dockerfile              |  10 +-
+ docker/robot/robot.Dockerfile                      |  29 +-
+ docker/vis_tools/foxglove.Dockerfile               |  11 +-
+ modules/docker-compose.robot.yaml                  |  13 +-
+ src/gazebo/launch/env.urdf                         | 357 ++++++-----
+ src/gazebo/launch/mr24/robot.urdf                  | 365 +++++++++++
+ src/gazebo/launch/robot_env.sdf                    | 668 +++++++++------------
+ src/gazebo/launch/sim.launch.py                    |  60 +-
+ src/robot/bringup_robot/launch/robot.launch.py     | 206 +++++--
+ src/robot/bringup_robot/package.xml                |  14 +-
+ src/robot/control/CMakeLists.txt                   |   7 +-
+ src/robot/control/config/params.yaml               |  14 +-
+ src/robot/control/include/control_core.hpp         |  47 ++
+ src/robot/control/include/control_node.hpp         |  63 ++
+ src/robot/control/package.xml                      |   3 +
+ src/robot/control/src/control_core.cpp             | 146 ++++-
+ src/robot/control/src/control_node.cpp             | 101 +++-
+ src/robot/costmap/CMakeLists.txt                   |   5 +-
+ src/robot/costmap/config/params.yaml               |  20 +-
+ src/robot/costmap/include/costmap_core.hpp         |  50 ++
+ src/robot/costmap/include/costmap_node.hpp         |  46 ++
+ src/robot/costmap/package.xml                      |   3 +
+ src/robot/costmap/src/costmap_core.cpp             | 170 +++++-
+ src/robot/costmap/src/costmap_node.cpp             |  88 ++-
+ src/robot/gps_sim/CMakeLists.txt                   |  47 ++
+ src/robot/gps_sim/config/params.yaml               |  13 +
+ src/robot/gps_sim/include/gps_sim_node.hpp         |  46 ++
+ src/robot/gps_sim/package.xml                      |  28 +
+ src/robot/gps_sim/src/gps_sim_node.cpp             | 134 +++++
+ src/robot/imu_sim/CMakeLists.txt                   |  41 ++
+ src/robot/imu_sim/config/params.yaml               |  18 +
+ src/robot/imu_sim/include/imu_sim_node.hpp         |  53 ++
+ src/robot/imu_sim/package.xml                      |  25 +
+ src/robot/imu_sim/src/imu_sim_node.cpp             | 171 ++++++
+ src/robot/localization/CMakeLists.txt              |  56 ++
+ src/robot/localization/config/params.yaml          |  25 +
+ .../localization/include/localization_core.hpp     | 106 ++++
+ .../localization/include/localization_node.hpp     |  73 +++
+ src/robot/localization/package.xml                 |  29 +
+ src/robot/localization/src/localization_core.cpp   | 501 ++++++++++++++++
+ src/robot/localization/src/localization_node.cpp   | 225 +++++++
+ src/robot/map_memory/CMakeLists.txt                |   8 +-
+ src/robot/map_memory/config/params.yaml            |  26 +-
+ src/robot/map_memory/include/map_memory_core.hpp   |  38 ++
+ src/robot/map_memory/include/map_memory_node.hpp   |  63 ++
+ src/robot/map_memory/package.xml                   |   3 +
+ src/robot/map_memory/src/map_memory_core.cpp       | 110 +++-
+ src/robot/map_memory/src/map_memory_node.cpp       | 141 ++++-
+ .../odometry_spoof/include/odometry_spoof.hpp      |  40 ++
+ src/robot/odometry_spoof/src/odometry_spoof.cpp    |  92 +--
+ src/robot/planner/CMakeLists.txt                   |   4 +-
+ src/robot/planner/config/params.yaml               |  16 +-
+ src/robot/planner/include/planner_core.hpp         | 125 +++-
+ src/robot/planner/include/planner_node.hpp         |  70 ++-
+ src/robot/planner/package.xml                      |   2 +
+ src/robot/planner/src/planner_core.cpp             | 262 +++++++-
+ src/robot/planner/src/planner_node.cpp             | 186 +++++-
+ 58 files changed, 4503 insertions(+), 773 deletions(-)

docker/vis_tools/foxglove.Dockerfile

@@ -12,11 +12,6 @@ services:
     profiles:
       - deploy
     command: /bin/bash -c "ros2 launch bringup_robot robot.launch.py"
-    devices:
-      - /dev/bus/usb:/dev/bus/usb
-    volumes:
-      - /dev:/dev
-    privileged: true
 
   robot_dev:
     build: *robot_build
@@ -26,7 +21,3 @@ services:
       - develop
     volumes:
       - ${MONO_DIR}/src/robot:/root/ament_ws/src
-      - /dev:/dev
-    devices:
-      - /dev/bus/usb:/dev/bus/usb
-    privileged: true