refactor: update parameter writing logic in Dynamixel hardware interface

- Changed the initialization process to write Operating Mode parameters in the first pass.
- Adjusted the logic to skip already written parameters, including Operating Mode and Limit parameters, in subsequent passes.
- Improved clarity and efficiency of parameter handling during initialization.

Author: Woojin-Crive

src/dynamixel_hardware_interface.cpp

@@ -698,38 +698,37 @@ bool DynamixelHardware::initItems(const std::string & type_filter)
     }
     dxl_torque_enable_[id] = torque_enabled;
 
-    // Write parameters in two passes:
-    // 1. First pass: Write all Limit parameters
+    // 1. First pass: Write Operating Mode parameters
     for (const auto& param : gpio.parameters) {
       const std::string& param_name = param.first;
+      if (param_name == "Operating Mode") {
+        if (!retryWriteItem(id, param_name, static_cast<uint32_t>(stoi(param.second)))) {
+          return false;
+        }
+      }
+    }
 
+    // 2. Second pass: Write all Limit parameters
+    for (const auto& param : gpio.parameters) {
+      const std::string& param_name = param.first;
       // Skip special parameters
-      if (param_name == "ID" || param_name == "type" || param_name == "Torque Enable") {
+      if (param_name == "ID" || param_name == "type" || param_name == "Torque Enable" || param_name == "Operating Mode") {
         continue;
       }
-
-      // Write Limit parameters first
       if (param_name.find("Limit") != std::string::npos) {
         if (!retryWriteItem(id, param_name, static_cast<uint32_t>(stoi(param.second)))) {
           return false;
         }
       }
     }
 
-    // 2. Second pass: Write all non-Limit parameters
+    // 3. Third pass: Write all other parameters (excluding already written ones)
     for (const auto& param : gpio.parameters) {
       const std::string& param_name = param.first;
-
-      // Skip special parameters
-      if (param_name == "ID" || param_name == "type" || param_name == "Torque Enable") {
+      // Skip special and already written parameters
+      if (param_name == "ID" || param_name == "type" || param_name == "Torque Enable" || param_name == "Operating Mode" || param_name.find("Limit") != std::string::npos) {
         continue;
       }
-
-      // Skip Limit parameters (already written)
-      if (param_name.find("Limit") != std::string::npos) {
-        continue;
-      }
-
       if (!retryWriteItem(id, param_name, static_cast<uint32_t>(stoi(param.second)))) {
         return false;
       }