seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

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

Merged
sl-jetson merged 1 commits from sl-perception/issue-646-vesc-odometry into main 2026-03-18 07:55:18 -04:00
Conversation 0 Commits 1 Files Changed 1 +5 -5
sl-perception commented 2026-03-17 09:54:49 -04:00
Collaborator
Copy Link

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

## 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](https://claude.com/claude-code)
sl-perception added 1 commit 2026-03-17 09:54:49 -04:00 
Replace single-motor vesc_odometry_bridge with dual-CAN differential
drive odometry for left (CAN 61) and right (CAN 79) VESC motors.

New files:
- diff_drive_odom.py: pure-Python kinematics (eRPM→wheel vel, exact arc
  integration, heading wrap), no ROS deps, fully unit-tested
- test/test_vesc_odometry.py: 20 unit tests (straight, arc, spin,
  invert_right, guard conditions) — all pass
- config/vesc_odometry_params.yaml: configurable wheel_radius,
  wheel_separation, motor_poles, invert_right, covariance tuning

Updated:
- vesc_odometry_bridge.py: rewritten as VESCCANOdometryNode; subscribes
  to /vesc/can_61/state and /vesc/can_79/state (std_msgs/String JSON);
  publishes /odom and /saltybot/wheel_odom (nav_msgs/Odometry) + TF
  odom→base_link with proper 6×6 covariance matrices
- odometry_bridge.launch.py: updated to launch vesc_can_odometry with
  vesc_odometry_params.yaml
- setup.py: added vesc_can_odometry entry point + config install
- pose_fusion_node.py: added optional wheel_odom_topic subscriber that
  feeds DiffDriveOdometry velocities into EKF via update_vo_velocity
- pose_fusion_params.yaml: added use_wheel_odom, wheel_odom_topic,
  sigma_wheel_vel_m_s, sigma_wheel_omega_r_s parameters

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson added 2 commits 2026-03-17 15:20:10 -04:00 
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
- VESCCANOdometryNode subscriptions now use left_state_topic/right_state_topic
  params (defaulting to /vesc/left/state and /vesc/right/state) instead of
  building /vesc/can_<id>/state from CAN IDs — those topics never existed
- Update right_can_id default: 79 → 68 (Mamba F722S architecture update)
- Update vesc_odometry_params.yaml: CAN IDs 61/79 → 56/68; add explicit
  left_state_topic and right_state_topic entries; remove stale can_N comments
- All IDs remain fully configurable via ROS2 params

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson force-pushed sl-perception/issue-646-vesc-odometry from eb3c9d40cf to 06101371ff 2026-03-18 07:55:09 -04:00 Compare
sl-jetson merged commit 4d0a377cee into main 2026-03-18 07:55:18 -04:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
2 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#649
No description provided.
Delete Branch "sl-perception/issue-646-vesc-odometry"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?