Added Dynamixel Y error code handling logic

CMakeLists.txt

@@ -1,30 +1,25 @@
-################################################################################
-# Set minimum required version of cmake, project name and compile options
-################################################################################
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.16)
 project(dynamixel_hardware_interface)
 
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  hardware_interface
+  rclcpp
+  rclcpp_lifecycle
+  pluginlib
+  realtime_tools
+  dynamixel_sdk
+  std_srvs
+  dynamixel_interfaces
+)
 
-################################################################################
-# Find and load build settings from external packages
-################################################################################
 find_package(ament_cmake REQUIRED)
-find_package(hardware_interface REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
-find_package(pluginlib REQUIRED)
-find_package(realtime_tools REQUIRED)
-
-find_package(dynamixel_sdk REQUIRED)
-find_package(std_srvs REQUIRED)
-find_package(dynamixel_interfaces REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
 
 ################################################################################
 # Build
@@ -36,42 +31,35 @@ add_library(
   src/dynamixel/dynamixel_info.cpp
   src/dynamixel/dynamixel.cpp
 )
-
-target_include_directories(
-  ${PROJECT_NAME}
-  PRIVATE
-  include
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/dynamixel_hardware_interface>
   ${dynamixel_sdk_INCLUDE_DIRS}
-  ${hardware_interface_INCLUDE_DIRS}
-  ${realtime_tools_INCLUDE_DIRS}
-  ${rclcpp_lifecycle_INCLUDE_DIRS}
 )
 
-target_link_libraries(${PROJECT_NAME}
-  PUBLIC
-  ${hardware_interface_TARGETS}
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  hardware_interface::hardware_interface
   pluginlib::pluginlib
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
+  realtime_tools::realtime_tools
   ${dynamixel_sdk_LIBRARIES}
   ${std_srvs_TARGETS}
   ${dynamixel_interfaces_TARGETS}
-  ${realtime_tools_TARGETS}
 )
 
 pluginlib_export_plugin_description_file(hardware_interface dynamixel_hardware_interface_plugin.xml)
 
-################################################################################
-# Install
-################################################################################
-install(TARGETS ${PROJECT_NAME}
-  DESTINATION lib
+install(
+  DIRECTORY include/
+  DESTINATION include/dynamixel_hardware_interface
 )
-
-install(DIRECTORY include/
-  DESTINATION include
+install(TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
 install(PROGRAMS
@@ -93,18 +81,6 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-################################################################################
-# Macro for ament package
-################################################################################
-ament_export_include_directories(include)
-ament_export_libraries(${PROJECT_NAME})
-ament_export_dependencies(
-  rclcpp
-  rclcpp_lifecycle
-  hardware_interface
-  pluginlib
-  dynamixel_sdk
-  dynamixel_interfaces
-)
-
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_package()

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,15 @@ 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

@@ -38,18 +38,25 @@ typedef struct
   std::string item_name;
 } ControlItem;
 
+typedef struct
+{
+  std::string data_name;
+  double value;
+  bool is_signed;
+} UnitItem;
+
 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 +80,47 @@ 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;
+  }
+  // fprintf(stderr, "[WARN] No unit mapping found for '%s' in ID %d, returning raw value\n", data_name.c_str(), id);
+  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);
+  }
+  // fprintf(stderr, "[WARN] No unit mapping found for '%s' in ID %d, returning raw value\n", data_name.c_str(), id);
+  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,66 @@
 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 +235,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 +360,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 +409,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,