Merge pull request #65 from ROBOTIS-GIT/feature-dy-error-code

Added Dynamixel Y error code handling logic

Author: Woojin-Crive

CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package dynamixel_hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.4.10 (2025-07-18)
+------------------
+* Added unit system to model files
+* Added support for Dynamixel Y Error Code handling
+* Contributors: Woojin Wie
+
 1.4.9 (2025-06-24)
 ------------------
 * Support ffw sensor model

CMakeLists.txt

@@ -39,9 +39,8 @@ add_library(
 
 target_include_directories(
   ${PROJECT_NAME}
-  PRIVATE
-  include
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
   ${dynamixel_sdk_INCLUDE_DIRS}
   ${hardware_interface_INCLUDE_DIRS}
@@ -67,7 +66,10 @@ pluginlib_export_plugin_description_file(hardware_interface dynamixel_hardware_i
 # Install
 ################################################################################
 install(TARGETS ${PROJECT_NAME}
-  DESTINATION lib
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
 install(DIRECTORY include/
@@ -98,6 +100,7 @@ endif()
 ################################################################################
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 ament_export_dependencies(
   rclcpp
   rclcpp_lifecycle

include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp

@@ -67,7 +67,7 @@ enum DxlError
   SET_BULK_READ_FAIL = -13,        ///< Failed to configure bulk read.
   SET_READ_ITEM_FAIL = -14,        ///< Failed to set read item.
   SET_WRITE_ITEM_FAIL = -15,       ///< Failed to set write item.
-  DLX_HARDWARE_ERROR = -16,        ///< Hardware error detected.
+  DXL_HARDWARE_ERROR = -16,        ///< Hardware error detected.
   DXL_REBOOT_FAIL = -17            ///< Reboot failed.
 };
 
@@ -260,6 +260,7 @@ class Dynamixel
   DxlError ProcessDirectReadData(
     uint8_t id,
     const std::vector<uint16_t> & item_addrs,
+    const std::vector<std::string> & item_names,
     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);
@@ -292,6 +293,25 @@ class Dynamixel
     std::string item_name,
     uint16_t item_addr,
     uint8_t item_size);
+
+  // Helper function for value conversion with unit info
+  double ConvertValueWithUnitInfo(
+    uint8_t id,
+    std::string item_name,
+    uint32_t raw_value,
+    uint8_t size,
+    bool is_signed);
+
+  // Helper function for converting unit values to raw values
+  uint32_t ConvertUnitValueToRawValue(
+    uint8_t id,
+    std::string item_name,
+    double unit_value,
+    uint8_t size,
+    bool is_signed);
+
+  // Helper function for writing values to buffer
+  void WriteValueToBuffer(uint8_t * buffer, uint8_t offset, uint32_t value, uint8_t size);
 };
 
 }  // namespace dynamixel_hardware_interface

include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp

@@ -40,16 +40,16 @@ typedef struct
 
 typedef  struct
 {
-  double torque_constant;
   double min_radian;
   double max_radian;
   int32_t value_of_zero_radian_position;
   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;
+  std::map<std::string, double> unit_map;
+  std::map<std::string, bool> sign_type_map;
 } DxlInfo;
 
 class DynamixelInfo
@@ -73,27 +73,45 @@ 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,
-  //   double & velocity_unit);
+
+  bool GetDxlUnitValue(uint8_t id, std::string data_name, double & unit_value);
+  bool GetDxlSignType(uint8_t id, std::string data_name, bool & is_signed);
+
+  // Template-based conversion methods
+  template<typename T>
+  double ConvertValueToUnit(uint8_t id, std::string data_name, T value);
+
+  template<typename T>
+  T ConvertUnitToValue(uint8_t id, std::string data_name, double unit_value);
+
+  // Helper method for internal use
+  double GetUnitMultiplier(uint8_t id, std::string data_name);
+
   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(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);}
 };
 
+// Template implementations
+template<typename T>
+double DynamixelInfo::ConvertValueToUnit(uint8_t id, std::string data_name, T value)
+{
+  auto it = dxl_info_[id].unit_map.find(data_name);
+  if (it != dxl_info_[id].unit_map.end()) {
+    return static_cast<double>(value) * it->second;
+  }
+  return static_cast<double>(value);
+}
+
+template<typename T>
+T DynamixelInfo::ConvertUnitToValue(uint8_t id, std::string data_name, double unit_value)
+{
+  auto it = dxl_info_[id].unit_map.find(data_name);
+  if (it != dxl_info_[id].unit_map.end()) {
+    return static_cast<T>(unit_value / it->second);
+  }
+  return static_cast<T>(unit_value);
+}
+
 }  // namespace dynamixel_hardware_interface
 
 #endif  // DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_INFO_HPP_

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -50,6 +50,80 @@
 namespace dynamixel_hardware_interface
 {
 
+// Hardware Error Status bit definitions
+struct HardwareErrorStatusBitInfo
+{
+  int bit;
+  const char * label;
+  const char * description;
+};
+
+static constexpr HardwareErrorStatusBitInfo HardwareErrorStatusTable[] = {
+  {0, "Input Voltage Error", "Detects that input voltage exceeds the configured operating voltage"},
+  {1, "Motor Hall Sensor Error", "Detects that Motor hall sensor value exceeds normal range"},
+  {2, "Overheating Error",
+    "Detects that internal temperature exceeds the configured operating temperature"},
+  {3, "Motor Encoder Error", "Detects malfunction of the motor encoder"},
+  {4,
+    "Electrical Shock Error",
+    "Detects electric shock on the circuit or insufficient power to operate the motor"},
+  {5, "Overload Error", "Detects that persistent load exceeds maximum output"},
+  {6, "Not used", "Always 0"},
+  {7, "Not used", "Always 0"}
+};
+
+inline const HardwareErrorStatusBitInfo * get_hardware_error_status_bit_info(int bit)
+{
+  for (const auto & entry : HardwareErrorStatusTable) {
+    if (entry.bit == bit) {return &entry;}
+  }
+  return nullptr;
+}
+
+// Error Code (153) definitions
+struct ErrorCodeInfo
+{
+  int value;
+  const char * label;
+  const char * description;
+};
+
+static constexpr ErrorCodeInfo ErrorCodeTable[] = {
+  {0x00, "No Error", "No error"},
+  {0x01, "Over Voltage Error", "Device supply voltage exceeds the Max Voltage Limit(60)"},
+  {0x02, "Low Voltage Error", "Device supply voltage exceeds the Min Voltage Limit(62)"},
+  {0x03,
+    "Inverter Overheating Error",
+    "The inverter temperature has exceeded the Inverter Temperature Limit(56)"},
+  {0x04,
+    "Motor Overheating Error",
+    "The motor temperature has exceeded the Motor Temperature Limit(57)"},
+  {0x05, "Overload Error", "Operating current exceeding rated current for an extended duration"},
+  {0x07, "Inverter Error", "An issue has occurred with the inverter"},
+  {0x09, "Battery Warning", "Low Multi-turn battery voltage. Replacement recommended"},
+  {0x0A, "Battery Error", "Multi-turn battery voltage is too low, causing issues"},
+  {0x0B, "Magnet Error", "Multi-turn position lost. Multi-turn reset required"},
+  {0x0C, "Multi-turn Error", "An issue has occurred with the Multi-turn IC"},
+  {0x0D, "Encoder Error", "An issue has occurred with the Encoder IC"},
+  {0x0E, "Hall Sensor Error", "An issue has occurred with the Hall Sensor"},
+  {0x0F, "Calibration Error", "Cannot find calibration Data"},
+  {0x11, "Following Error", "Position control error exceeds the Following Error Threshold(44)"},
+  {0x12, "Bus Watchdog Error", "An issue has occurred with the Bus Watchdog"},
+  {0x13, "Over Speed Error", "Rotates at a speed of 120% or more than the Velocity Limit(72)"},
+  {0x14,
+    "Position Limit Reached Error",
+    "In position control mode, the current position has moved beyond the Max/Min Position Limit"
+    " + Position Limit Threshold(38) range."}
+};
+
+inline const ErrorCodeInfo * get_error_code_info(int value)
+{
+  for (const auto & entry : ErrorCodeTable) {
+    if (entry.value == value) {return &entry;}
+  }
+  return nullptr;
+}
+
 /**
  * @brief Constants for hardware state interface names.
  */
@@ -175,14 +249,16 @@ class DynamixelHardware : public
 private:
   ///// ros
   rclcpp::Logger logger_;
+  rclcpp::Clock clock_;
 
   ///// dxl error
   DxlStatus dxl_status_;
   DxlError dxl_comm_err_;
   std::map<uint8_t /*id*/, uint8_t /*err*/> dxl_hw_err_;
+  std::map<uint8_t /*id*/, uint8_t /*error code*/> dxl_error_code_;
   DxlTorqueStatus dxl_torque_status_;
   std::map<uint8_t /*id*/, bool /*enable*/> dxl_torque_state_;
-  std::map<uint8_t /*id*/, bool /*enable*/> dxl_torque_enable_;
+  std::vector<uint8_t> torque_enabled_ids_;
   double err_timeout_ms_;
   rclcpp::Duration read_error_duration_{0, 0};
   rclcpp::Duration write_error_duration_{0, 0};
@@ -298,8 +374,9 @@ class DynamixelHardware : public
   ///// function
   /**
    * @brief Sets up the joint-to-transmission and transmission-to-joint matrices.
+   * @return True if matrices were set up successfully, false otherwise.
    */
-  void SetMatrix();
+  bool SetMatrix();
 
   /**
    * @brief Calculates the joint states from transmission states.
@@ -346,12 +423,30 @@ class DynamixelHardware : public
     const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
     std::shared_ptr<std_srvs::srv::SetBool::Response> response);
 
+  rclcpp::Service<dynamixel_interfaces::srv::GetDataFromDxl>::SharedPtr get_dxl_error_summary_srv_;
+  void get_dxl_error_summary_srv_callback(
+    const std::shared_ptr<dynamixel_interfaces::srv::GetDataFromDxl::Request> request,
+    std::shared_ptr<dynamixel_interfaces::srv::GetDataFromDxl::Response> response);
+
   void initRevoluteToPrismaticParam();
 
   double revoluteToPrismatic(double revolute_value);
 
   double prismaticToRevolute(double prismatic_value);
 
+  /**
+   * @brief Get a formatted error summary for a specific Dynamixel ID
+   * @param id The Dynamixel ID
+   * @return A string containing the error summary
+   */
+  std::string getErrorSummary(uint8_t id) const;
+
+  /**
+   * @brief Get error summaries for all Dynamixel IDs
+   * @return A string containing error summaries for all Dynamixels
+   */
+  std::string getAllErrorSummaries() const;
+
   void MapInterfaces(
     size_t outer_size,
     size_t inner_size,

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.9</version>
+  <version>1.4.10</version>
   <description>
     ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
   </description>
@@ -12,16 +12,20 @@
   <author email="[email protected]">Sungho Woo</author>
   <author email="[email protected]">Woojin Wie</author>
   <author email="[email protected]">Wonho Yun</author>
+
   <buildtool_depend>ament_cmake</buildtool_depend>
+
   <depend>rclcpp</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
   <depend>realtime_tools</depend>
   <depend>dynamixel_sdk</depend>
   <depend>std_srvs</depend>
   <depend>dynamixel_interfaces</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

param/dxl_model/2xc430_w250.model

@@ -5,7 +5,11 @@ value_of_max_radian_position	4095
 value_of_min_radian_position	0
 min_radian	-3.14159265
 max_radian	3.14159265
-velocity_unit	0.229
+
+[unit info]
+Data Name	value	unit	Sign Type
+Present Velocity	0.0239691227	rad/s	signed
+Goal Velocity	0.0239691227	rad/s	signed
 
 [control table]
 Address	Size	Data Name

param/dxl_model/2xl430_w250.model

@@ -5,7 +5,11 @@ value_of_max_radian_position	4095
 value_of_min_radian_position	0
 min_radian	-3.14159265
 max_radian	3.14159265
-velocity_unit	0.229
+
+[unit info]
+Data Name	value	unit	Sign Type
+Present Velocity	0.0239691227	rad/s	signed
+Goal Velocity	0.0239691227	rad/s	signed
 
 [control table]
 Address	Size	Data Name

param/dxl_model/ffw_sg2_drive_1.model

@@ -5,7 +5,11 @@ value_of_max_radian_position	262144
 value_of_min_radian_position	-262144
 min_radian	-3.14159265
 max_radian	3.14159265
-velocity_unit	0.01
+
+[unit info]
+Data Name	value	unit	Sign Type
+Present Velocity	0.0010471976	rad/s	signed
+Goal Velocity	0.0010471976	rad/s	signed
 
 [control table]
 Address	Size	Data Name

param/dxl_model/ffw_sg2_drive_2.model

@@ -5,7 +5,11 @@ value_of_max_radian_position	262144
 value_of_min_radian_position	-262144
 min_radian	-3.14159265
 max_radian	3.14159265
-velocity_unit	0.01
+
+[unit info]
+Data Name	value	unit	Sign Type
+Present Velocity	0.0010471976	rad/s	signed
+Goal Velocity	0.0010471976	rad/s	signed
 
 [control table]
 Address	Size	Data Name