Merge pull request #47 from ROBOTIS-GIT/feature-rcu

Add virtual_dxl feaure

Author: Woojin-Crive

CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package dynamixel_hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.4.7 (2025-06-19)
+------------------
+* Added virtual_dxl and support for rcu
+* Contributors: Woojin Wie
+
 1.4.6 (2025-05-30)
 ------------------
 * Changed dynamixel_sdk_TARGETS to dynamixel_sdk_LIBRARIES in target_link_libraries

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 communication operations
+#define MAX_COMM_RETRIES 5  ///< Maximum number of retries for communication 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;                 ///< IDs of the Dynamixel motors.
   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,12 @@ 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;}
+
+  DxlError InitTorqueStates(std::vector<uint8_t> id_arr, bool disable_torque = false);
+
 private:
   bool checkReadType();
   bool checkWriteType();
@@ -237,8 +249,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/dynamixel_info.hpp

@@ -47,6 +47,7 @@ typedef  struct
   int32_t value_of_max_radian_position;
   int32_t value_of_min_radian_position;
   uint16_t model_num;
+  double velocity_unit;
 
   std::vector<ControlItem> item;
 } DxlInfo;
@@ -72,24 +73,25 @@ class DynamixelInfo
   void ReadDxlModelFile(uint8_t id, uint16_t model_num);
   bool GetDxlControlItem(uint8_t id, std::string item_name, uint16_t & addr, uint8_t & size);
   bool CheckDxlControlItem(uint8_t id, std::string item_name);
-  bool GetDxlTypeInfo(
-    uint8_t id,
-    int32_t & value_of_zero_radian_position,
-    int32_t & value_of_max_radian_position,
-    int32_t & value_of_min_radian_position,
-    double & min_radian,
-    double & max_radian,
-    double & torque_constant);
+  // bool GetDxlTypeInfo(
+  //   uint8_t id,
+  //   int32_t & value_of_zero_radian_position,
+  //   int32_t & value_of_max_radian_position,
+  //   int32_t & value_of_min_radian_position,
+  //   double & min_radian,
+  //   double & max_radian,
+  //   double & torque_constant,
+  //   double & velocity_unit);
   int32_t ConvertRadianToValue(uint8_t id, double radian);
   double ConvertValueToRadian(uint8_t id, int32_t value);
   inline int16_t ConvertEffortToCurrent(uint8_t id, double effort)
   {return static_cast<int16_t>(effort / dxl_info_[id].torque_constant);}
   inline double ConvertCurrentToEffort(uint8_t id, int16_t current)
   {return static_cast<double>(current * dxl_info_[id].torque_constant);}
-  inline double ConvertValueRPMToVelocityRPS(int32_t value_rpm)
-  {return static_cast<double>(value_rpm * 0.01 / 60.0 * 2.0 * M_PI);}
-  inline int32_t ConvertVelocityRPSToValueRPM(double vel_rps)
-  {return static_cast<int32_t>(vel_rps * 100.0 * 60.0 / 2.0 / M_PI);}
+  inline double ConvertValueRPMToVelocityRPS(uint8_t id, int32_t value_rpm)
+  {return static_cast<double>(value_rpm * dxl_info_[id].velocity_unit / 60.0 * 2.0 * M_PI);}
+  inline int32_t ConvertVelocityRPSToValueRPM(uint8_t id, double vel_rps)
+  {return static_cast<int32_t>(vel_rps / dxl_info_[id].velocity_unit * 60.0 / 2.0 / M_PI);}
 };
 
 }  // namespace dynamixel_hardware_interface

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};
@@ -262,15 +265,6 @@ class DynamixelHardware : public
    */
   bool initItems(const std::string & type_filter);
 
-  /**
-   * @brief Helper function to retry writing an item to a Dynamixel device.
-   * @param id The ID of the Dynamixel device.
-   * @param item_name The name of the item to write.
-   * @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);
-
   /**
    * @brief Initializes Dynamixel items.
    * @return True if initialization was successful, false otherwise.
@@ -362,7 +356,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 = "",

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.6</version>
+  <version>1.4.7</version>
   <description>
     ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
   </description>

param/dxl_model/2xc430_w250.model

@@ -5,6 +5,7 @@ value_of_max_radian_position	4095
 value_of_min_radian_position	0
 min_radian	-3.14159265
 max_radian	3.14159265
+velocity_unit	0.229
 
 [control table]
 Address	Size	Data Name

param/dxl_model/2xl430_w250.model

@@ -5,6 +5,7 @@ value_of_max_radian_position	4095
 value_of_min_radian_position	0
 min_radian	-3.14159265
 max_radian	3.14159265
+velocity_unit	0.229
 
 [control table]
 Address	Size	Data Name

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