Primary GPS + IMU: VN-300 Driver Support

.gitmodules

@@ -10,3 +10,6 @@
 [submodule "src/akros2_msgs"]
 	path = src/akros2_msgs
 	url = https://github.com/adityakamath/akros2_msgs.git
+[submodule "src/sensor_vn300"]
+	path = src/sensor_vn300
+	url = git@github.com:uwrobotics/VectorNav-ROS2-Mirror.git

Dockerfile

@@ -40,6 +40,7 @@ RUN apt-get update && apt-get install -y \
     ros-humble-joint-trajectory-controller \
     ros-humble-joy \
     ros-humble-teleop-twist-joy \
+    ros-humble-robot-localization \
     cppcheck \
     uncrustify \
     && if [ "$(dpkg --print-architecture)" != "arm64" ]; then \

NOTICE

@@ -25,4 +25,16 @@ src/akros2_msgs  (git submodule)
   License:  Apache License, Version 2.0
   Copyright (c) 2023 Aditya Kamath
   See: src/akros2_msgs/LICENSE
+
+--------------------------------------------------------------------------------
+src/sensor_vn300  (vectornav, git submodule)
+  Upstream: https://github.com/dawonn/vectornav
+  Fork:     https://github.com/uwrobotics/VectorNav-ROS2-Mirror
+  License:  MIT
+  Copyright (c) 2020 Dereck Wonnacott
+  See: src/sensor_vn300/LICENCE
+
+  Bundles the VectorNav programming library (vnproglib):
+    src/sensor_vn300/vectornav/vnproglib-1.2.0.0
+    Copyright (c) VectorNav Technologies, LLC
 --------------------------------------------------------------------------------

docker-compose.yml

@@ -40,6 +40,9 @@ services:
     # defaults to 994 (the usual Ubuntu 'input' GID).
     group_add:
       - "${INPUT_GID:-994}"
+      # Host 'dialout' group (GID 20) owns /dev/ttyUSB*; without it the non-root
+      # container user gets "Permission denied" opening the VN-300 serial port.
+      - "${DIALOUT_GID:-20}"
     network_mode: host
     stdin_open: true
     tty: true

src/sparky_description/config/sparky_controllers.yaml

@@ -54,7 +54,14 @@ diff_drive_controller:
 
     # Closed loop: integrate the wheel state fed back by the (mock) hardware.
     open_loop: false
-    enable_odom_tf: true
+    # odom->base_link TF is owned by robot_localization's ekf_local
+    # (sparky_localization), which fuses this wheel odom with the VN-300 IMU.
+    # Two nodes publishing the same transform is the #1 robot_localization bug,
+    # so diff_drive only publishes the /diff_drive_controller/odom TOPIC here.
+    # NOTE: if you run the drivetrain WITHOUT sparky_localization, RViz will have
+    # no odom->base_link -- either launch localization (use_gps:=false is fine)
+    # or temporarily set this back to true.
+    enable_odom_tf: false
 
     cmd_vel_timeout: 0.5
     # Subscribe to a plain Twist (geometry_msgs/Twist) instead of TwistStamped,

src/sparky_localization/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+project(sparky_localization)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/sparky_localization/README.md

@@ -0,0 +1,154 @@
+# sparky_localization
+
+State estimation for the Sparky rover using [`robot_localization`](https://github.com/cra-ros-pkg/robot_localization).
+Fuses wheel odometry (`diff_drive_controller`) with the VN-300 IMU/GNSS to
+produce the REP-105 TF tree `map -> odom -> base_link`.
+
+## Architecture
+
+Standard dual-EKF + navsat_transform pattern:
+
+```
+/diff_drive_controller/odom ─┐
+/vectornav/imu ──────────────┼─► ekf_local  ─► odom→base_link + /odometry/filtered/local
+                             │
+/vectornav/gnss ─► navsat_transform ─► /odometry/gps ─┐
+/vectornav/imu ──────────────────────────────────────┤
+/odometry/filtered/global ───────────────────────────┘
+/diff_drive_controller/odom ─┐
+/vectornav/imu ──────────────┼─► ekf_global ─► map→odom + /odometry/filtered/global
+/odometry/gps ───────────────┘
+```
+
+- **`ekf_local`** owns `odom → base_link` — continuous, smooth, **no GPS
+  dependency**. This is what compensates wheel-odometry heading drift with the
+  IMU. Used for control.
+- **`navsat_transform`** converts `NavSatFix` (lat/lon) into map-frame XY.
+- **`ekf_global`** owns `map → odom` — drift-corrected global pose for
+  navigation (Nav2 GPS waypoints).
+
+## Usage
+
+```bash
+# Full stack: local + global EKF + GPS
+ros2 launch sparky_localization localization.launch.py
+
+# Local EKF only — no GPS needed (indoors, sensor unplugged, vectornav not
+# built). Still produces odom→base_link and drives/visualizes fine.
+ros2 launch sparky_localization localization.launch.py use_gps:=false
+```
+
+Run alongside the drivetrain and the VN-300 driver:
+
+```bash
+ros2 launch drivetrain_bringup drivetrain_can_ps4_rviz.launch.py   # /diff_drive_controller/odom
+ros2 launch vectornav vectornav.launch.py                          # /vectornav/imu, /vectornav/gnss
+ros2 launch sparky_localization localization.launch.py
+```
+
+## Arguments
+
+| Argument       | Default | Purpose                                                        |
+|----------------|---------|----------------------------------------------------------------|
+| `use_gps`      | `true`  | `true`: dual-EKF + GPS (`map→odom`). `false`: local EKF only.  |
+| `use_sim_time` | `false` | Use `/clock` simulated time.                                   |
+
+## Testing without the drivetrain (mock drivetrain + real VN-300)
+
+You can validate plumbing and heading fusion at a desk: **mock** wheel odometry
+(faked from `/cmd_vel`) + the **real** VN-300 IMU you move by hand + the EKF, in
+RViz. No ODrive/CAN required.
+
+```bash
+# Terminal 1 — mock drivetrain + RViz (fixed frame is already 'odom')
+ros2 launch drivetrain_bringup drivetrain_rviz.launch.py
+
+# Terminal 2 — real VN-300 driver (needs /dev/ttyUSB0 + dialout access)
+ros2 launch vectornav vectornav.launch.py
+
+# Terminal 3 — localization. Indoors there is usually no GPS fix, so:
+ros2 launch sparky_localization localization.launch.py use_gps:=false
+```
+
+> Because `enable_odom_tf: false`, the mock drivetrain alone no longer publishes
+> `odom→base_link` — **the rover only appears in RViz once localization runs.**
+> That is the EKF now owning the transform.
+
+**Verify the plumbing:**
+
+```bash
+ros2 run tf2_tools view_frames              # odom -> base_link (+ map->odom if use_gps)
+ros2 topic hz /vectornav/imu                # sensor flowing
+ros2 topic hz /diff_drive_controller/odom   # ~50 Hz
+ros2 topic echo /odometry/filtered/local --once
+```
+
+**Heading-compensation test (the whole point):**
+
+1. RViz: Fixed Frame = `odom`, add a TF/Axes display for `base_link`.
+2. Keep `/cmd_vel` at zero, then **physically rotate the VN-300** — `base_link`
+   should rotate to match (yaw is IMU-driven). Confirm numerically:
+   `ros2 topic echo /odometry/filtered/local --field pose.pose.orientation`
+3. Drive in place — `ros2 topic pub /cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.3}}'`
+   — `base_link` translates in `odom` (mock wheel odom) while heading still
+   tracks the real IMU. That is the fusion working.
+
+**GPS test** (outdoors / with a fix only): launch with `use_gps:=true`, confirm
+`ros2 topic echo /vectornav/gnss` shows a fix, `/odometry/gps` appears, and
+`view_frames` shows `map -> odom`. Set RViz Fixed Frame = `map`.
+
+Caveat: mock odometry is "perfect" and integrates `/cmd_vel` only — it ignores
+the IMU for translation, so the wheel path won't curve when you twist the
+sensor. This test validates plumbing + heading, not full trajectory accuracy
+(that needs the real rover moving).
+
+## Topics
+
+| Topic                        | Type                     | Direction | Notes                          |
+|------------------------------|--------------------------|-----------|--------------------------------|
+| `/diff_drive_controller/odom`| `nav_msgs/Odometry`      | in        | wheel odometry                 |
+| `/vectornav/imu`             | `sensor_msgs/Imu`        | in        | VN-300 orientation + rates     |
+| `/vectornav/gnss`            | `sensor_msgs/NavSatFix`  | in        | VN-300 GNSS (GPS half only)    |
+| `/odometry/filtered/local`   | `nav_msgs/Odometry`      | out       | local EKF estimate             |
+| `/odometry/filtered/global`  | `nav_msgs/Odometry`      | out       | global EKF estimate            |
+| `/odometry/gps`              | `nav_msgs/Odometry`      | out       | GPS in map frame               |
+
+## GPS availability
+
+The stack degrades gracefully without GPS:
+
+- `use_gps:=false` → only `ekf_local` runs. Full `odom→base_link`, zero GPS
+  dependency, nothing crashes.
+- `use_gps:=true` but no fix / GNSS node down → `ekf_global` dead-reckons from
+  odom + IMU; `map→odom` simply stops being corrected. Still no crash.
+
+## Configuration
+
+All EKF and navsat parameters live in [`config/ekf.yaml`](config/ekf.yaml)
+(`two_d_mode: true` — Sparky drives on flat ground).
+
+### ⚠️ Must calibrate before trusting GPS waypoints
+
+1. **VN-300 mount transform.** The launch publishes a placeholder static TF
+   `base_link → vectornav` at `z=0.3`, no rotation. Measure the real sensor
+   pose and update it in [`launch/localization.launch.py`](launch/localization.launch.py)
+   — or add a link/joint to `sparky_description` and delete the static publisher.
+2. **`yaw_offset` / `magnetic_declination_radians`** in `config/ekf.yaml`. These
+   define which way "north" is for GPS. Wrong values make the rover drive off at
+   an angle to every waypoint. Verify by driving straight with a fix and
+   checking `/odometry/gps` heading matches reality.
+3. **IMU frame must be ENU (REP-103)**, not NED — required by
+   `robot_localization`. Confirm the VN-300 driver is configured for ENU output.
+
+## Dependencies
+
+Requires the `robot_localization` package (installed via the repo `Dockerfile`:
+`ros-humble-robot-localization`).
+
+## Related
+
+- Wheel odometry / `odom→base_link` handoff: `sparky_description`
+  (`sparky_controllers.yaml` has `enable_odom_tf: false` so the EKF owns that
+  transform — two nodes publishing it is the #1 `robot_localization` bug).
+- VN-300 driver: `sensor_vn300` (topics `/vectornav/imu`, `/vectornav/gnss`).
+- Next layer: Nav2 GPS waypoint following, which consumes `map→odom` from here.

src/sparky_localization/config/ekf.yaml

@@ -0,0 +1,129 @@
+# Sparky CIRC 2026 -- robot_localization configuration (dual-EKF + navsat)
+#
+# Frames follow REP-105:  map -> odom -> base_link
+#   ekf_local  owns  odom -> base_link   (continuous, GPS-free)
+#   ekf_global owns  map  -> odom        (drift-corrected, needs GPS)
+#
+# Sensor inputs:
+#   /diff_drive_controller/odom  (nav_msgs/Odometry)  wheel odometry
+#   /vectornav/imu               (sensor_msgs/Imu)    VN-300 orientation + rates
+#   /odometry/gps                (nav_msgs/Odometry)  from navsat_transform_node
+#
+# The _config matrices are 15 booleans in this order:
+#   [ x,     y,     z,
+#     roll,  pitch, yaw,
+#     vx,    vy,    vz,
+#     vroll, vpitch,vyaw,
+#     ax,    ay,    az ]
+#
+# two_d_mode: true  -- Sparky drives on (locally) flat ground, so we zero out
+# z/roll/pitch and their rates. This is the single biggest stability win for a
+# ground rover and is strongly recommended.
+
+# ---------------------------------------------------------------------------
+# LOCAL EKF  --  odom -> base_link   (no GPS; always runs)
+# ---------------------------------------------------------------------------
+ekf_local:
+  ros__parameters:
+    frequency: 30.0
+    sensor_timeout: 0.2
+    two_d_mode: true
+    publish_tf: true
+
+    map_frame: map
+    odom_frame: odom
+    base_link_frame: base_link
+    world_frame: odom          # <-- local EKF estimates the odom frame
+
+    # Wheel odometry: trust body-frame forward velocity + yaw rate only.
+    odom0: /diff_drive_controller/odom
+    odom0_config: [false, false, false,
+                   false, false, false,
+                   true,  false, false,
+                   false, false, true,
+                   false, false, false]
+    odom0_differential: false
+    odom0_queue_size: 10
+
+    # VN-300 IMU: absolute yaw + yaw rate (this is what compensates wheel-odom
+    # heading drift). Keep yaw absolute -- the VN-300 gives a true heading.
+    imu0: /vectornav/imu
+    imu0_config: [false, false, false,
+                  false, false, true,
+                  false, false, false,
+                  false, false, true,
+                  false, false, false]
+    imu0_differential: false
+    imu0_relative: true
+    imu0_remove_gravitational_acceleration: true
+    imu0_queue_size: 10
+
+# ---------------------------------------------------------------------------
+# GLOBAL EKF  --  map -> odom   (adds GPS; only when use_gps:=true)
+# ---------------------------------------------------------------------------
+ekf_global:
+  ros__parameters:
+    frequency: 30.0
+    sensor_timeout: 0.2
+    two_d_mode: true
+    publish_tf: true
+
+    map_frame: map
+    odom_frame: odom
+    base_link_frame: base_link
+    world_frame: map           # <-- global EKF estimates the map frame
+
+    odom0: /diff_drive_controller/odom
+    odom0_config: [false, false, false,
+                   false, false, false,
+                   true,  false, false,
+                   false, false, true,
+                   false, false, false]
+    odom0_differential: false
+    odom0_queue_size: 10
+
+    imu0: /vectornav/imu
+    imu0_config: [false, false, false,
+                  false, false, true,
+                  false, false, false,
+                  false, false, true,
+                  false, false, false]
+    imu0_differential: false
+    imu0_relative: true
+    imu0_remove_gravitational_acceleration: true
+    imu0_queue_size: 10
+
+    # GPS position (map-frame x,y) from navsat_transform_node. Absolute, so it
+    # corrects long-run drift in the map frame.
+    odom1: /odometry/gps
+    odom1_config: [true,  true,  false,
+                   false, false, false,
+                   false, false, false,
+                   false, false, false,
+                   false, false, false]
+    odom1_differential: false
+    odom1_queue_size: 10
+
+# ---------------------------------------------------------------------------
+# navsat_transform_node  --  NavSatFix (lat/lon) -> map-frame odometry
+# ---------------------------------------------------------------------------
+navsat_transform:
+  ros__parameters:
+    frequency: 30.0
+    delay: 3.0
+    # Set these for your competition location (Toronto/CIRC area shown).
+    # magnetic_declination: https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml
+    # If the VN-300 already reports TRUE heading, leave declination at 0.
+    magnetic_declination_radians: 0.0
+    # yaw_offset compensates the IMU's zero-heading convention. For a REP-103
+    # ENU IMU that reads 0 facing East, this is 0. VERIFY with the calibration
+    # procedure (see the package README) before trusting GPS waypoints.
+    yaw_offset: 0.0
+
+    zero_altitude: true
+    broadcast_cartesian_transform: false
+    publish_filtered_gps: true
+    # Use the IMU heading (VN-300 has a true dual-antenna heading) rather than
+    # deriving yaw from motion. More robust at low speed / standstill.
+    use_odometry_yaw: false
+    wait_for_datum: false