feat: Enable VESC driver telemetry publishing (Issue #681)
vesc_driver_node.py: - Add VescState dataclass with to_dict() serialization - Add CAN_PACKET_STATUS/STATUS_4/STATUS_5 (9/16/27) RX constants - Add FAULT_NAMES lookup (11 VESC FW 6.6 fault codes) - Add background CAN RX thread (_rx_loop / _dispatch_frame) that parses STATUS broadcast frames using struct.unpack - Add publishers for /saltybot/vesc/left and /saltybot/vesc/right (std_msgs/String JSON) at configurable telemetry_rate_hz (default 10 Hz) - Combine watchdog + publish into single timer callback - Proper thread cleanup in destroy_node() Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
05ba557dca
commit
06db56103f
@ -4,23 +4,84 @@ VESC CAN driver node using python-can with SocketCAN.
|
||||
Subscribes to /cmd_vel (geometry_msgs/Twist) and sends duty cycle
|
||||
commands to two VESC motor controllers via CAN bus.
|
||||
|
||||
Also receives VESC STATUS broadcast frames and publishes telemetry:
|
||||
/saltybot/vesc/left (std_msgs/String JSON)
|
||||
/saltybot/vesc/right (std_msgs/String JSON)
|
||||
|
||||
VESC CAN protocol:
|
||||
Extended frame ID = (CAN_PACKET_ID << 8) | VESC_ID
|
||||
CAN_PACKET_SET_DUTY = 0 -> payload: int32 (duty * 100000)
|
||||
"""
|
||||
|
||||
import json
|
||||
import struct
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional
|
||||
|
||||
import can
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
# VESC CAN packet IDs
|
||||
# ── VESC CAN packet IDs ────────────────────────────────────────────────────────
|
||||
CAN_PACKET_SET_DUTY = 0
|
||||
CAN_PACKET_SET_RPM = 3
|
||||
CAN_PACKET_STATUS = 9 # RPM, phase current, duty
|
||||
CAN_PACKET_STATUS_4 = 16 # FET temp, motor temp, input current
|
||||
CAN_PACKET_STATUS_5 = 27 # Input voltage
|
||||
|
||||
FAULT_NAMES = {
|
||||
0: "NONE", 1: "OVER_VOLTAGE",
|
||||
2: "UNDER_VOLTAGE", 3: "DRV",
|
||||
4: "ABS_OVER_CURRENT", 5: "OVER_TEMP_FET",
|
||||
6: "OVER_TEMP_MOTOR", 7: "GATE_DRIVER_OVER_VOLTAGE",
|
||||
8: "GATE_DRIVER_UNDER_VOLTAGE_3V3", 9: "MCU_UNDER_VOLTAGE",
|
||||
10: "BOOTING_FROM_WATCHDOG_RESET", 11: "ENCODER_SPI_FAULT",
|
||||
}
|
||||
|
||||
ALIVE_TIMEOUT_S = 1.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class VescState:
|
||||
can_id: int
|
||||
rpm: int = 0
|
||||
current_a: float = 0.0
|
||||
current_in_a: float = 0.0
|
||||
duty_cycle: float = 0.0
|
||||
voltage_v: float = 0.0
|
||||
temp_fet_c: float = 0.0
|
||||
temp_motor_c: float = 0.0
|
||||
fault_code: int = 0
|
||||
last_ts: float = field(default_factory=lambda: 0.0)
|
||||
|
||||
@property
|
||||
def is_alive(self) -> bool:
|
||||
return (time.monotonic() - self.last_ts) < ALIVE_TIMEOUT_S
|
||||
|
||||
@property
|
||||
def fault_name(self) -> str:
|
||||
return FAULT_NAMES.get(self.fault_code, f"UNKNOWN_{self.fault_code}")
|
||||
|
||||
def to_dict(self) -> dict:
|
||||
return {
|
||||
"can_id": self.can_id,
|
||||
"rpm": self.rpm,
|
||||
"current_a": round(self.current_a, 2),
|
||||
"current_in_a": round(self.current_in_a, 2),
|
||||
"duty_cycle": round(self.duty_cycle, 4),
|
||||
"voltage_v": round(self.voltage_v, 2),
|
||||
"temp_fet_c": round(self.temp_fet_c, 1),
|
||||
"temp_motor_c": round(self.temp_motor_c, 1),
|
||||
"fault_code": self.fault_code,
|
||||
"fault_name": self.fault_name,
|
||||
"alive": self.is_alive,
|
||||
"stamp": time.time(),
|
||||
}
|
||||
|
||||
|
||||
def make_can_id(packet_id: int, vesc_id: int) -> int:
|
||||
@ -40,6 +101,7 @@ class VescCanDriver(Node):
|
||||
self.declare_parameter('wheel_radius', 0.1)
|
||||
self.declare_parameter('max_speed', 5.0)
|
||||
self.declare_parameter('command_timeout', 1.0)
|
||||
self.declare_parameter('telemetry_rate_hz', 10)
|
||||
|
||||
# Read parameters
|
||||
self.interface = self.get_parameter('interface').value
|
||||
@ -48,6 +110,7 @@ class VescCanDriver(Node):
|
||||
self.wheel_sep = self.get_parameter('wheel_separation').value
|
||||
self.max_speed = self.get_parameter('max_speed').value
|
||||
self.cmd_timeout = self.get_parameter('command_timeout').value
|
||||
tel_hz = int(self.get_parameter('telemetry_rate_hz').value)
|
||||
|
||||
# Open CAN bus
|
||||
try:
|
||||
@ -62,17 +125,36 @@ class VescCanDriver(Node):
|
||||
|
||||
self._last_cmd_time = time.monotonic()
|
||||
|
||||
# Per-VESC telemetry state
|
||||
self._state: dict[int, VescState] = {
|
||||
self.left_id: VescState(can_id=self.left_id),
|
||||
self.right_id: VescState(can_id=self.right_id),
|
||||
}
|
||||
self._state_lock = threading.Lock()
|
||||
|
||||
# Telemetry publishers
|
||||
self._pub_left = self.create_publisher(String, '/saltybot/vesc/left', 10)
|
||||
self._pub_right = self.create_publisher(String, '/saltybot/vesc/right', 10)
|
||||
|
||||
# CAN RX background thread
|
||||
self._running = True
|
||||
self._rx_thread = threading.Thread(target=self._rx_loop, name='vesc_rx', daemon=True)
|
||||
self._rx_thread.start()
|
||||
|
||||
# Subscriber
|
||||
self.create_subscription(Twist, '/cmd_vel', self._cmd_vel_cb, 10)
|
||||
|
||||
# Watchdog timer (10 Hz)
|
||||
self.create_timer(0.1, self._watchdog_cb)
|
||||
# Watchdog + telemetry publish timer
|
||||
self.create_timer(1.0 / max(1, tel_hz), self._watchdog_and_publish_cb)
|
||||
|
||||
self.get_logger().info(
|
||||
f'VESC CAN driver ready — left={self.left_id} right={self.right_id}'
|
||||
f'VESC CAN driver ready — left={self.left_id} right={self.right_id} '
|
||||
f'telemetry={tel_hz} Hz'
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Command velocity callback
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _cmd_vel_cb(self, msg: Twist):
|
||||
self._last_cmd_time = time.monotonic()
|
||||
@ -88,12 +170,68 @@ class VescCanDriver(Node):
|
||||
self._send_duty(self.left_id, left_duty)
|
||||
self._send_duty(self.right_id, right_duty)
|
||||
|
||||
def _watchdog_cb(self):
|
||||
# ------------------------------------------------------------------
|
||||
# Watchdog + telemetry publish (combined timer callback)
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _watchdog_and_publish_cb(self):
|
||||
elapsed = time.monotonic() - self._last_cmd_time
|
||||
if elapsed > self.cmd_timeout:
|
||||
self._send_duty(self.left_id, 0.0)
|
||||
self._send_duty(self.right_id, 0.0)
|
||||
|
||||
with self._state_lock:
|
||||
l_dict = self._state[self.left_id].to_dict()
|
||||
r_dict = self._state[self.right_id].to_dict()
|
||||
|
||||
self._pub_left.publish(String(data=json.dumps(l_dict)))
|
||||
self._pub_right.publish(String(data=json.dumps(r_dict)))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# CAN RX background thread
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _rx_loop(self) -> None:
|
||||
"""Receive VESC STATUS broadcast frames and update telemetry state."""
|
||||
while self._running:
|
||||
try:
|
||||
msg = self.bus.recv(timeout=0.1)
|
||||
except Exception:
|
||||
continue
|
||||
if msg is None or not msg.is_extended_id:
|
||||
continue
|
||||
self._dispatch_frame(msg.arbitration_id, bytes(msg.data))
|
||||
|
||||
def _dispatch_frame(self, arb_id: int, data: bytes) -> None:
|
||||
packet_type = (arb_id >> 8) & 0xFFFF
|
||||
vesc_id = arb_id & 0xFF
|
||||
|
||||
if vesc_id not in self._state:
|
||||
return
|
||||
|
||||
now = time.monotonic()
|
||||
with self._state_lock:
|
||||
s = self._state[vesc_id]
|
||||
|
||||
if packet_type == CAN_PACKET_STATUS and len(data) >= 8:
|
||||
rpm = struct.unpack('>i', data[0:4])[0]
|
||||
current_a = struct.unpack('>h', data[4:6])[0] / 10.0
|
||||
duty_cycle = struct.unpack('>h', data[6:8])[0] / 1000.0
|
||||
s.rpm = rpm
|
||||
s.current_a = current_a
|
||||
s.duty_cycle = duty_cycle
|
||||
s.last_ts = now
|
||||
|
||||
elif packet_type == CAN_PACKET_STATUS_4 and len(data) >= 6:
|
||||
s.temp_fet_c = struct.unpack('>h', data[0:2])[0] / 10.0
|
||||
s.temp_motor_c = struct.unpack('>h', data[2:4])[0] / 10.0
|
||||
s.current_in_a = struct.unpack('>h', data[4:6])[0] / 10.0
|
||||
|
||||
elif packet_type == CAN_PACKET_STATUS_5 and len(data) >= 2:
|
||||
s.voltage_v = struct.unpack('>h', data[0:2])[0] / 10.0
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# CAN send helpers
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _send_duty(self, vesc_id: int, duty: float):
|
||||
@ -111,6 +249,8 @@ class VescCanDriver(Node):
|
||||
self.get_logger().error(f'CAN send error (id={vesc_id}): {e}')
|
||||
|
||||
def destroy_node(self):
|
||||
self._running = False
|
||||
self._rx_thread.join(timeout=1.0)
|
||||
self._send_duty(self.left_id, 0.0)
|
||||
self._send_duty(self.right_id, 0.0)
|
||||
self.bus.shutdown()
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user