fix: Enhance error handling in Dynamixel::InitDxlComm and ReadItem methods

Author: Woojin-Crive

src/dynamixel/dynamixel.cpp

@@ -199,7 +199,17 @@ 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));
 
       // Check if Hardware Error Status control item exists
@@ -214,46 +224,38 @@ DxlError Dynamixel::InitDxlComm(
         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) + "/ ";
-              }
+        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());
-          }
+        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);
-            }
+        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 {
@@ -264,7 +266,6 @@ DxlError Dynamixel::InitDxlComm(
         );
       }
 
-      fprintf(stderr, "[ID:%03d] Hardware Error detected, rebooting...\n", it_id);
       Reboot(it_id);
       return DxlError::DXL_HARDWARE_ERROR;
     } else {
@@ -274,14 +275,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);
@@ -784,6 +777,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",