From 2804f39d4a2cec51a04e12bbf95da15f796da8e5 Mon Sep 17 00:00:00 2001 From: Woojin Wie Date: Wed, 15 Jan 2025 03:50:58 +0000 Subject: [PATCH] feat: Enhance DXL item initialization by prioritizing 'Limit' parameters. --- src/dynamixel_hardware_interface.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/dynamixel_hardware_interface.cpp b/src/dynamixel_hardware_interface.cpp index c00667d..ef9d21c 100644 --- a/src/dynamixel_hardware_interface.cpp +++ b/src/dynamixel_hardware_interface.cpp @@ -551,8 +551,27 @@ bool DynamixelHardware::InitDxlItems() RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Dxl Items"); for (const hardware_interface::ComponentInfo & gpio : info_.gpios) { uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); + + // First write items containing "Limit" for (auto it : gpio.parameters) { - if (it.first != "ID" && it.first != "type") { + if (it.first != "ID" && it.first != "type" && it.first.find("Limit") != std::string::npos) { + if (dxl_comm_->WriteItem( + id, it.first, + static_cast(stoi(it.second))) != DxlError::OK) + { + RCLCPP_ERROR_STREAM(logger_, "[ID:" << std::to_string(id) << "] Write Item error"); + 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(stoi(it.second))) != DxlError::OK)