Merge pull request #85 from ROBOTIS-GIT/feature-hardware-error-handling

Woojin-Crive · web-flow · commit 49a8b09fef16 · 2025-09-18T19:51:35.000+09:00
Hardware error handling for Dynamixel Y series
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package dynamixel_hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.4.15 (2025-09-17)
+-------------------
+* Added support for hardware error handling for Dynamixel Y series
+* Contributors: Woojin Wie
+
 1.4.14 (2025-09-03)
 -------------------
 * Added Current units to YM070-210-*099 and YM080-230-*099 model files
diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp
@@ -71,6 +71,80 @@ enum DxlError
   DXL_REBOOT_FAIL = -17            ///< Reboot failed.
 };
 
+// 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;
+}
+
 /**
  * @struct IndirectInfo
  * @brief Structure for storing indirect addressing information for Dynamixel motors.
diff --git a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp
@@ -49,81 +49,6 @@
 
 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.
  */
diff --git a/package.xml b/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.14</version>
+  <version>1.4.15</version>
   <description>
     ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
   </description>
diff --git a/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp
@@ -199,13 +199,76 @@ DxlError Dynamixel::InitDxlComm(
     if (dxl_comm_result != COMM_SUCCESS) {
       fprintf(stderr, " - COMM_ERROR : %s\n", packet_handler_->getTxRxResult(dxl_comm_result));
       return DxlError::CANNOT_FIND_CONTROL_ITEM;
-    } else if (dxl_error != 0) {
+    }
+
+    // First, read the model file to get the control table structure
+    try {
+      dxl_info_.ReadDxlModelFile(it_id, dxl_model_number);
+    } catch (const std::exception & e) {
+      fprintf(stderr, "[InitDxlComm][ID:%03d] Error reading model file: %s\n", it_id, e.what());
+      return DxlError::CANNOT_FIND_CONTROL_ITEM;
+    }
+
+    if (dxl_error != 0) {
       fprintf(stderr, " - RX_PACKET_ERROR : %s\n", packet_handler_->getRxPacketError(dxl_error));
-      uint32_t err = 0;
-      if (ReadItem(it_id, "Hardware Error Status", err) == DxlError::OK) {
-        fprintf(stderr, "[ID:%03d] Read Hardware Error Status : %x\n", it_id, err);
+
+      // Check if Hardware Error Status control item exists
+      uint16_t hw_error_addr, error_code_addr;
+      uint8_t hw_error_size, error_code_size;
+      bool hw_error_exists = dxl_info_.GetDxlControlItem(
+        it_id, "Hardware Error Status", hw_error_addr, hw_error_size);
+      bool error_code_exists = dxl_info_.GetDxlControlItem(
+        it_id, "Error Code", error_code_addr, error_code_size);
+
+      if (hw_error_exists) {
+        uint32_t hw_error_status = 0;
+        ReadItem(it_id, "Hardware Error Status", hw_error_status);
+
+        std::string error_string = "";
+        uint8_t hw_error_byte = static_cast<uint8_t>(hw_error_status);
+
+        for (int bit = 0; bit < 8; ++bit) {
+          if (hw_error_byte & (1 << bit)) {
+            const HardwareErrorStatusBitInfo * bit_info = get_hardware_error_status_bit_info(bit);
+            if (bit_info) {
+              error_string += bit_info->label;
+              error_string += " (" + std::string(bit_info->description) + ")/ ";
+            } else {
+              error_string += "Unknown Error Bit " + std::to_string(bit) + "/ ";
+            }
+          }
+        }
+
+        if (!error_string.empty()) {
+          fprintf(
+            stderr, "[ID:%03d] Hardware Error Details: 0x%x (%d): %s\n",
+            it_id, hw_error_byte, hw_error_byte, error_string.c_str());
+        }
+      } else if (error_code_exists) {
+        uint32_t error_code = 0;
+        ReadItem(it_id, "Error Code", error_code);
+
+        uint8_t error_code_byte = static_cast<uint8_t>(error_code);
+        if (error_code_byte != 0x00) {
+          const ErrorCodeInfo * error_info = get_error_code_info(error_code_byte);
+          if (error_info) {
+            fprintf(
+              stderr, "[ID:%03d] Error Code Details: 0x%x (%s): %s\n",
+              it_id, error_code_byte, error_info->label, error_info->description);
+          } else {
+            fprintf(
+              stderr, "[ID:%03d] Error Code Details: 0x%x (Unknown Error Code)\n",
+              it_id, error_code_byte);
+          }
+        }
+      } else {
+        fprintf(
+          stderr,
+          "[ID:%03d] Neither Hardware Error Status nor Error Code control items available.\n",
+          it_id
+        );
       }
-      fprintf(stderr, "[ID:%03d] Hardware Error detected, rebooting...\n", it_id);
+
       Reboot(it_id);
       return DxlError::DXL_HARDWARE_ERROR;
     } else {
@@ -215,14 +278,6 @@ DxlError Dynamixel::InitDxlComm(
         model_name.c_str());
     }
 
-    // First, read the model file to get the control table structure
-    try {
-      dxl_info_.ReadDxlModelFile(it_id, dxl_model_number);
-    } catch (const std::exception & e) {
-      fprintf(stderr, "[InitDxlComm][ID:%03d] Error reading model file: %s\n", it_id, e.what());
-      return DxlError::CANNOT_FIND_CONTROL_ITEM;
-    }
-
     // Read firmware version and reload model file with firmware-specific version if available
     uint8_t firmware_version = 0;
     DxlError fw_result = ReadFirmwareVersion(it_id, firmware_version);
@@ -725,6 +780,16 @@ DxlError Dynamixel::ReadItem(uint8_t id, std::string item_name, uint32_t & data)
         return DxlError::ITEM_READ_FAIL;
       }
     } else if (dxl_error != 0) {
+      bool is_alert = dxl_error & 0x80;
+      if (is_alert) {
+        fprintf(
+          stderr,
+          "[ReadItem][ID:%03d][comm_id:%03d] RX_PACKET_ERROR : %s\n",
+          id,
+          comm_id,
+          packet_handler_->getRxPacketError(dxl_error));
+        return DxlError::OK;
+      }
       fprintf(
         stderr,
         "[ReadItem][ID:%03d][comm_id:%03d] RX_PACKET_ERROR : %s (retry %d/%d)\n",
