adding support to publish present temnperature, load, input current, and voltage

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -373,6 +373,15 @@ class DynamixelHardware : public
 
   // Move dxl_comm_ to the end for safe destruction order
   std::shared_ptr<Dynamixel> dxl_comm_;
+
+  /**
+   * @brief populates a specific telementry value for a given servo
+   * @return void
+   */
+  void processTelemetryInterface(
+    const std::string& interface_name,
+    double value,
+    size_t index);
 };
 
 // Conversion maps between ROS2 and Dynamixel interface names

src/dynamixel_hardware_interface.cpp

@@ -33,6 +33,14 @@
 namespace dynamixel_hardware_interface
 {
 
+// Telemetry interface name constants
+namespace TelemetryInterfaces {
+  constexpr const char* TEMPERATURE = "Present Temperature";
+  constexpr const char* VOLTAGE = "Present Input Voltage";
+  constexpr const char* CURRENT = "Present Current";
+  constexpr const char* LOAD = "Present Load";
+}
+
 DynamixelHardware::DynamixelHardware()
 : rclcpp::Node("dynamixel_hardware_interface"),
   logger_(rclcpp::get_logger("dynamixel_hardware_interface"))
@@ -363,6 +371,11 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
   dxl_state_pub_uni_ptr_->msg_.id.resize(num_of_pub_data);
   dxl_state_pub_uni_ptr_->msg_.dxl_hw_state.resize(num_of_pub_data);
   dxl_state_pub_uni_ptr_->msg_.torque_state.resize(num_of_pub_data);
+  // Add telemetry arrays
+  dxl_state_pub_uni_ptr_->msg_.present_temperature.resize(num_of_pub_data);
+  dxl_state_pub_uni_ptr_->msg_.present_input_voltage.resize(num_of_pub_data);
+  dxl_state_pub_uni_ptr_->msg_.present_current.resize(num_of_pub_data);
+  dxl_state_pub_uni_ptr_->msg_.present_load.resize(num_of_pub_data);
   dxl_state_pub_uni_ptr_->unlock();
 
   using namespace std::placeholders;
@@ -607,6 +620,46 @@ hardware_interface::CallbackReturn DynamixelHardware::stop()
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
+struct TelemetryHandler {
+  double scale_factor;
+  std::function<void(DynamixelHardware*, int16_t, size_t)> setter;
+};
+
+void DynamixelHardware::processTelemetryInterface(
+  const std::string& interface_name,
+  double value,
+  size_t index)
+{
+  // Static map for constant time lookup - initialized once
+  static const std::unordered_map<std::string, TelemetryHandler> telemetry_map = {
+      {TelemetryInterfaces::TEMPERATURE, 
+       {1.0, [](DynamixelHardware* hw, int16_t val, size_t idx) {
+         hw->dxl_state_pub_uni_ptr_->msg_.present_temperature.at(idx) = val;
+       }}},
+      {TelemetryInterfaces::VOLTAGE,
+       {10.0, [](DynamixelHardware* hw, int16_t val, size_t idx) {
+         hw->dxl_state_pub_uni_ptr_->msg_.present_input_voltage.at(idx) = val;
+       }}},
+      {TelemetryInterfaces::CURRENT,
+       {1.0, [](DynamixelHardware* hw, int16_t val, size_t idx) {
+         hw->dxl_state_pub_uni_ptr_->msg_.present_current.at(idx) = val;
+       }}},
+      {TelemetryInterfaces::LOAD,
+       {1.0, [](DynamixelHardware* hw, int16_t val, size_t idx) {
+         hw->dxl_state_pub_uni_ptr_->msg_.present_load.at(idx) = val;
+       }}}
+    };
+
+  auto it = telemetry_map.find(interface_name);
+  if (it != telemetry_map.end()) {
+    const auto& handler = it->second;
+    double scale = handler.scale_factor;
+    auto setter = handler.setter;
+    int16_t converted_value = static_cast<int16_t>(value * scale);
+    setter(this, converted_value, index);
+  }
+}
+
 hardware_interface::return_type DynamixelHardware::read(
   [[maybe_unused]] const rclcpp::Time & time, const rclcpp::Duration & period)
 {
@@ -656,6 +709,19 @@ hardware_interface::return_type DynamixelHardware::read(
       auto ts_it = dxl_torque_state_.find({it.comm_id, it.id});
       bool ts = (ts_it != dxl_torque_state_.end()) ? ts_it->second : false;
       dxl_state_pub_uni_ptr_->msg_.torque_state.at(index) = ts;
+
+      // Initialize telemetry values to 0
+      dxl_state_pub_uni_ptr_->msg_.present_temperature.at(index) = 0;
+      dxl_state_pub_uni_ptr_->msg_.present_input_voltage.at(index) = 0;
+      dxl_state_pub_uni_ptr_->msg_.present_current.at(index) = 0;
+      dxl_state_pub_uni_ptr_->msg_.present_load.at(index) = 0;
+
+      // Extract telemetry values from state interfaces
+      for (size_t i = 0; i < it.interface_name_vec.size(); i++) {
+        const std::string& interface_name = it.interface_name_vec.at(i);
+        double value = *it.value_ptr_vec.at(i);
+        processTelemetryInterface(interface_name, value, index);
+      }
       index++;
     }
     dxl_state_pub_uni_ptr_->unlockAndPublish();