Add IMU readings

Author: glpuga

TEST_CHECKLIST.md

@@ -0,0 +1,79 @@
+# VESC Hardware Interface Test Checklist
+
+## IMU State Interface Units Verification
+
+### Quaternion Orientation (unitless, normalized)
+- [ ] Verify `orientation.x` is normalized quaternion component
+- [ ] Verify `orientation.y` is normalized quaternion component
+- [ ] Verify `orientation.z` is normalized quaternion component
+- [ ] Verify `orientation.w` is normalized quaternion component
+- [ ] Verify quaternion magnitude: sqrt(x² + y² + z² + w²) ≈ 1.0
+
+### Euler Angles (radians)
+- [ ] Verify `roll` is in radians (compare raw VESC degrees * π/180)
+- [ ] Verify `pitch` is in radians (compare raw VESC degrees * π/180)
+- [ ] Verify `yaw` is in radians (compare raw VESC degrees * π/180)
+- [ ] Verify range: all values between -π and +π
+
+### Angular Velocity (rad/s)
+- [ ] Verify `angular_velocity.x` is in rad/s (compare raw VESC deg/s * π/180)
+- [ ] Verify `angular_velocity.y` is in rad/s (compare raw VESC deg/s * π/180)
+- [ ] Verify `angular_velocity.z` is in rad/s (compare raw VESC deg/s * π/180)
+
+### Linear Acceleration (m/s²)
+- [ ] Verify `linear_acceleration.x` is in m/s² (compare raw VESC 'g' * 9.80665)
+- [ ] Verify `linear_acceleration.y` is in m/s² (compare raw VESC 'g' * 9.80665)
+- [ ] Verify `linear_acceleration.z` is in m/s² (compare raw VESC 'g' * 9.80665)
+- [ ] Verify stationary gravity reading: sqrt(x² + y² + z²) ≈ 9.80665 m/s²
+
+### Magnetic Field (raw units)
+- [ ] Verify `magnetic_field.x` matches raw VESC magnetometer value
+- [ ] Verify `magnetic_field.y` matches raw VESC magnetometer value
+- [ ] Verify `magnetic_field.z` matches raw VESC magnetometer value
+
+## Motor Position Interface
+
+### Position Unit Conversion (radians, mechanical)
+- [ ] Verify position is in mechanical radians (output shaft)
+- [ ] Test conversion formula: `mechanical_rad = (vesc_deg * π/180) / gear_ratio`
+- [ ] With gear_ratio=1.0: verify 360° VESC = 2π rad mechanical
+- [ ] With gear_ratio=10.0: verify 3600° VESC = 2π rad mechanical
+- [ ] Verify position increases with forward motor rotation
+
+### Position and Pole Pairs
+- [ ] Verify pole_pairs does NOT affect position reading
+- [ ] Position should be independent of pole_pairs parameter
+
+### Position Command Conversion (radians to degrees)
+- [ ] Verify command conversion: `vesc_deg = mechanical_rad * gear_ratio * 180/π`
+- [ ] Command 2π rad with gear_ratio=1.0 → verify VESC receives 360°
+- [ ] Command π rad with gear_ratio=5.0 → verify VESC receives 900°
+
+## Motor Velocity Interface
+
+### Velocity Unit Conversion (rad/s, mechanical)
+- [ ] Verify velocity is in mechanical rad/s (output shaft)
+- [ ] Test conversion formula: `mechanical_rad_s = (vesc_erpm / pole_pairs) * (2π/60) / gear_ratio`
+- [ ] With pole_pairs=1, gear_ratio=1.0: verify 60 ERPM = 2π rad/s
+- [ ] With pole_pairs=7, gear_ratio=1.0: verify 420 ERPM = 2π rad/s
+- [ ] With pole_pairs=1, gear_ratio=10.0: verify 600 ERPM = 2π rad/s
+
+### Velocity and Gear Ratio
+- [ ] Verify higher gear_ratio reduces mechanical velocity (given constant ERPM)
+- [ ] With gear_ratio=1.0 vs 10.0: same ERPM → 10x difference in rad/s
+- [ ] Verify velocity sign matches motor rotation direction
+
+### Velocity and Pole Pairs
+- [ ] Verify higher pole_pairs increases mechanical velocity (given constant ERPM)
+- [ ] With pole_pairs=1 vs 7: same ERPM → 7x difference in rad/s
+- [ ] ERPM = motor_rpm * pole_pairs (verify this relationship)
+
+### Velocity Command Conversion (rad/s to ERPM)
+- [ ] Verify command conversion: `erpm = mechanical_rad_s * (60/2π) * gear_ratio * pole_pairs`
+- [ ] Command 2π rad/s with gear_ratio=1.0, pole_pairs=1 → verify VESC receives 60 ERPM
+- [ ] Command 1 rad/s with gear_ratio=5.0, pole_pairs=7 → verify VESC receives ~333 ERPM
+
+## Published topics
+
+- [ ] Verify `~/vesc_state` topic publishes `vesc_msgs::msg::VescState` messages
+- [ ] Verify `~/vesc_imu` topic publishes `vesc_msgs::msg::VescImuStamped` messages

vesc_hardware/include/vesc_hardware/vesc_hardware.hpp

@@ -44,6 +44,7 @@
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
 #include "vesc_driver/vesc_interface.hpp"
+#include "vesc_msgs/msg/vesc_imu_stamped.hpp"
 #include "vesc_msgs/msg/vesc_state.hpp"
 
 namespace vesc_hardware
@@ -146,7 +147,9 @@ class VescHardware : public hardware_interface::SystemInterface {
 
   // VESC packet processing
   void processValuesPacket(const vesc_driver::VescPacketValues *values_packet);
+  void processImuPacket(const vesc_driver::VescPacketImu *imu_packet);
   void publishVescState(const vesc_driver::VescPacketValues & values_packet);
+  void publishVescImu(const vesc_driver::VescPacketImu & imu_packet);
 
   // Conversion functions for VESC values to mechanical values (for reading
   // state)
@@ -185,6 +188,24 @@ class VescHardware : public hardware_interface::SystemInterface {
   std::atomic<double> hw_state_position_;
   std::atomic<double> hw_state_velocity_;
 
+  // IMU state storage (atomic for thread-safe access from callback)
+  std::atomic<double> hw_imu_orientation_x_;
+  std::atomic<double> hw_imu_orientation_y_;
+  std::atomic<double> hw_imu_orientation_z_;
+  std::atomic<double> hw_imu_orientation_w_;
+  std::atomic<double> hw_imu_roll_;
+  std::atomic<double> hw_imu_pitch_;
+  std::atomic<double> hw_imu_yaw_;
+  std::atomic<double> hw_imu_angular_velocity_x_;
+  std::atomic<double> hw_imu_angular_velocity_y_;
+  std::atomic<double> hw_imu_angular_velocity_z_;
+  std::atomic<double> hw_imu_linear_acceleration_x_;
+  std::atomic<double> hw_imu_linear_acceleration_y_;
+  std::atomic<double> hw_imu_linear_acceleration_z_;
+  std::atomic<double> hw_imu_magnetic_field_x_;
+  std::atomic<double> hw_imu_magnetic_field_y_;
+  std::atomic<double> hw_imu_magnetic_field_z_;
+
   // Command storage (servo only - state mirrors command since VESC can't read it)
   double hw_command_servo_;
 
@@ -200,6 +221,12 @@ class VescHardware : public hardware_interface::SystemInterface {
   hardware_values_publisher_;
   std::shared_ptr<realtime_tools::RealtimePublisher<vesc_msgs::msg::VescState>>
   realtime_hardware_values_publisher_;
+
+  // Publisher for IMU data
+  std::shared_ptr<rclcpp::Publisher<vesc_msgs::msg::VescImuStamped>>
+  imu_publisher_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<vesc_msgs::msg::VescImuStamped>>
+  realtime_imu_publisher_;
 };
 
 }  // namespace vesc_hardware

vesc_hardware/src/vesc_hardware.cpp

@@ -45,6 +45,32 @@
 namespace
 {
 constexpr char CUSTOM_HW_IF_SERVO[] = "servo";
+
+// IMU sensor interface names - Quaternion orientation
+constexpr char CUSTOM_HW_IF_ORIENTATION_X[] = "orientation.x";
+constexpr char CUSTOM_HW_IF_ORIENTATION_Y[] = "orientation.y";
+constexpr char CUSTOM_HW_IF_ORIENTATION_Z[] = "orientation.z";
+constexpr char CUSTOM_HW_IF_ORIENTATION_W[] = "orientation.w";
+
+// IMU sensor interface names - Euler angles
+constexpr char CUSTOM_HW_IF_ROLL[] = "roll";
+constexpr char CUSTOM_HW_IF_PITCH[] = "pitch";
+constexpr char CUSTOM_HW_IF_YAW[] = "yaw";
+
+// IMU sensor interface names - Angular velocity
+constexpr char CUSTOM_HW_IF_ANGULAR_VELOCITY_X[] = "angular_velocity.x";
+constexpr char CUSTOM_HW_IF_ANGULAR_VELOCITY_Y[] = "angular_velocity.y";
+constexpr char CUSTOM_HW_IF_ANGULAR_VELOCITY_Z[] = "angular_velocity.z";
+
+// IMU sensor interface names - Linear acceleration
+constexpr char CUSTOM_HW_IF_LINEAR_ACCELERATION_X[] = "linear_acceleration.x";
+constexpr char CUSTOM_HW_IF_LINEAR_ACCELERATION_Y[] = "linear_acceleration.y";
+constexpr char CUSTOM_HW_IF_LINEAR_ACCELERATION_Z[] = "linear_acceleration.z";
+
+// IMU sensor interface names - Magnetic field
+constexpr char CUSTOM_HW_IF_MAGNETIC_FIELD_X[] = "magnetic_field.x";
+constexpr char CUSTOM_HW_IF_MAGNETIC_FIELD_Y[] = "magnetic_field.y";
+constexpr char CUSTOM_HW_IF_MAGNETIC_FIELD_Z[] = "magnetic_field.z";
 }  // namespace
 
 namespace vesc_hardware
@@ -189,13 +215,24 @@ hardware_interface::CallbackReturn VescHardware::on_init(
   if (publish_raw_state_) {
     hardware_values_publisher_ =
       get_node()->create_publisher<vesc_msgs::msg::VescState>(
-        "~/hardware_values", 10);
+        "~/vesc_state", 10);
 
     // Create realtime publisher wrapper
     realtime_hardware_values_publisher_ =
       std::make_shared<realtime_tools::RealtimePublisher<
           vesc_msgs::msg::VescState>>(
           hardware_values_publisher_);
+
+    // Create publisher for IMU data
+    imu_publisher_ =
+      get_node()->create_publisher<vesc_msgs::msg::VescImuStamped>(
+        "~/vesc_imu", 10);
+
+    // Create realtime publisher wrapper
+    realtime_imu_publisher_ =
+      std::make_shared<realtime_tools::RealtimePublisher<
+          vesc_msgs::msg::VescImuStamped>>(
+          imu_publisher_);
   }
 
   return hardware_interface::CallbackReturn::SUCCESS;
@@ -288,9 +325,10 @@ VescHardware::read(
   const rclcpp::Time & /*time*/,
   const rclcpp::Duration & /*period*/)
 {
-  // Request state from VESC (non-blocking)
+  // Request state and IMU data from VESC (non-blocking)
   if (vesc_interface_) {
     vesc_interface_->requestState();
+    vesc_interface_->requestImuData();
   }
 
   // Update state interfaces with current values from atomic variables
@@ -372,6 +410,80 @@ void VescHardware::populate_state_definitions()
     false,
     [this]() {return hw_command_servo_;}
   };
+
+  // IMU state interfaces - Quaternion orientation
+  state_interfaces_[CUSTOM_HW_IF_ORIENTATION_X] = {
+    false,
+    [this]() {return hw_imu_orientation_x_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_ORIENTATION_Y] = {
+    false,
+    [this]() {return hw_imu_orientation_y_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_ORIENTATION_Z] = {
+    false,
+    [this]() {return hw_imu_orientation_z_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_ORIENTATION_W] = {
+    false,
+    [this]() {return hw_imu_orientation_w_.load(std::memory_order_relaxed);}
+  };
+
+  // IMU state interfaces - Euler angles
+  state_interfaces_[CUSTOM_HW_IF_ROLL] = {
+    false,
+    [this]() {return hw_imu_roll_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_PITCH] = {
+    false,
+    [this]() {return hw_imu_pitch_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_YAW] = {
+    false,
+    [this]() {return hw_imu_yaw_.load(std::memory_order_relaxed);}
+  };
+
+  // IMU state interfaces - Angular velocity
+  state_interfaces_[CUSTOM_HW_IF_ANGULAR_VELOCITY_X] = {
+    false,
+    [this]() {return hw_imu_angular_velocity_x_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_ANGULAR_VELOCITY_Y] = {
+    false,
+    [this]() {return hw_imu_angular_velocity_y_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_ANGULAR_VELOCITY_Z] = {
+    false,
+    [this]() {return hw_imu_angular_velocity_z_.load(std::memory_order_relaxed);}
+  };
+
+  // IMU state interfaces - Linear acceleration
+  state_interfaces_[CUSTOM_HW_IF_LINEAR_ACCELERATION_X] = {
+    false,
+    [this]() {return hw_imu_linear_acceleration_x_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_LINEAR_ACCELERATION_Y] = {
+    false,
+    [this]() {return hw_imu_linear_acceleration_y_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_LINEAR_ACCELERATION_Z] = {
+    false,
+    [this]() {return hw_imu_linear_acceleration_z_.load(std::memory_order_relaxed);}
+  };
+
+  // IMU state interfaces - Magnetic field
+  state_interfaces_[CUSTOM_HW_IF_MAGNETIC_FIELD_X] = {
+    false,
+    [this]() {return hw_imu_magnetic_field_x_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_MAGNETIC_FIELD_Y] = {
+    false,
+    [this]() {return hw_imu_magnetic_field_y_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_MAGNETIC_FIELD_Z] = {
+    false,
+    [this]() {return hw_imu_magnetic_field_z_.load(std::memory_order_relaxed);}
+  };
 }
 
 void VescHardware::populate_command_definitions()
@@ -418,6 +530,93 @@ void VescHardware::processValuesPacket(
   publishVescState(*values_packet);
 }
 
+void VescHardware::processImuPacket(
+  const vesc_driver::VescPacketImu *imu_packet)
+{
+  if (!imu_packet) {
+    return;
+  }
+
+  // Conversion lambda: degrees to radians
+  auto deg_to_rad = [](double deg) { return deg * M_PI / 180.0; };
+
+  // Standard gravity constant for converting acceleration from 'g' to m/s²
+  constexpr double STANDARD_GRAVITY = 9.80665;
+
+  // Store quaternion orientation (already in correct units)
+  hw_imu_orientation_x_.store(imu_packet->q_x(), std::memory_order_relaxed);
+  hw_imu_orientation_y_.store(imu_packet->q_y(), std::memory_order_relaxed);
+  hw_imu_orientation_z_.store(imu_packet->q_z(), std::memory_order_relaxed);
+  hw_imu_orientation_w_.store(imu_packet->q_w(), std::memory_order_relaxed);
+
+  // Store Euler angles (convert from degrees to radians)
+  hw_imu_roll_.store(deg_to_rad(imu_packet->roll()), std::memory_order_relaxed);
+  hw_imu_pitch_.store(deg_to_rad(imu_packet->pitch()), std::memory_order_relaxed);
+  hw_imu_yaw_.store(deg_to_rad(imu_packet->yaw()), std::memory_order_relaxed);
+
+  // Store angular velocity (convert from degrees/second to radians/second)
+  hw_imu_angular_velocity_x_.store(deg_to_rad(imu_packet->gyr_x()), std::memory_order_relaxed);
+  hw_imu_angular_velocity_y_.store(deg_to_rad(imu_packet->gyr_y()), std::memory_order_relaxed);
+  hw_imu_angular_velocity_z_.store(deg_to_rad(imu_packet->gyr_z()), std::memory_order_relaxed);
+
+  // Store linear acceleration (convert from 'g' to m/s²)
+  hw_imu_linear_acceleration_x_.store(imu_packet->acc_x() * STANDARD_GRAVITY, std::memory_order_relaxed);
+  hw_imu_linear_acceleration_y_.store(imu_packet->acc_y() * STANDARD_GRAVITY, std::memory_order_relaxed);
+  hw_imu_linear_acceleration_z_.store(imu_packet->acc_z() * STANDARD_GRAVITY, std::memory_order_relaxed);
+
+  // Store magnetic field (in raw units as received)
+  hw_imu_magnetic_field_x_.store(imu_packet->mag_x(), std::memory_order_relaxed);
+  hw_imu_magnetic_field_y_.store(imu_packet->mag_y(), std::memory_order_relaxed);
+  hw_imu_magnetic_field_z_.store(imu_packet->mag_z(), std::memory_order_relaxed);
+
+  // Publish raw IMU data
+  publishVescImu(*imu_packet);
+}
+
+void VescHardware::publishVescImu(
+  const vesc_driver::VescPacketImu & imu_packet)
+{
+  if (!realtime_imu_publisher_) {
+    return;
+  }
+
+  if (realtime_imu_publisher_->trylock()) {
+    auto & msg = realtime_imu_publisher_->msg_;
+
+    // Set timestamp
+    msg.header.stamp = get_node()->now();
+    msg.header.frame_id = "";
+
+    // Yaw, Pitch, Roll (in degrees as received)
+    msg.imu.ypr.x = imu_packet.yaw();
+    msg.imu.ypr.y = imu_packet.pitch();
+    msg.imu.ypr.z = imu_packet.roll();
+
+    // Linear acceleration (in m/s²)
+    msg.imu.linear_acceleration.x = imu_packet.acc_x();
+    msg.imu.linear_acceleration.y = imu_packet.acc_y();
+    msg.imu.linear_acceleration.z = imu_packet.acc_z();
+
+    // Angular velocity (in degrees/second as received)
+    msg.imu.angular_velocity.x = imu_packet.gyr_x();
+    msg.imu.angular_velocity.y = imu_packet.gyr_y();
+    msg.imu.angular_velocity.z = imu_packet.gyr_z();
+
+    // Compass (in raw units as received)
+    msg.imu.compass.x = imu_packet.mag_x();
+    msg.imu.compass.y = imu_packet.mag_y();
+    msg.imu.compass.z = imu_packet.mag_z();
+
+    // Orientation quaternion
+    msg.imu.orientation.w = imu_packet.q_w();
+    msg.imu.orientation.x = imu_packet.q_x();
+    msg.imu.orientation.y = imu_packet.q_y();
+    msg.imu.orientation.z = imu_packet.q_z();
+
+    realtime_imu_publisher_->unlockAndPublish();
+  }
+}
+
 void VescHardware::publishVescState(
   const vesc_driver::VescPacketValues & values_packet)
 {
@@ -495,6 +694,10 @@ void VescHardware::vescPacketCallback(
         fw_packet->paired() ? "yes" : "no"
       );
     }
+  } else if (packet->name() == "ImuData") {
+    const auto *imu_packet =
+      dynamic_cast<const vesc_driver::VescPacketImu *>(packet.get());
+    processImuPacket(imu_packet);
   }
 }