Add raw vesc status publisher

Author: glpuga

vesc_hardware/CMakeLists.txt

@@ -11,7 +11,9 @@ find_package(rclcpp REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(realtime_tools REQUIRED)
 find_package(vesc_driver REQUIRED)
+find_package(vesc_hardware_msgs REQUIRED)
 
 ## COMPILE
 add_library(
@@ -28,7 +30,9 @@ target_link_libraries(vesc_hardware PUBLIC
   pluginlib::pluginlib
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
+  realtime_tools::realtime_tools
   vesc_driver::vesc_driver
+  ${vesc_hardware_msgs_TARGETS}
 )
 
 # Export hardware plugins
@@ -53,7 +57,9 @@ ament_export_dependencies(
   pluginlib
   rclcpp
   rclcpp_lifecycle
+  realtime_tools
   vesc_driver
+  vesc_hardware_msgs
 )
 
 if(BUILD_TESTING)

vesc_hardware/include/vesc_hardware/vesc_hardware.hpp

@@ -42,7 +42,9 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
 #include "vesc_driver/vesc_interface.hpp"
+#include "vesc_hardware_msgs/msg/vesc_hardware_values.hpp"
 
 namespace vesc_hardware
 {
@@ -144,6 +146,7 @@ class VescHardware : public hardware_interface::SystemInterface {
 
   // VESC packet processing
   void processValuesPacket(const vesc_driver::VescPacketValues *values_packet);
+  void publishVescState(const vesc_driver::VescPacketValues & values_packet);
 
   // Conversion functions for VESC values to mechanical values (for reading
   // state)
@@ -174,8 +177,9 @@ class VescHardware : public hardware_interface::SystemInterface {
 
   // Hardware parameters
   std::string device_;
-  double gear_ratio_;  // Gear ratio between motor and output (default: 1.0)
-  int pole_pairs_;  // Motor pole pairs (default: 1)
+  double gear_ratio_;  // Gear ratio between motor and output
+  int pole_pairs_;  // Motor pole pairs
+  bool publish_raw_state_;  // Whether to publish raw VESC telemetry
 
   // State storage (atomic for thread-safe access from callback)
   std::atomic<double> hw_state_position_;
@@ -190,6 +194,12 @@ class VescHardware : public hardware_interface::SystemInterface {
 
   // VESC interface
   std::unique_ptr<vesc_driver::VescInterface> vesc_interface_;
+
+  // Publisher for hardware values
+  std::shared_ptr<rclcpp::Publisher<vesc_hardware_msgs::msg::VescHardwareValues>>
+  hardware_values_publisher_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<vesc_hardware_msgs::msg::VescHardwareValues>>
+  realtime_hardware_values_publisher_;
 };
 
 }  // namespace vesc_hardware

vesc_hardware/package.xml

@@ -14,7 +14,9 @@
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
   <depend>vesc_driver</depend>
+  <depend>vesc_hardware_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

vesc_hardware/src/vesc_hardware.cpp

@@ -83,9 +83,19 @@ hardware_interface::CallbackReturn VescHardware::on_init(
     pole_pairs_ = 1;
   }
 
+  // Read optional publish_raw_state parameter (default: false)
+  auto it_publish_raw_state = info_.hardware_parameters.find("publish_raw_state");
+  if (it_publish_raw_state != info_.hardware_parameters.end()) {
+    publish_raw_state_ = (it_publish_raw_state->second == "true" ||
+      it_publish_raw_state->second == "1");
+  } else {
+    publish_raw_state_ = false;
+  }
+
   RCLCPP_INFO(get_logger(), "Configured device: %s", device_.c_str());
   RCLCPP_INFO(get_logger(), "Gear ratio: %.2f, Pole pairs: %d", gear_ratio_,
               pole_pairs_);
+  RCLCPP_INFO(get_logger(), "Publish raw state: %s", publish_raw_state_ ? "true" : "false");
 
   // Validate interfaces
   if (info_.joints.size() != 1) {
@@ -175,6 +185,19 @@ hardware_interface::CallbackReturn VescHardware::on_init(
   hw_state_velocity_ = 0.0;
   hw_command_servo_ = 0.0;
 
+  // Create publisher for hardware values (only if enabled)
+  if (publish_raw_state_) {
+    hardware_values_publisher_ =
+      get_node()->create_publisher<vesc_hardware_msgs::msg::VescHardwareValues>(
+        "~/hardware_values", 10);
+
+    // Create realtime publisher wrapper
+    realtime_hardware_values_publisher_ =
+      std::make_shared<realtime_tools::RealtimePublisher<
+          vesc_hardware_msgs::msg::VescHardwareValues>>(
+          hardware_values_publisher_);
+  }
+
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
@@ -390,6 +413,60 @@ void VescHardware::processValuesPacket(
   double mechanical_velocity =
     convertERPMtoMechanicalRadSec(values_packet->rpm());
   hw_state_velocity_.store(mechanical_velocity, std::memory_order_relaxed);
+
+  // Publish telemetry data
+  publishVescState(*values_packet);
+}
+
+void VescHardware::publishVescState(
+  const vesc_driver::VescPacketValues & values_packet)
+{
+  if (!realtime_hardware_values_publisher_) {
+    return;
+  }
+
+  if (realtime_hardware_values_publisher_->trylock()) {
+    auto & msg = realtime_hardware_values_publisher_->msg_;
+
+    // Temperature measurements
+    msg.temp_fet = values_packet.temp_fet();
+    msg.temp_motor = values_packet.temp_motor();
+    msg.temp_mos1 = values_packet.temp_mos1();
+    msg.temp_mos2 = values_packet.temp_mos2();
+    msg.temp_mos3 = values_packet.temp_mos3();
+
+    // Current measurements
+    msg.avg_motor_current = values_packet.avg_motor_current();
+    msg.avg_input_current = values_packet.avg_input_current();
+    msg.avg_id = values_packet.avg_id();
+    msg.avg_iq = values_packet.avg_iq();
+
+    // Voltage measurements
+    msg.v_in = values_packet.v_in();
+    msg.avg_vd = values_packet.avg_vd();
+    msg.avg_vq = values_packet.avg_vq();
+
+    // Duty cycle and speed
+    msg.duty_cycle_now = values_packet.duty_cycle_now();
+    msg.rpm = values_packet.rpm();
+
+    // Energy and charge tracking
+    msg.amp_hours = values_packet.amp_hours();
+    msg.amp_hours_charged = values_packet.amp_hours_charged();
+    msg.watt_hours = values_packet.watt_hours();
+    msg.watt_hours_charged = values_packet.watt_hours_charged();
+
+    // Position and distance tracking
+    msg.tachometer = values_packet.tachometer();
+    msg.tachometer_abs = values_packet.tachometer_abs();
+    msg.pid_pos_now = values_packet.pid_pos_now();
+
+    // Status
+    msg.fault_code = values_packet.fault_code();
+    msg.controller_id = values_packet.controller_id();
+
+    realtime_hardware_values_publisher_->unlockAndPublish();
+  }
 }
 
 void VescHardware::vescPacketCallback(

vesc_hardware_msgs/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.5)
+project(vesc_hardware_msgs)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+rosidl_generate_interfaces(vesc_hardware_msgs
+  "msg/VescHardwareValues.msg"
+  "msg/VescHardwareValuesStamped.msg"
+  DEPENDENCIES
+    builtin_interfaces
+    std_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

vesc_hardware_msgs/msg/VescHardwareValues.msg

@@ -0,0 +1,39 @@
+# VESC hardware interface telemetry values
+# Mirrors the VescPacketValues structure from vesc_driver
+
+# Temperature measurements (degrees Celsius)
+float64 temp_fet             # FET temperature
+float64 temp_motor           # Motor temperature
+float64 temp_mos1            # MOSFET 1 temperature
+float64 temp_mos2            # MOSFET 2 temperature
+float64 temp_mos3            # MOSFET 3 temperature
+
+# Current measurements (amperes)
+float64 avg_motor_current    # Average motor current
+float64 avg_input_current    # Average input current
+float64 avg_id               # Average D-axis current
+float64 avg_iq               # Average Q-axis current
+
+# Voltage measurements (volts)
+float64 v_in                 # Input voltage
+float64 avg_vd               # Average D-axis voltage
+float64 avg_vq               # Average Q-axis voltage
+
+# Duty cycle and speed
+float64 duty_cycle_now       # Current duty cycle (0.0 to 1.0)
+float64 rpm                  # Motor electrical speed (RPM)
+
+# Energy and charge tracking
+float64 amp_hours            # Charge drawn (ampere-hours)
+float64 amp_hours_charged    # Charge regenerated (ampere-hours)
+float64 watt_hours           # Energy drawn (watt-hours)
+float64 watt_hours_charged   # Energy regenerated (watt-hours)
+
+# Position and distance tracking
+int32 tachometer             # Net tachometer count
+int32 tachometer_abs         # Absolute tachometer count
+float64 pid_pos_now          # Current PID position (degrees)
+
+# Status
+int32 fault_code             # Fault code (0 = no fault)
+int32 controller_id          # VESC controller ID

vesc_hardware_msgs/msg/VescHardwareValuesStamped.msg

@@ -0,0 +1,4 @@
+# Stamped version of VescHardwareValues
+
+std_msgs/Header header
+VescHardwareValues values

vesc_hardware_msgs/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>vesc_hardware_msgs</name>
+  <version>1.1.0</version>
+  <description>
+    ROS message definitions for VESC hardware interface telemetry.
+  </description>
+  <maintainer email="joebetz@seas.upenn.edu">Johannes Betz</maintainer>
+  <license>BSD</license>
+  <url type="website">http://ros.org/wiki/vesc_hardware_msgs</url>
+  <url type="repository">https://github.com/f1tenth/vesc</url>
+  <url type="bugtracker">https://github.com/f1tenth/vesc/issues</url>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <depend>builtin_interfaces</depend>
+  <depend>std_msgs</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>