Refactor perception and integrate costmap, planner, and control modules

.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
+[Demo Video](assets/demos/demo_nov30.mp4)
+![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

docker/gazebo/gazeboserver.Dockerfile

@@ -5,7 +5,7 @@ FROM ${BASE_IMAGE} AS source
 
 WORKDIR ${AMENT_WS}/src
 
-# Copy in source code
+# Copy in source code 
 COPY src/gazebo gazebo
 
 RUN apt-get update && apt-get install -y --no-install-recommends curl \
@@ -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
@@ -63,7 +63,7 @@ RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" && \
     colcon build \
         --cmake-args -DCMAKE_BUILD_TYPE=Release --install-base "${WATONOMOUS_INSTALL}"
 
-# Source and Build Artifact Cleanup
+# Source and Build Artifact Cleanup 
 RUN rm -rf src/* build/* devel/* install/* log/*
 
 # Entrypoint will run before any CMD on launch. Sources ~/opt/<ROS_DISTRO>/setup.bash and ~/ament_ws/install/setup.bash

docker/robot/robot.Dockerfile

@@ -9,23 +9,24 @@ RUN apt-get update && apt-get install -y --no-install-recommends curl \
  && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \
  && 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 in source code 
 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 +51,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
 
@@ -89,7 +90,7 @@ RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" && \
     colcon build \
     --cmake-args -DCMAKE_BUILD_TYPE=Release --install-base "${WATONOMOUS_INSTALL}"
 
-# Source and Build Artifact Cleanup
+# Source and Build Artifact Cleanup 
 RUN rm -rf src/* build/* devel/* install/* log/*
 
 # Entrypoint will run before any CMD on launch. Sources ~/opt/<ROS_DISTRO>/setup.bash and ~/ament_ws/install/setup.bash

docker/vis_tools/foxglove.Dockerfile

@@ -5,7 +5,7 @@ FROM ${BASE_IMAGE} AS source
 
 WORKDIR ${AMENT_WS}/src
 
-# Copy in source code
+# Copy in source code 
 COPY src/wato_msgs wato_msgs
 
 RUN apt-get update && apt-get install -y --no-install-recommends curl \
@@ -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 \
@@ -70,7 +69,7 @@ RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" && \
     colcon build \
         --cmake-args -DCMAKE_BUILD_TYPE=Release --install-base "${WATONOMOUS_INSTALL}"
 
-# Source and Build Artifact Cleanup
+# Source and Build Artifact Cleanup 
 RUN rm -rf src/* build/* devel/* install/* log/*
 
 # Entrypoint will run before any CMD on launch. Sources ~/opt/<ROS_DISTRO>/setup.bash and ~/ament_ws/install/setup.bash

modules/docker-compose.robot.yaml

@@ -1,5 +1,5 @@
 services:
-  robot:
+  robot: 
     build: &robot_build
       context: ..
       dockerfile: docker/robot/robot.Dockerfile
@@ -12,12 +12,7 @@ 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
     image: "${ROBOT_IMAGE:?}:dev_${TAG}"
@@ -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