Improve read() and write() algorithmic performance

Author: glpuga

vesc_hardware/include/vesc_hardware/vesc_hardware.hpp

@@ -30,9 +30,10 @@
 #define VESC_HARDWARE__VESC_HARDWARE_HPP_
 
 #include <atomic>
+#include <functional>
 #include <memory>
 #include <string>
-#include <vector>
+#include <unordered_map>
 
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
@@ -154,34 +155,41 @@ class VescHardware : public hardware_interface::SystemInterface {
   double convertMechanicalRadToDeg(double mechanical_position_rad) const;
   double convertMechanicalRadSecToERPM(double mechanical_velocity_rad_s) const;
 
-  // Interface tracking structure
-  struct InterfaceInfo
+  // Interface definition initialization
+  void populate_state_definitions();
+  void populate_command_definitions();
+
+  // Interface data structures
+  struct StateInterfaceData
+  {
+    bool requested;  // Whether this interface was requested in URDF
+    std::function<double()> get_value;  // Functor to retrieve current state value
+  };
+
+  struct CommandInterfaceData
   {
-    std::string name;  // Interface name (position or velocity)
-    std::string type;  // Interface type string
     bool requested;  // Whether this interface was requested in URDF
+    std::function<void(double)> set_command;  // Functor to send command to hardware
   };
 
   // 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)
 
-  // VESC interface
-  std::unique_ptr<vesc_driver::VescInterface> vesc_interface_;
-
-  // Interface availability tracking
-  std::vector<InterfaceInfo> state_interfaces_;
-  std::vector<InterfaceInfo> command_interfaces_;
-
   // State storage (atomic for thread-safe access from callback)
   std::atomic<double> hw_state_position_;
   std::atomic<double> hw_state_velocity_;
 
-  // Command storage
-  double hw_command_position_;
-  double hw_command_velocity_;
+  // Command storage (servo only - state mirrors command since VESC can't read it)
   double hw_command_servo_;
+
+  // Interface availability tracking
+  std::unordered_map<std::string, StateInterfaceData> state_interfaces_;
+  std::unordered_map<std::string, CommandInterfaceData> command_interfaces_;
+
+  // VESC interface
+  std::unique_ptr<vesc_driver::VescInterface> vesc_interface_;
 };
 
 }  // namespace vesc_hardware

vesc_hardware/src/vesc_hardware.cpp

@@ -42,6 +42,11 @@
 #include "rclcpp/rclcpp.hpp"
 #include "vesc_driver/vesc_packet.hpp"
 
+namespace
+{
+constexpr char CUSTOM_HW_IF_SERVO[] = "servo";
+}  // namespace
+
 namespace vesc_hardware
 {
 hardware_interface::CallbackReturn VescHardware::on_init(
@@ -92,82 +97,64 @@ hardware_interface::CallbackReturn VescHardware::on_init(
   const auto & joint = info_.joints[0];
 
   // Initialize list of supported interfaces
-  state_interfaces_ = {{hardware_interface::HW_IF_POSITION, "position", false},
-    {hardware_interface::HW_IF_VELOCITY, "velocity", false},
-    {"servo", "servo", false}};
-
-  command_interfaces_ = {
-    {hardware_interface::HW_IF_POSITION, "position", false},
-    {hardware_interface::HW_IF_VELOCITY, "velocity", false},
-    {"servo", "servo", false}};
+  populate_state_definitions();
+  populate_command_definitions();
 
   // Check which state interfaces are requested
   for (const auto & state_interface : joint.state_interfaces) {
-    bool found = false;
-    for (auto & supported_interface : state_interfaces_) {
-      if (state_interface.name == supported_interface.name) {
-        if (supported_interface.requested) {
-          RCLCPP_FATAL(
-              get_logger(),
-              "Duplicate state interface '%s' requested for joint '%s'",
-              state_interface.name.c_str(), joint.name.c_str());
-          return hardware_interface::CallbackReturn::ERROR;
-        }
-        supported_interface.requested = true;
-        found = true;
-        RCLCPP_INFO(get_logger(), "State interface '%s' requested",
-                    state_interface.name.c_str());
-        break;
-      }
-    }
-    if (!found) {
+    auto it = state_interfaces_.find(state_interface.name);
+    if (it == state_interfaces_.end()) {
       RCLCPP_FATAL(get_logger(),
                    "Unsupported state interface '%s' requested for joint '%s'",
                    state_interface.name.c_str(), joint.name.c_str());
       return hardware_interface::CallbackReturn::ERROR;
     }
+    if (it->second.requested) {
+      RCLCPP_FATAL(
+          get_logger(),
+          "Duplicate state interface '%s' requested for joint '%s'",
+          state_interface.name.c_str(), joint.name.c_str());
+      return hardware_interface::CallbackReturn::ERROR;
+    }
+    it->second.requested = true;
+    RCLCPP_INFO(get_logger(), "State interface '%s' requested",
+                state_interface.name.c_str());
   }
 
   // Check which command interfaces are requested
   for (const auto & command_interface : joint.command_interfaces) {
-    bool found = false;
-    for (auto & supported_interface : command_interfaces_) {
-      if (command_interface.name == supported_interface.name) {
-        if (supported_interface.requested) {
-          RCLCPP_FATAL(
-              get_logger(),
-              "Duplicate command interface '%s' requested for joint '%s'",
-              command_interface.name.c_str(), joint.name.c_str());
-          return hardware_interface::CallbackReturn::ERROR;
-        }
-        supported_interface.requested = true;
-        found = true;
-        RCLCPP_INFO(get_logger(), "Command interface '%s' requested",
-                    command_interface.name.c_str());
-        break;
-      }
-    }
-    if (!found) {
+    auto it = command_interfaces_.find(command_interface.name);
+    if (it == command_interfaces_.end()) {
       RCLCPP_FATAL(
           get_logger(),
           "Unsupported command interface '%s' requested for joint '%s'",
           command_interface.name.c_str(), joint.name.c_str());
       return hardware_interface::CallbackReturn::ERROR;
     }
+    if (it->second.requested) {
+      RCLCPP_FATAL(
+          get_logger(),
+          "Duplicate command interface '%s' requested for joint '%s'",
+          command_interface.name.c_str(), joint.name.c_str());
+      return hardware_interface::CallbackReturn::ERROR;
+    }
+    it->second.requested = true;
+    RCLCPP_INFO(get_logger(), "Command interface '%s' requested",
+                command_interface.name.c_str());
   }
 
   // Check that at least one interface is requested
   bool has_state_interface = false;
-  for (const auto & iface : state_interfaces_) {
-    if (iface.requested) {
+  for (const auto & [name, data] : state_interfaces_) {
+    if (data.requested) {
       has_state_interface = true;
       break;
     }
   }
 
   bool has_command_interface = false;
-  for (const auto & iface : command_interfaces_) {
-    if (iface.requested) {
+  for (const auto & [name, data] : command_interfaces_) {
+    if (data.requested) {
       has_command_interface = true;
       break;
     }
@@ -186,8 +173,6 @@ hardware_interface::CallbackReturn VescHardware::on_init(
   // Initialize state and command storage
   hw_state_position_ = 0.0;
   hw_state_velocity_ = 0.0;
-  hw_command_position_ = 0.0;
-  hw_command_velocity_ = 0.0;
   hw_command_servo_ = 0.0;
 
   return hardware_interface::CallbackReturn::SUCCESS;
@@ -200,15 +185,15 @@ VescHardware::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 
   // Log which interfaces are active
   RCLCPP_INFO(get_logger(), "Active state interfaces:");
-  for (const auto & iface : state_interfaces_) {
-    if (iface.requested) {
-      RCLCPP_INFO(get_logger(), "  - %s", iface.type.c_str());
+  for (const auto & [name, data] : state_interfaces_) {
+    if (data.requested) {
+      RCLCPP_INFO(get_logger(), "  - %s", name.c_str());
     }
   }
   RCLCPP_INFO(get_logger(), "Active command interfaces:");
-  for (const auto & iface : command_interfaces_) {
-    if (iface.requested) {
-      RCLCPP_INFO(get_logger(), "  - %s", iface.type.c_str());
+  for (const auto & [name, data] : command_interfaces_) {
+    if (data.requested) {
+      RCLCPP_INFO(get_logger(), "  - %s", name.c_str());
     }
   }
 
@@ -289,14 +274,9 @@ VescHardware::read(
   // (These are updated asynchronously by vescPacketCallback)
   for (const auto &[name, descr] : joint_state_interfaces_) {
     std::string interface_type = name.substr(name.find_last_of("/") + 1);
-
-    if (interface_type == hardware_interface::HW_IF_POSITION) {
-      set_state(name, hw_state_position_.load(std::memory_order_relaxed));
-    } else if (interface_type == hardware_interface::HW_IF_VELOCITY) {
-      set_state(name, hw_state_velocity_.load(std::memory_order_relaxed));
-    } else if (interface_type == "servo") {
-      // Servo state mirrors command because the VESC cannot read servo position
-      set_state(name, hw_command_servo_);
+    auto it = state_interfaces_.find(interface_type);
+    if (it != state_interfaces_.end() && it->second.get_value) {
+      set_state(name, it->second.get_value());
     }
   }
 
@@ -317,22 +297,9 @@ VescHardware::write(
   for (const auto &[name, descr] : joint_command_interfaces_) {
     // Extract interface type from full name "motor_joint/velocity"
     std::string interface_type = name.substr(name.find_last_of("/") + 1);
-
-    if (interface_type == hardware_interface::HW_IF_POSITION) {
-      hw_command_position_ = get_command(name);
-      double vesc_position = convertMechanicalRadToDeg(hw_command_position_);
-      vesc_interface_->setPosition(vesc_position);
-    } else if (interface_type == hardware_interface::HW_IF_VELOCITY) {
-      hw_command_velocity_ = get_command(name);
-      double vesc_erpm = convertMechanicalRadSecToERPM(hw_command_velocity_);
-      vesc_interface_->setSpeed(vesc_erpm);
-    } else if (interface_type == "servo") {
-      hw_command_servo_ = get_command(name);
-      vesc_interface_->setServo(hw_command_servo_);
-    } else {
-      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000,
-                           "Unknown command interface type: %s",
-                           interface_type.c_str());
+    auto it = command_interfaces_.find(interface_type);
+    if (it != command_interfaces_.end() && it->second.set_command) {
+      it->second.set_command(get_command(name));
     }
   }
 
@@ -368,6 +335,47 @@ double VescHardware::convertMechanicalRadSecToERPM(
   return erpm;
 }
 
+void VescHardware::populate_state_definitions()
+{
+  state_interfaces_[hardware_interface::HW_IF_POSITION] = {
+    false,
+    [this]() {return hw_state_position_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[hardware_interface::HW_IF_VELOCITY] = {
+    false,
+    [this]() {return hw_state_velocity_.load(std::memory_order_relaxed);}
+  };
+  state_interfaces_[CUSTOM_HW_IF_SERVO] = {
+    false,
+    [this]() {return hw_command_servo_;}
+  };
+}
+
+void VescHardware::populate_command_definitions()
+{
+  command_interfaces_[hardware_interface::HW_IF_POSITION] = {
+    false,
+    [this](double value) {
+      double vesc_position = convertMechanicalRadToDeg(value);
+      vesc_interface_->setPosition(vesc_position);
+    }
+  };
+  command_interfaces_[hardware_interface::HW_IF_VELOCITY] = {
+    false,
+    [this](double value) {
+      double vesc_erpm = convertMechanicalRadSecToERPM(value);
+      vesc_interface_->setSpeed(vesc_erpm);
+    }
+  };
+  command_interfaces_[CUSTOM_HW_IF_SERVO] = {
+    false,
+    [this](double value) {
+      hw_command_servo_ = value;
+      vesc_interface_->setServo(hw_command_servo_);
+    }
+  };
+}
+
 void VescHardware::processValuesPacket(
   const vesc_driver::VescPacketValues *values_packet)
 {