DynamixelState.msg doesn't return temperature, voltage, load, or current

[bueche]

On Ros2 humble (and assuming later) the dxl_state doesn't return: temperature, voltage, load, or current. I've tested a fix with the following changes:

• /dynamixel_hardware_interface/src/dynamixel_hardware_interface.cpp:

358,368d357
<   dxl_state_pub_uni_ptr_->lock();
<   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_.temperature.resize(num_of_pub_data);
<   dxl_state_pub_uni_ptr_->msg_.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();
< 
654d642
<     
659,690d646
<       
<       // Initialize telemetry values to 0
<       dxl_state_pub_uni_ptr_->msg_.temperature.at(index) = 0;
<       dxl_state_pub_uni_ptr_->msg_.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);
<         
<         if (interface_name == "Present Temperature") {
<           dxl_state_pub_uni_ptr_->msg_.temperature.at(index) = 
<             static_cast<int16_t>(value);
<         }
<         else if (interface_name == "Present Input Voltage") {
< 	  
<           dxl_state_pub_uni_ptr_->msg_.voltage.at(index) = 
<             static_cast<int16_t>(value * 10.0);
<         }
<         else if (interface_name == "Present Current") {
<           // Value is already in mA (raw units) from model file  
<           dxl_state_pub_uni_ptr_->msg_.present_current.at(index) = 
<             static_cast<int16_t>(value);
<         }
<         else if (interface_name == "Present Load") {
<           dxl_state_pub_uni_ptr_->msg_.present_load.at(index) = 
<             static_cast<int16_t>(value);
<         }
<       }
<   

• dynamixel_interfaces/msg/DynamixelState.msg :

std_msgs/Header header

int32 comm_state
int32[] id
bool[] torque_state
int32[] dxl_hw_state
int16[] temperature       <--- NEW
int16[] voltage           <--- NEW
int16[] present_current   <--- NEW
int16[] present_load       <--- NEW

int32 COMM_STATE_OK = 0
int32 COMM_STATE_CANNOT_FIND_CONTROL_ITEM = -1
int32 COMM_STATE_OPEN_PORT_FAIL = -2
: 
: