feat: Enhance Dynamixel interface with communication ID handling and virtual DXL support

- Added `comm_id` to `HandlerVarType` for better communication management.
- Updated methods to utilize `comm_id` for reading and writing operations.
- Introduced handling for virtual DXL components in GPIO initialization and state management.
- Improved error handling and logging for GPIO parameter checks.
- Updated model files to include new virtual DXL models.

Author: Woojin-Crive

include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp

@@ -102,7 +102,8 @@ typedef struct
  */
 typedef struct
 {
-  uint8_t id;                                  ///< ID of the Dynamixel motor.
+  uint8_t comm_id;                             ///< ID of the Dynamixel to be communicated.
+  std::vector<uint8_t> id_arr;                 ///< ID of the Dynamixel motor.
   std::vector<std::string> item_name;          ///< List of control item names.
   std::vector<uint8_t> item_size;              ///< Sizes of the control items.
   std::vector<uint16_t> item_addr;             ///< Addresses of the control items.
@@ -159,6 +160,8 @@ class Dynamixel
   // direct inform for bulk write
   std::map<uint8_t /*id*/, IndirectInfo> direct_info_write_;
 
+  std::map<uint8_t /*id*/, uint8_t> comm_id_;
+
 public:
   explicit Dynamixel(const char * path);
   ~Dynamixel();
@@ -170,13 +173,13 @@ class Dynamixel
 
   // DXL Read Setting
   DxlError SetDxlReadItems(
-    uint8_t id, std::vector<std::string> item_names,
+    uint8_t id, uint8_t comm_id, std::vector<std::string> item_names,
     std::vector<std::shared_ptr<double>> data_vec_ptr);
   DxlError SetMultiDxlRead();
 
   // DXL Write Setting
   DxlError SetDxlWriteItems(
-    uint8_t id, std::vector<std::string> item_names,
+    uint8_t id, uint8_t comm_id, std::vector<std::string> item_names,
     std::vector<std::shared_ptr<double>> data_vec_ptr);
   DxlError SetMultiDxlWrite();
 
@@ -186,7 +189,7 @@ class Dynamixel
   DxlError WriteMultiDxlData();
 
   // Set Dxl Option
-  DxlError SetOperatingMode(uint8_t id, uint8_t dynamixel_mode);
+  // DxlError SetOperatingMode(uint8_t id, uint8_t dynamixel_mode);
   DxlError DynamixelEnable(std::vector<uint8_t> id_arr);
   DxlError DynamixelDisable(std::vector<uint8_t> id_arr);
 
@@ -208,6 +211,10 @@ class Dynamixel
 
   static std::string DxlErrorToString(DxlError error_num);
 
+  DxlError ReadDxlModelFile(uint8_t id, uint16_t model_num);
+
+  void SetCommId(uint8_t id, uint8_t comm_id) {comm_id_[id] = comm_id;}
+
 private:
   bool checkReadType();
   bool checkWriteType();
@@ -240,8 +247,9 @@ class Dynamixel
 
   // Read - Data Processing
   DxlError ProcessReadData(
-    uint8_t id,
+    uint8_t comm_id,
     uint16_t indirect_addr,
+    const std::vector<uint8_t> & id_arr,
     const std::vector<std::string> & item_names,
     const std::vector<uint8_t> & item_sizes,
     const std::vector<std::shared_ptr<double>> & data_ptrs,

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -62,6 +62,7 @@ constexpr char HW_IF_TORQUE_ENABLE[] = "torque_enable";
 typedef struct HandlerVarType_
 {
   uint8_t id;                                /**< ID of the Dynamixel component. */
+  uint8_t comm_id;                           /**< ID of the Dynamixel to be communicated. */
   std::string name;                          /**< Name of the component. */
   std::vector<std::string> interface_name_vec; /**< Vector of interface names. */
   std::vector<std::shared_ptr<double>> value_ptr_vec; /**< Vector interface values. */
@@ -227,6 +228,7 @@ class DynamixelHardware : public
   std::string port_name_;
   std::string baud_rate_;
   std::vector<uint8_t> dxl_id_;
+  std::vector<uint8_t> virtual_dxl_id_;
 
   std::vector<uint8_t> sensor_id_;
   std::map<uint8_t /*id*/, std::string /*interface_name*/> sensor_item_;
@@ -270,7 +272,7 @@ class DynamixelHardware : public
    * @param value The value to write.
    * @return True if write was successful, false if timeout occurred.
    */
-  bool retryWriteItem(uint8_t id, const std::string & item_name, uint32_t value);
+  // bool retryWriteItem(uint8_t id, const std::string & item_name, uint32_t value);
 
   /**
    * @brief Initializes Dynamixel items.

param/dxl_model/dynamixel.model

@@ -49,3 +49,9 @@ Number	Name
 536 sensorxel_joy.model
 600 sensorxel_joy.model
 602 ffw_g10_rcu.model
+620 ffw_sg2_steer_1.model
+621 ffw_sg2_steer_2.model
+622 ffw_sg2_steer_3.model
+623 ffw_sg2_drive_1.model
+624 ffw_sg2_drive_2.model
+625 ffw_sg2_drive_3.model