Refactor: Move Hardware Error Status and Error Code definitions to dynamixel.hpp

Woojin-Crive · Woojin-Crive · commit 124ab3f176a3 · 2025-09-16T13:27:01.000+09:00
- Moved HardwareErrorStatusBitInfo and ErrorCodeInfo structures along with their corresponding tables and functions from dynamixel_hardware_interface.hpp to dynamixel.hpp for better organization.
- Updated the Dynamixel class to utilize the new error handling structures and provide detailed error reporting for hardware status and error codes.
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/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp
@@ -201,10 +201,63 @@ DxlError Dynamixel::InitDxlComm(
       return DxlError::CANNOT_FIND_CONTROL_ITEM;
     } else 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;
+        DxlError hw_error_result = ReadItem(it_id, "Hardware Error Status", hw_error_status);
+
+        if (hw_error_result == DxlError::OK) {
+          fprintf(stderr, "[ID:%03d] Read Hardware Error Status : 0x%x\n", it_id, 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;
+        DxlError error_code_result = ReadItem(it_id, "Error Code", error_code);
+
+        if (error_code_result == DxlError::OK) {
+          fprintf(stderr, "[ID:%03d] Read Error Code : 0x%x\n", it_id, 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;
