diff --git a/vesc_hardware/CMakeLists.txt b/vesc_hardware/CMakeLists.txt index 2fb6f45..1f218fd 100644 --- a/vesc_hardware/CMakeLists.txt +++ b/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) diff --git a/vesc_hardware/include/vesc_hardware/vesc_hardware.hpp b/vesc_hardware/include/vesc_hardware/vesc_hardware.hpp index e25e1a9..44b79c2 100644 --- a/vesc_hardware/include/vesc_hardware/vesc_hardware.hpp +++ b/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 hw_state_position_; @@ -190,6 +194,12 @@ class VescHardware : public hardware_interface::SystemInterface { // VESC interface std::unique_ptr vesc_interface_; + + // Publisher for hardware values + std::shared_ptr> + hardware_values_publisher_; + std::shared_ptr> + realtime_hardware_values_publisher_; }; } // namespace vesc_hardware diff --git a/vesc_hardware/package.xml b/vesc_hardware/package.xml index 9f344b5..196864b 100644 --- a/vesc_hardware/package.xml +++ b/vesc_hardware/package.xml @@ -14,7 +14,9 @@ hardware_interface pluginlib rclcpp_lifecycle + realtime_tools vesc_driver + vesc_hardware_msgs ament_lint_auto ament_lint_common diff --git a/vesc_hardware/src/vesc_hardware.cpp b/vesc_hardware/src/vesc_hardware.cpp index ba96c0d..bcfc340 100644 --- a/vesc_hardware/src/vesc_hardware.cpp +++ b/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( + "~/hardware_values", 10); + + // Create realtime publisher wrapper + realtime_hardware_values_publisher_ = + std::make_shared>( + 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( diff --git a/vesc_hardware_msgs/CMakeLists.txt b/vesc_hardware_msgs/CMakeLists.txt new file mode 100644 index 0000000..dfa445e --- /dev/null +++ b/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() diff --git a/vesc_hardware_msgs/msg/VescHardwareValues.msg b/vesc_hardware_msgs/msg/VescHardwareValues.msg new file mode 100644 index 0000000..e905ce2 --- /dev/null +++ b/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 diff --git a/vesc_hardware_msgs/msg/VescHardwareValuesStamped.msg b/vesc_hardware_msgs/msg/VescHardwareValuesStamped.msg new file mode 100644 index 0000000..643c369 --- /dev/null +++ b/vesc_hardware_msgs/msg/VescHardwareValuesStamped.msg @@ -0,0 +1,4 @@ +# Stamped version of VescHardwareValues + +std_msgs/Header header +VescHardwareValues values diff --git a/vesc_hardware_msgs/package.xml b/vesc_hardware_msgs/package.xml new file mode 100644 index 0000000..cb465af --- /dev/null +++ b/vesc_hardware_msgs/package.xml @@ -0,0 +1,30 @@ + + + + vesc_hardware_msgs + 1.1.0 + + ROS message definitions for VESC hardware interface telemetry. + + Johannes Betz + BSD + http://ros.org/wiki/vesc_hardware_msgs + https://github.com/f1tenth/vesc + https://github.com/f1tenth/vesc/issues + + ament_cmake_auto + + rosidl_default_generators + builtin_interfaces + std_msgs + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + +