refactor: reorganize dxl_comm_ member for safe destruction and improve error handling in stop method

Author: Woojin-Crive

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -224,7 +224,6 @@ class DynamixelHardware : public
   bool CommReset();
 
   ///// dxl variable
-  std::shared_ptr<Dynamixel> dxl_comm_;
   std::string port_name_;
   std::string baud_rate_;
   std::vector<uint8_t> dxl_id_;
@@ -369,6 +368,9 @@ class DynamixelHardware : public
     const std::string & conversion_iface = "",
     const std::string & conversion_name = "",
     std::function<double(double)> conversion = nullptr);
+
+  // Move dxl_comm_ to the end for safe destruction order
+  std::shared_ptr<Dynamixel> dxl_comm_;
 };
 
 // Conversion maps between ROS2 and Dynamixel interface names

src/dynamixel/dynamixel.cpp

@@ -44,6 +44,7 @@ Dynamixel::Dynamixel(const char * path)
 
 Dynamixel::~Dynamixel()
 {
+  fprintf(stderr, "Dynamixel destructor start\n");
   if (group_sync_read_) {
     delete group_sync_read_;
     group_sync_read_ = nullptr;
@@ -73,11 +74,8 @@ Dynamixel::~Dynamixel()
     delete port_handler_;
     port_handler_ = nullptr;
   }
-  if (packet_handler_) {
-    delete packet_handler_;
-    packet_handler_ = nullptr;
-  }
-  fprintf(stderr, "closed port\n");
+  packet_handler_ = nullptr;
+  fprintf(stderr, "Dynamixel destructor end\n");
 }
 
 DxlError Dynamixel::InitDxlComm(
@@ -1301,11 +1299,15 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms)
           "FastBulkRead failed 10 times, switching to normal BulkRead permanently.\n");
         use_fast_read_protocol_ = false;
         // Set up normal bulk read handler
-        std::vector<uint8_t> id_arr;
+        std::vector<uint8_t> indirect_id_arr;
+
         for (auto it_read_data : read_data_list_) {
-          id_arr.push_back(it_read_data.id);
+          if (CheckIndirectReadAvailable(it_read_data.id) == DxlError::OK) {
+            indirect_id_arr.push_back(it_read_data.id);
+          }
         }
-        SetBulkReadHandler(id_arr);
+
+        SetBulkReadHandler(indirect_id_arr);
       }
       // Return error for this attempt
       return DxlError::BULK_READ_FAIL;

src/dynamixel_hardware_interface.cpp

@@ -475,7 +475,12 @@ hardware_interface::CallbackReturn DynamixelHardware::start()
 
 hardware_interface::CallbackReturn DynamixelHardware::stop()
 {
-  dxl_comm_->DynamixelDisable(dxl_id_);
+  if (dxl_comm_) {
+    dxl_comm_->DynamixelDisable(dxl_id_);
+  } else {
+    RCLCPP_ERROR_STREAM(logger_, "Dynamixel Hardware Stop Fail : dxl_comm_ is nullptr");
+    return hardware_interface::CallbackReturn::ERROR;
+  }
 
   RCLCPP_INFO_STREAM(logger_, "Dynamixel Hardware Stop!");