Add virtual_dxl feaure

include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp

@@ -45,6 +45,9 @@ namespace dynamixel_hardware_interface
 #define SYNC 0  ///< Synchronous communication.
 #define BULK 1  ///< Bulk communication.
 
+/// @brief Maximum number of retries for write operations
+#define MAX_WRITE_RETRIES 10  ///< Maximum number of retries for write operations
+
 /// @brief Error codes for Dynamixel operations.
 enum DxlError
 {
@@ -99,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.
@@ -156,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();
@@ -167,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();
 
@@ -183,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);
 
@@ -205,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();
@@ -237,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_;
@@ -245,6 +247,7 @@ class DynamixelHardware : public
   std::vector<HandlerVarType> hdl_sensor_states_;
 
   ///// handler controller variable
+  std::vector<HandlerVarType> hdl_gpio_controller_states_;
   std::vector<HandlerVarType> hdl_gpio_controller_commands_;
 
   bool is_set_hdl_{false};
@@ -269,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.
@@ -362,7 +365,7 @@ class DynamixelHardware : public
     size_t outer_size,
     size_t inner_size,
     std::vector<HandlerVarType> & outer_handlers,
-    const std::vector<HandlerVarType> & inner_handlers,
+    std::vector<HandlerVarType> & inner_handlers,
     double ** matrix,
     const std::unordered_map<std::string, std::vector<std::string>> & iface_map,
     const std::string & conversion_iface = "",

param/dxl_model/dynamixel.model

@@ -48,3 +48,10 @@ Number	Name
 230	omy_end.model
 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