Add Support for Various Command Interfaces

CHANGELOG.rst

@@ -2,6 +2,17 @@
 Changelog for package dynamixel_hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.4.4 (2025-05-28)
+------------------
+* Added proper command interface support with ROS2-Dynamixel interface mapping
+* Improved error handling and robustness throughout the codebase
+* Implemented per-device torque enable control (replacing global control)
+* Added support for new sensor model (sensorxel_joy)
+* Enhanced joint state-command synchronization
+* Improved parameter initialization organization
+* Added robust error handling for model file reading
+* Contributors: Woojin Wie
+
 1.4.3 (2025-04-10)
 ------------------
 * Fixed build errors

include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 //
-// Authors: Hye-Jong KIM, Sungho Woo
+// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie
 
 #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_HPP_
 #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_HPP_
@@ -110,8 +110,8 @@ class Dynamixel
 {
 private:
   // dxl communication variable
-  dynamixel::PortHandler * port_handler_;
-  dynamixel::PacketHandler * packet_handler_;
+  dynamixel::PortHandler * port_handler_ = nullptr;
+  dynamixel::PacketHandler * packet_handler_ = nullptr;
 
   // dxl info variable from dxl_model file
   DynamixelInfo dxl_info_;
@@ -126,24 +126,35 @@ class Dynamixel
   std::vector<RWItemList> read_data_list_;
 
   // sync read
-  dynamixel::GroupFastSyncRead * group_sync_read_;
+  dynamixel::GroupSyncRead * group_sync_read_ = nullptr;
+  // bulk read
+  dynamixel::GroupBulkRead * group_bulk_read_ = nullptr;
+  // fast sync read
+  dynamixel::GroupFastSyncRead * group_fast_sync_read_ = nullptr;
+  // fast bulk read
+  dynamixel::GroupFastBulkRead * group_fast_bulk_read_ = nullptr;
+
+  // fast read protocol state (applies to both sync and bulk)
+  bool use_fast_read_protocol_ = true;
+  bool fast_read_permanent_ = false;
+  int fast_read_fail_count_ = 0;
+
   // indirect inform for sync read
   std::map<uint8_t /*id*/, IndirectInfo> indirect_info_read_;
 
-  // bulk read
-  dynamixel::GroupFastBulkRead * group_bulk_read_;
-
   // write item (sync or bulk) variable
   bool write_type_;
   std::vector<RWItemList> write_data_list_;
 
   // sync write
-  dynamixel::GroupSyncWrite * group_sync_write_;
+  dynamixel::GroupSyncWrite * group_sync_write_ = nullptr;
   // indirect inform for sync write
   std::map<uint8_t /*id*/, IndirectInfo> indirect_info_write_;
 
   // bulk write
-  dynamixel::GroupBulkWrite * group_bulk_write_;
+  dynamixel::GroupBulkWrite * group_bulk_write_ = nullptr;
+  // direct inform for bulk write
+  std::map<uint8_t /*id*/, IndirectInfo> direct_info_write_;
 
 public:
   explicit Dynamixel(const char * path);
@@ -202,11 +213,19 @@ class Dynamixel
   DxlError SetSyncReadItemAndHandler();
   DxlError SetSyncReadHandler(std::vector<uint8_t> id_arr);
   DxlError GetDxlValueFromSyncRead(double period_ms);
+  DxlError SetFastSyncReadHandler(std::vector<uint8_t> id_arr);
 
   // BulkRead
   DxlError SetBulkReadItemAndHandler();
   DxlError SetBulkReadHandler(std::vector<uint8_t> id_arr);
   DxlError GetDxlValueFromBulkRead(double period_ms);
+  DxlError SetFastBulkReadHandler(std::vector<uint8_t> id_arr);
+
+  // DirectRead for BulkRead
+  DxlError AddDirectRead(uint8_t id, std::string item_name, uint16_t item_addr, uint8_t item_size);
+
+  // Check Indirect Read
+  DxlError CheckIndirectReadAvailable(uint8_t id);
 
   // Read - Indirect Address
   void ResetIndirectRead(std::vector<uint8_t> id_arr);
@@ -225,13 +244,19 @@ class Dynamixel
     const std::vector<std::shared_ptr<double>> & data_ptrs,
     std::function<uint32_t(uint8_t, uint16_t, uint8_t)> get_data_func);
 
+  DxlError ProcessDirectReadData(
+    uint8_t id,
+    const std::vector<uint16_t> & item_addrs,
+    const std::vector<uint8_t> & item_sizes,
+    const std::vector<std::shared_ptr<double>> & data_ptrs,
+    std::function<uint32_t(uint8_t, uint16_t, uint8_t)> get_data_func);
+
   // Read - Communication
   DxlError ProcessReadCommunication(
-    dynamixel::GroupFastSyncRead * group_sync_read,
-    dynamixel::GroupFastBulkRead * group_bulk_read,
     dynamixel::PortHandler * port_handler,
     double period_ms,
-    bool is_sync);
+    bool is_sync,
+    bool is_fast);
 
   // SyncWrite
   DxlError SetSyncWriteItemAndHandler();
@@ -243,8 +268,12 @@ class Dynamixel
   DxlError SetBulkWriteHandler(std::vector<uint8_t> id_arr);
   DxlError SetDxlValueToBulkWrite();
 
+  // Check Indirect Write
+  DxlError CheckIndirectWriteAvailable(uint8_t id);
+
   // Write - Indirect Address
   void ResetIndirectWrite(std::vector<uint8_t> id_arr);
+  void ResetDirectWrite(std::vector<uint8_t> id_arr);
   DxlError AddIndirectWrite(
     uint8_t id,
     std::string item_name,

include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 //
-// Authors: Hye-Jong KIM, Sungho Woo
+// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie
 
 #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_INFO_HPP_
 #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_INFO_HPP_

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 //
-// Authors: Hye-Jong KIM, Sungho Woo
+// Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie
 
 #ifndef DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL_HARDWARE_INTERFACE_HPP_
 #define DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL_HARDWARE_INTERFACE_HPP_
@@ -21,6 +21,8 @@
 #include <string>
 #include <vector>
 #include <map>
+#include <unordered_map>
+#include <functional>
 
 #include "rclcpp/rclcpp.hpp"
 #include "ament_index_cpp/get_package_share_directory.hpp"
@@ -29,6 +31,7 @@
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
 
 #include "dynamixel_hardware_interface/visibility_control.h"
 #include "dynamixel_hardware_interface/dynamixel/dynamixel.hpp"
@@ -43,13 +46,6 @@
 
 #include "std_srvs/srv/set_bool.hpp"
 
-#define PRESENT_POSITION_INDEX 0
-#define PRESENT_VELOCITY_INDEX 1
-#define PRESENT_EFFORT_INDEX 2
-
-#define GOAL_POSITION_INDEX 0
-// #define GOAL_VELOCITY_INDEX 1  // TODO: to be implemented
-#define GOAL_CURRENT_INDEX 1
 
 namespace dynamixel_hardware_interface
 {
@@ -185,14 +181,13 @@ class DynamixelHardware : public
   std::map<uint8_t /*id*/, uint8_t /*err*/> dxl_hw_err_;
   DxlTorqueStatus dxl_torque_status_;
   std::map<uint8_t /*id*/, bool /*enable*/> dxl_torque_state_;
+  std::map<uint8_t /*id*/, bool /*enable*/> dxl_torque_enable_;
   double err_timeout_ms_;
   rclcpp::Duration read_error_duration_{0, 0};
   rclcpp::Duration write_error_duration_{0, 0};
   bool is_read_in_error_{false};
   bool is_write_in_error_{false};
 
-  bool global_torque_enable_{true};
-
   bool use_revolute_to_prismatic_{false};
   std::string conversion_dxl_name_{""};
   std::string conversion_joint_name_{""};
@@ -229,7 +224,6 @@ class DynamixelHardware : public
   bool CommReset();
 
   ///// dxl variable
-  std::shared_ptr<Dynamixel> dxl_comm_;
   std::string port_name_;
   std::string baud_rate_;
   std::vector<uint8_t> dxl_id_;
@@ -250,6 +244,9 @@ class DynamixelHardware : public
   std::vector<HandlerVarType> hdl_gpio_sensor_states_;
   std::vector<HandlerVarType> hdl_sensor_states_;
 
+  ///// handler controller variable
+  std::vector<HandlerVarType> hdl_gpio_controller_commands_;
+
   bool is_set_hdl_{false};
 
   // joint <-> transmission matrix
@@ -360,6 +357,41 @@ class DynamixelHardware : public
   double revoluteToPrismatic(double revolute_value);
 
   double prismaticToRevolute(double prismatic_value);
+
+  void MapInterfaces(
+    size_t outer_size,
+    size_t inner_size,
+    std::vector<HandlerVarType> & outer_handlers,
+    const std::vector<HandlerVarType> & inner_handlers,
+    double ** matrix,
+    const std::unordered_map<std::string, std::vector<std::string>> & iface_map,
+    const std::string & conversion_iface = "",
+    const std::string & conversion_name = "",
+    std::function<double(double)> conversion = nullptr);
+
+  // Move dxl_comm_ to the end for safe destruction order
+  std::shared_ptr<Dynamixel> dxl_comm_;
+};
+
+// Conversion maps between ROS2 and Dynamixel interface names
+inline const std::unordered_map<std::string, std::vector<std::string>> ros2_to_dxl_cmd_map = {
+  {hardware_interface::HW_IF_POSITION, {"Goal Position"}},
+  {hardware_interface::HW_IF_VELOCITY, {"Goal Velocity"}},
+  {hardware_interface::HW_IF_EFFORT, {"Goal Current"}}
+};
+
+// Mapping for Dynamixel command interface names to ROS2 state interface names
+inline const std::unordered_map<std::string, std::vector<std::string>> dxl_to_ros2_cmd_map = {
+  {"Goal Position", {hardware_interface::HW_IF_POSITION}},
+  {"Goal Velocity", {hardware_interface::HW_IF_VELOCITY}},
+  {"Goal Current", {hardware_interface::HW_IF_EFFORT}}
+};
+
+// Mapping for ROS2 state interface names to Dynamixel state interface names
+inline const std::unordered_map<std::string, std::vector<std::string>> ros2_to_dxl_state_map = {
+  {hardware_interface::HW_IF_POSITION, {"Present Position"}},
+  {hardware_interface::HW_IF_VELOCITY, {"Present Velocity"}},
+  {hardware_interface::HW_IF_EFFORT, {"Present Current", "Present Load"}}
 };
 
 }  // namespace dynamixel_hardware_interface

package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dynamixel_hardware_interface</name>
-  <version>1.4.3</version>
+  <version>1.4.4</version>
   <description>
     ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
   </description>

param/dxl_model/dynamixel.model

@@ -46,3 +46,5 @@ Number	Name
 35074	rh_p12_rn.model
 220	omy_hat.model
 230	omy_end.model
+536 sensorxel_joy.model
+600 sensorxel_joy.model

param/dxl_model/ph42_020_s300.model

@@ -62,7 +62,9 @@ Address	Size	Data Name
 592	2	Present Input Voltage
 594	1	Present Temperature
 878	1	Backup Ready
+168	2	Indirect Address 1
+634	1	Indirect Data 1
 168	2	Indirect Address Write
 634	1	Indirect Data Write
-350	2	Indirect Address Read
+296	2	Indirect Address Read
 698	1	Indirect Data Read

param/dxl_model/sensorxel_joy.model

@@ -0,0 +1,21 @@
+[control table]
+Address	Size    Data Name
+0	2	Model Number
+2	1	ROBOT_Generation
+3	1	Board_Number
+5	1	Firmware_Version_Major
+6	1	Firmware_Version_Minor
+7	1	ID
+8	1	Baud Rate
+10	1	Boot_Version_Major
+11	1	Boot_Version_MInor
+12	4	Error_Code
+20	4	Micros
+24	4	Millis
+28	1	JOYSTICK TACT SWITCH
+29	2	JOYSTICK X VALUE
+31	2	JOYSTICK Y VALUE
+40	2	IMU ACC X
+42	2	IMU ACC Y
+44	2	IMU ACC Z
+46	2	IMU ACC SUM