Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enhance DXL item initialization by prioritizing 'Limit' parameters. #5

Merged
merged 1 commit into from
Jan 17, 2025
Merged

Enhance DXL item initialization by prioritizing 'Limit' parameters. #5

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 20 additions & 1 deletion src/dynamixel_hardware_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<uint8_t>(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<uint32_t>(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) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The current implementation separates the processing of "Limit" items and other items into two loops. While this approach works, it can be optimized by combining the loops into one. By doing so, we reduce the number of iterations over gpio.parameters, making the code simpler and slightly more efficient. Additionally, separating the conditions for "Limit" items and others within the loop maintains the existing logic while improving readability and maintainability.

Here’s a suggested implementation:

bool DynamixelHardware::InitDxlItems()
{
  RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Dxl Items");

  for (const hardware_interface::ComponentInfo & gpio : info_.gpios) {
    uint8_t id = static_cast<uint8_t>(stoi(gpio.parameters.at("ID")));

    // Single loop to handle all parameters
    for (auto it : gpio.parameters) {
      if (it.first == "ID" || it.first == "type") {
        continue; // Skip ID and type
      }

      // Write "Limit" items first
      if (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 (Limit)");
          return false;
        }
        RCLCPP_INFO_STREAM(
          logger_,
          "[ID:" << std::to_string(id) << "] (Limit) item_name:" << it.first.c_str() << "\tdata:" <<
            stoi(it.second));
      } else {
        // Write other items
        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");
          return false;
        }
        RCLCPP_INFO_STREAM(
          logger_,
          "[ID:" << std::to_string(id) << "] item_name:" << it.first.c_str() << "\tdata:" <<
            stoi(it.second));
      }
    }
  }
  return true;
}

Key Benefits:

Efficiency: The parameters are iterated only once.
Maintainability: Combining the loops reduces redundancy, and separating conditions ("Limit" vs. others) within the loop improves clarity.
Logic Preservation: The "Limit" items are still processed before others, maintaining the intended behavior.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you for your suggestion to combine the limit parameters and other parameters into a single loop. However, the original code explicitly ensures that all limit parameters are written before any other parameters. This two-phase approach is critical because it guarantees that subsequent parameter writes will fall within valid, pre-established limits.

When we merge everything into a single loop, we lose that strict ordering. Although the if (it.first.find("Limit") ... ) check is present, we can’t guarantee all limit parameters will be handled first in a single pass—especially in containers where iteration order can be non-deterministic. That means some non-limit parameters might get set before the corresponding limit parameters, which could lead to invalid configurations.

I do appreciate the goal of reducing loops and simplifying the code, but preserving the limit-first logic is paramount for reliable initialization. If you have any other ideas on how to keep this guarantee while simplifying the loops, I am open to further discussion!

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you for your detailed explanation and for pointing out the importance of preserving the strict ordering of limit parameters before other parameters. You are absolutely correct that combining everything into a single loop could break the intended logic by potentially allowing non-limit parameters to be set before their corresponding limits, especially in cases where the iteration order of the container is non-deterministic.

Additionally, I considered an alternative approach where the first loop processes and removes limit parameters from the container, reducing the number of iterations required in the second loop. However, as this would involve modifying the container during iteration, it might not yield significant efficiency improvements and could introduce unnecessary complexity.

Given these points, I agree that your proposed two-phase approach is the most reliable way to ensure the initialization process remains robust and predictable. Thank you again for the clarification, and I’m happy to proceed with your method. If any further ideas come to mind, I’ll be sure to share them! 😊

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)
Expand Down