From ee16bae9fb279ca7f312dba24ba45e6347fbbdaf Mon Sep 17 00:00:00 2001 From: sl-controls Date: Tue, 17 Mar 2026 12:32:11 -0400 Subject: [PATCH] fix: Make VESC CAN IDs configurable, default 56/68 (Issue #667) FSESC 6.7 Pro Mini Dual uses CAN IDs 56/68, not 61/79. Updates all driver, telemetry, and odometry bridge files to use correct defaults. Co-Authored-By: Claude Sonnet 4.6 --- .../config/vesc_odometry_params.yaml | 2 +- .../vesc_odometry_bridge.py | 5 +-- .../config/vesc_params.yaml | 4 +-- .../saltybot_vesc_driver/vesc_driver_node.py | 4 +-- .../config/vesc_telemetry_params.yaml | 8 ++--- .../vesc_telemetry_node.py | 8 ++--- .../test/test_vesc_telemetry.py | 32 +++++++++---------- 7 files changed, 32 insertions(+), 31 deletions(-) diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/config/vesc_odometry_params.yaml b/jetson/ros2_ws/src/saltybot_nav2_slam/config/vesc_odometry_params.yaml index 78b0374..b66aafe 100644 --- a/jetson/ros2_ws/src/saltybot_nav2_slam/config/vesc_odometry_params.yaml +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/config/vesc_odometry_params.yaml @@ -2,7 +2,7 @@ vesc_can_odometry: ros__parameters: # ── CAN motor IDs (used for CAN addressing) ─────────────────────────────── left_can_id: 56 # left motor VESC CAN ID - right_can_id: 79 # right motor VESC CAN ID + right_can_id: 68 # right motor VESC CAN ID # ── State topic names (must match what telemetry publishes) ─────────────── left_state_topic: /vesc/left/state diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/vesc_odometry_bridge.py b/jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/vesc_odometry_bridge.py index 27dced5..e2d91cc 100644 --- a/jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/vesc_odometry_bridge.py +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/vesc_odometry_bridge.py @@ -3,7 +3,8 @@ vesc_odometry_bridge.py — Differential drive odometry from dual VESC CAN motors (Issue #646). Subscribes to per-motor VESC telemetry topics (configurable, defaulting to -/vesc/left/state and /vesc/right/state as published by the telemetry node), +/vesc/left/state and /vesc/right/state as published by the telemetry node) +for CAN IDs 56 (left) and 68 (right), applies differential drive kinematics via DiffDriveOdometry, and publishes: /odom nav_msgs/Odometry — consumed by Nav2 / slam_toolbox @@ -57,7 +58,7 @@ class VESCCANOdometryNode(Node): # ── Parameters ──────────────────────────────────────────────────────── self.declare_parameter("left_can_id", 56) # CAN ID for left motor - self.declare_parameter("right_can_id", 79) # CAN ID for right motor + self.declare_parameter("right_can_id", 68) # CAN ID for right motor # Configurable state topic names — default to the names telemetry publishes self.declare_parameter("left_state_topic", "/vesc/left/state") self.declare_parameter("right_state_topic", "/vesc/right/state") diff --git a/jetson/ros2_ws/src/saltybot_vesc_driver/config/vesc_params.yaml b/jetson/ros2_ws/src/saltybot_vesc_driver/config/vesc_params.yaml index 2b95821..0132279 100644 --- a/jetson/ros2_ws/src/saltybot_vesc_driver/config/vesc_params.yaml +++ b/jetson/ros2_ws/src/saltybot_vesc_driver/config/vesc_params.yaml @@ -2,8 +2,8 @@ vesc_can_driver: ros__parameters: interface: can0 bitrate: 500000 - left_motor_id: 61 - right_motor_id: 79 + left_motor_id: 56 + right_motor_id: 68 wheel_separation: 0.5 wheel_radius: 0.1 max_speed: 5.0 diff --git a/jetson/ros2_ws/src/saltybot_vesc_driver/saltybot_vesc_driver/vesc_driver_node.py b/jetson/ros2_ws/src/saltybot_vesc_driver/saltybot_vesc_driver/vesc_driver_node.py index d5ff4f6..9e05b51 100644 --- a/jetson/ros2_ws/src/saltybot_vesc_driver/saltybot_vesc_driver/vesc_driver_node.py +++ b/jetson/ros2_ws/src/saltybot_vesc_driver/saltybot_vesc_driver/vesc_driver_node.py @@ -95,8 +95,8 @@ class VescCanDriver(Node): # Declare parameters self.declare_parameter('interface', 'can0') - self.declare_parameter('left_motor_id', 61) - self.declare_parameter('right_motor_id', 79) + self.declare_parameter('left_motor_id', 56) + self.declare_parameter('right_motor_id', 68) self.declare_parameter('wheel_separation', 0.5) self.declare_parameter('wheel_radius', 0.1) self.declare_parameter('max_speed', 5.0) diff --git a/jetson/ros2_ws/src/saltybot_vesc_telemetry/config/vesc_telemetry_params.yaml b/jetson/ros2_ws/src/saltybot_vesc_telemetry/config/vesc_telemetry_params.yaml index 88dd6f7..2be902f 100644 --- a/jetson/ros2_ws/src/saltybot_vesc_telemetry/config/vesc_telemetry_params.yaml +++ b/jetson/ros2_ws/src/saltybot_vesc_telemetry/config/vesc_telemetry_params.yaml @@ -7,10 +7,10 @@ vesc_telemetry: can_interface: "can0" # CAN IDs assigned to each VESC in VESC Tool - # Left motor VESC: ID 61 (0x3D) - # Right motor VESC: ID 79 (0x4F) - left_can_id: 61 - right_can_id: 79 + # Left motor VESC: ID 56 (0x38) + # Right motor VESC: ID 68 (0x44) + left_can_id: 56 + right_can_id: 68 # This node's CAN ID used as sender_id in GET_VALUES requests (0-127) sender_can_id: 127 diff --git a/jetson/ros2_ws/src/saltybot_vesc_telemetry/saltybot_vesc_telemetry/vesc_telemetry_node.py b/jetson/ros2_ws/src/saltybot_vesc_telemetry/saltybot_vesc_telemetry/vesc_telemetry_node.py index f327451..24c52be 100644 --- a/jetson/ros2_ws/src/saltybot_vesc_telemetry/saltybot_vesc_telemetry/vesc_telemetry_node.py +++ b/jetson/ros2_ws/src/saltybot_vesc_telemetry/saltybot_vesc_telemetry/vesc_telemetry_node.py @@ -15,8 +15,8 @@ Published topics Parameters ---------- can_interface str 'can0' SocketCAN interface - left_can_id int 61 CAN ID of left VESC - right_can_id int 79 CAN ID of right VESC + left_can_id int 56 CAN ID of left VESC + right_can_id int 68 CAN ID of right VESC poll_rate_hz int 20 Publish + request rate (10-50 Hz) sender_can_id int 127 This node's CAN ID overcurrent_threshold_a float 60.0 Fault alert threshold (A) @@ -62,8 +62,8 @@ class VescTelemetryNode(Node): # Parameters # ---------------------------------------------------------------- self.declare_parameter("can_interface", "can0") - self.declare_parameter("left_can_id", 61) - self.declare_parameter("right_can_id", 79) + self.declare_parameter("left_can_id", 56) + self.declare_parameter("right_can_id", 68) self.declare_parameter("poll_rate_hz", 20) self.declare_parameter("sender_can_id", 127) self.declare_parameter("overcurrent_threshold_a", 60.0) diff --git a/jetson/ros2_ws/src/saltybot_vesc_telemetry/test/test_vesc_telemetry.py b/jetson/ros2_ws/src/saltybot_vesc_telemetry/test/test_vesc_telemetry.py index 06f58d3..5b3f68e 100644 --- a/jetson/ros2_ws/src/saltybot_vesc_telemetry/test/test_vesc_telemetry.py +++ b/jetson/ros2_ws/src/saltybot_vesc_telemetry/test/test_vesc_telemetry.py @@ -141,29 +141,29 @@ class TestParseStatus5: class TestMakeGetValuesRequest: def test_arb_id_encodes_packet_type_and_target(self): - arb_id, payload = make_get_values_request(sender_id=127, target_id=61) - expected_arb = (CAN_PACKET_PROCESS_SHORT_BUFFER << 8) | 61 + arb_id, payload = make_get_values_request(sender_id=127, target_id=56) + expected_arb = (CAN_PACKET_PROCESS_SHORT_BUFFER << 8) | 56 assert arb_id == expected_arb def test_payload_contains_sender_command(self): - _, payload = make_get_values_request(sender_id=127, target_id=61) + _, payload = make_get_values_request(sender_id=127, target_id=56) assert len(payload) == 3 assert payload[0] == 127 # sender_id assert payload[1] == 0x00 # send_mode assert payload[2] == COMM_GET_VALUES def test_right_vesc(self): - arb_id, payload = make_get_values_request(sender_id=127, target_id=79) - assert (arb_id & 0xFF) == 79 + arb_id, payload = make_get_values_request(sender_id=127, target_id=68) + assert (arb_id & 0xFF) == 68 assert payload[2] == COMM_GET_VALUES def test_sender_id_in_payload(self): - _, payload = make_get_values_request(sender_id=42, target_id=61) + _, payload = make_get_values_request(sender_id=42, target_id=56) assert payload[0] == 42 def test_arb_id_is_extended(self): # Extended IDs can exceed 0x7FF (11-bit limit) - arb_id, _ = make_get_values_request(127, 61) + arb_id, _ = make_get_values_request(127, 56) assert arb_id > 0x7FF @@ -173,7 +173,7 @@ class TestMakeGetValuesRequest: class TestVescState: def test_defaults(self): - s = VescState(can_id=61) + s = VescState(can_id=56) assert s.rpm == 0 assert s.current_a == pytest.approx(0.0) assert s.voltage_v == pytest.approx(0.0) @@ -181,42 +181,42 @@ class TestVescState: assert s.has_fault is False def test_is_alive_fresh(self): - s = VescState(can_id=61) + s = VescState(can_id=56) s.last_status_ts = time.monotonic() assert s.is_alive is True def test_is_alive_stale(self): - s = VescState(can_id=61) + s = VescState(can_id=56) s.last_status_ts = time.monotonic() - 2.0 # 2 s ago > 1 s timeout assert s.is_alive is False def test_is_alive_never_received(self): - s = VescState(can_id=61) + s = VescState(can_id=56) # last_status_ts defaults to 0.0 — monotonic() will be >> 1 s assert s.is_alive is False def test_has_fault_true(self): - s = VescState(can_id=61) + s = VescState(can_id=56) s.fault_code = FAULT_CODE_ABS_OVER_CURRENT assert s.has_fault is True def test_has_fault_false_on_none(self): - s = VescState(can_id=61) + s = VescState(can_id=56) s.fault_code = FAULT_CODE_NONE assert s.has_fault is False def test_fault_name_known(self): - s = VescState(can_id=61) + s = VescState(can_id=56) s.fault_code = FAULT_CODE_OVER_TEMP_FET assert s.fault_name == "OVER_TEMP_FET" def test_fault_name_unknown(self): - s = VescState(can_id=61) + s = VescState(can_id=56) s.fault_code = 99 assert "UNKNOWN" in s.fault_name def test_fault_name_none(self): - s = VescState(can_id=61) + s = VescState(can_id=56) assert s.fault_name == "NONE"