sl-perception created pull request seb/saltylab-firmware#649

sl-perception
Tue, 17 Mar 2026 09:54:50 -0400

649#feat: VESC CAN odometry (Issue #646)#

Summary

Implements differential drive odometry from dual VESC CAN motors (CAN IDs 61/79) for Nav2.

New: diff_drive_odom.py

Pure-Python kinematics engine, no ROS2 deps

eRPM → mechanical RPM (÷ motor_poles) → wheel surface velocity

Exact arc integration to minimise heading error accumulation

invert_right flag for physically-mirrored motor mounting

Heading normalised to (−π, π] after every step

New: vesc_odometry_bridge.py (replaces single-motor version)

VESCCANOdometryNode subscribes to /vesc/can_61/state and /vesc/can_79/state (std_msgs/String JSON)

Publishes /odom (nav_msgs/Odometry) for Nav2 / slam_toolbox

Publishes /saltybot/wheel_odom for saltybot_pose_fusion EKF

Broadcasts TF odom → base_link

Full 6×6 pose + twist covariance matrices (tunable via params)

New: config/vesc_odometry_params.yaml

Configurable wheel_radius, wheel_separation, motor_poles, invert_right

Updated: pose_fusion_node.py

Optional wheel_odom_topic subscriber → feeds EKF via update_vo_velocity

Configurable sigmas: sigma_wheel_vel_m_s, sigma_wheel_omega_r_s

Tests: test/test_vesc_odometry.py

20 unit tests, all passing (no ROS2 required)

Covers: straight, arc, spin-in-place, full rotation, invert_right, guard conditions, heading wrap

Test plan

All 20 unit tests pass: pytest jetson/ros2_ws/src/saltybot_nav2_slam/test/test_vesc_odometry.py -v

Launch with odometry_bridge.launch.py and verify /odom publishes at 50Hz

Confirm /saltybot/wheel_odom received by pose_fusion node

Drive straight: x increases, y ≈ 0, theta ≈ 0

Drive arc: verify heading matches expected turn angle

ros2 run tf2_ros tf2_echo odom base_link confirms TF

🤖 Generated with Claude Code