feat: Add controller and retry when the parameter initilization fails

Author: Woojin-Crive

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -235,6 +235,9 @@ class DynamixelHardware : public
   std::vector<uint8_t> sensor_id_;
   std::map<uint8_t /*id*/, std::string /*interface_name*/> sensor_item_;
 
+  std::vector<uint8_t> controller_id_;
+  std::map<uint8_t /*id*/, std::string /*interface_name*/> controller_item_;
+
   ///// handler variable
   std::vector<HandlerVarType> hdl_trans_states_;
   std::vector<HandlerVarType> hdl_trans_commands_;
@@ -254,11 +257,33 @@ class DynamixelHardware : public
   double ** joint_to_transmission_matrix_;
 
   /**
-   * @brief Initializes the Dynamixel items.
+   * @brief Helper function to initialize items for a specific type.
+   * @param type_filter The type of items to initialize ("controller" or "dxl" or "sensor").
+   * @return True if initialization was successful, false otherwise.
+   */
+  bool initItems(const std::string& type_filter);
+
+  /**
+   * @brief Helper function to retry writing an item to a Dynamixel device.
+   * @param id The ID of the Dynamixel device.
+   * @param item_name The name of the item to write.
+   * @param value The value to write.
+   * @return True if write was successful, false if timeout occurred.
+   */
+  bool retryWriteItem(uint8_t id, const std::string& item_name, uint32_t value);
+
+  /**
+   * @brief Initializes Dynamixel items.
    * @return True if initialization was successful, false otherwise.
    */
   bool InitDxlItems();
 
+  /**
+   * @brief Initializes the controller items.
+   * @return True if initialization was successful, false otherwise.
+   */
+  bool InitControllerItems();
+
   /**
    * @brief Initializes the read items for Dynamixel.
    * @return True if initialization was successful, false otherwise.

src/dynamixel_hardware_interface.cpp

@@ -105,6 +105,8 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
       dxl_id_.push_back(static_cast<uint8_t>(stoi(gpio.parameters.at("ID"))));
     } else if (gpio.parameters.at("type") == "sensor") {
       sensor_id_.push_back(static_cast<uint8_t>(stoi(gpio.parameters.at("ID"))));
+    } else if (gpio.parameters.at("type") == "controller") {
+      controller_id_.push_back(static_cast<uint8_t>(stoi(gpio.parameters.at("ID"))));
     } else {
       RCLCPP_ERROR_STREAM(logger_, "Invalid DXL / Sensor type");
       exit(-1);
@@ -115,6 +117,35 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
   int trying_cnt = 60;
   int cnt = 0;
 
+  if(controller_id_.size() > 0 ) {
+    while (trying_connect) {
+      std::vector<uint8_t> id_arr;
+      for (auto controller : controller_id_) {
+        id_arr.push_back(controller);
+      }
+      if (dxl_comm_->InitDxlComm(id_arr, port_name_, baud_rate_) == DxlError::OK) {
+        RCLCPP_INFO_STREAM(logger_, "Trying to connect to the communication port...");
+        trying_connect = false;
+      } else {
+        sleep(1);
+        cnt++;
+        if (cnt > trying_cnt) {
+          RCLCPP_ERROR_STREAM(logger_, "Cannot connect communication port! :(");
+          cnt = 0;
+        }
+      }
+    }
+  }
+
+  if (!InitControllerItems()) {
+    RCLCPP_ERROR_STREAM(logger_, "Error: InitControllerItems");
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  trying_connect = true;
+  trying_cnt = 60;
+  cnt = 0;
+
   while (trying_connect) {
     std::vector<uint8_t> id_arr;
     for (auto dxl : dxl_id_) {
@@ -566,6 +597,7 @@ bool DynamixelHardware::CommReset()
       usleep(200 * 1000);
     }
     if (!result) {continue;}
+    if (!InitControllerItems()) {continue;}
     if (!InitDxlItems()) {continue;}
     if (!InitDxlReadItems()) {continue;}
     if (!InitDxlWriteItems()) {continue;}
@@ -582,48 +614,71 @@ bool DynamixelHardware::CommReset()
   return false;
 }
 
-bool DynamixelHardware::InitDxlItems()
-{
-  RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Dxl Items");
+bool DynamixelHardware::retryWriteItem(uint8_t id, const std::string& item_name, uint32_t value) {
+  rclcpp::Time start_time = this->now();
+  rclcpp::Duration error_duration(0, 0);
+  
+  while (true) {
+    if (dxl_comm_->WriteItem(id, item_name, value) == DxlError::OK) {
+      RCLCPP_INFO_STREAM(
+        logger_,
+        "[ID:" << std::to_string(id) << "] item_name:" << item_name.c_str() << "\tdata:" << value);
+      return true;
+    }
+    
+    error_duration = this->now() - start_time;
+    if (error_duration.seconds() * 1000 >= err_timeout_ms_) {
+      RCLCPP_ERROR_STREAM(
+        logger_,
+        "[ID:" << std::to_string(id) << "] Write Item error (Timeout: " << 
+        error_duration.seconds() * 1000 << "ms/" << err_timeout_ms_ << "ms)");
+      return false;
+    }
+    RCLCPP_WARN_STREAM(
+      logger_,
+      "[ID:" << std::to_string(id) << "] Write Item retry...");
+  }
+}
+
+bool DynamixelHardware::initItems(const std::string& type_filter) {
+  RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Items for type: " << type_filter);
   for (const hardware_interface::ComponentInfo & gpio : info_.gpios) {
+    if (gpio.parameters.at("type") != type_filter) {
+      continue;
+    }
     uint8_t id = static_cast<uint8_t>(stoi(gpio.parameters.at("ID")));
+    
     // First write items containing "Limit"
     for (auto it : gpio.parameters) {
       if (it.first != "ID" && it.first != "type" && it.first.find("Limit") != std::string::npos) {
-        if (dxl_comm_->WriteItem(
-            id, it.first,
-            static_cast<uint32_t>(stoi(it.second))) != DxlError::OK)
-        {
-          RCLCPP_ERROR_STREAM(logger_, "[ID:" << std::to_string(id) << "] Write Item error");
+        if (!retryWriteItem(id, it.first, static_cast<uint32_t>(stoi(it.second)))) {
           return false;
         }
-        RCLCPP_INFO_STREAM(
-          logger_,
-          "[ID:" << std::to_string(id) << "] item_name:" << it.first.c_str() << "\tdata:" <<
-            stoi(it.second));
       }
     }
 
     // Then write the remaining items
     for (auto it : gpio.parameters) {
       if (it.first != "ID" && it.first != "type" && it.first.find("Limit") == std::string::npos) {
-        if (dxl_comm_->WriteItem(
-            id, it.first,
-            static_cast<uint32_t>(stoi(it.second))) != DxlError::OK)
-        {
-          RCLCPP_ERROR_STREAM(logger_, "[ID:" << std::to_string(id) << "] Write Item error");
+        if (!retryWriteItem(id, it.first, static_cast<uint32_t>(stoi(it.second)))) {
           return false;
         }
-        RCLCPP_INFO_STREAM(
-          logger_,
-          "[ID:" << std::to_string(id) << "] item_name:" << it.first.c_str() << "\tdata:" <<
-            stoi(it.second));
       }
     }
   }
   return true;
 }
 
+bool DynamixelHardware::InitControllerItems()
+{
+  return initItems("controller");
+}
+
+bool DynamixelHardware::InitDxlItems()
+{
+  return initItems("dxl") && initItems("sensor");
+}
+
 bool DynamixelHardware::InitDxlReadItems()
 {
   RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Dxl Read Items");