Support rising tag #97
Conversation
Signed-off-by: Tatsuro Sakaguchi <[email protected]>
Summary of ChangesHello @Tacha-S, I'm Gemini Code Assist1! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed! This pull request addresses issue #96 by integrating URDF calibration data into the Dynamixel hardware interface. It specifically enables the system to read the 'rising' attribute from joint calibration tags within the URDF and translate it into a 'Homing Offset' parameter for the respective joints. This ensures that Dynamixel motors are initialized with their correct calibrated positions, improving the accuracy and reliability of robot control. Highlights
Using Gemini Code AssistThe full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips. Invoking Gemini You can request assistance from Gemini at any point by creating a comment using either
Customization To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a Limitations & Feedback Gemini Code Assist may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments. If you're interested in giving your feedback about your experience with Gemini Code Assist for Github and other Google products, sign up here. You can also get AI-powered code generation, chat, as well as code reviews directly in the IDE at no cost with the Gemini Code Assist IDE Extension. Footnotes
|
![gemini-code-assist[bot]](https://avatars.githubusercontent.com/in/956858?s=60&v=4){kind=small}
There was a problem hiding this comment.
Code Review
This pull request adds a useful feature to parse the rising calibration value from a URDF and apply it as a Homing Offset. The implementation is a good start, but I've found a critical bug and several areas for improvement.
My review includes a critical fix for an incorrect iterator comparison that could cause a crash. I've also suggested a more robust way to find the correct motor to apply the offset to, avoiding a fragile assumption about component ordering. Additionally, I've provided feedback on improving performance, readability by extracting the new logic into a helper function, and maintainability by using named constants.
Please note that while adding the #include <tinyxml2.h>, the corresponding dependency was not added to package.xml and CMakeLists.txt. This will likely cause build failures in a clean environment. You should add a dependency on tinyxml2_vendor.
src/dynamixel_hardware_interface.cpp
Outdated
| auto rising = std::atof(calibration_element->Attribute("rising")); | ||
| std::string name = joint_element->Attribute("name"); | ||
| auto itr = std::find_if( | ||
| info_.joints.begin(), info_.joints.end(), | ||
| [&name](const hardware_interface::ComponentInfo & joint) { | ||
| return joint.name == name; | ||
| }); | ||
| if (itr != info_.gpios.end()) { | ||
| auto params = info_.gpios[std::distance(info_.joints.begin(), itr)].parameters; | ||
| if (std::find_if(params.begin(), params.end(), | ||
| [](const std::pair<std::string, std::string> & p) { | ||
| return p.first == "Homing Offset"; | ||
| }) == params.end()) | ||
| { | ||
| info_.gpios[std::distance(info_.joints.begin(), itr)].parameters.emplace( | ||
| "Homing Offset", | ||
| std::to_string(std::round(rising * (180.0 / M_PI) / (360.0 / 4095)))); | ||
| } | ||
| } |
There was a problem hiding this comment.
This block of code has several issues that should be addressed:
- Critical Bug: The condition
itr != info_.gpios.end()on line 240 is incorrect. The iteratoritris forinfo_.joints, so it must be compared withinfo_.joints.end(). This bug can lead to out-of-bounds memory access and undefined behavior. - Fragile Logic: The code uses
std::distanceto get an index frominfo_.jointsand then uses this index to accessinfo_.gpios. This assumes a direct 1-to-1 ordered mapping between joints and gpios, which is not guaranteed and makes the code brittle. A more robust approach is to find the GPIO component by its name. - Inefficiency:
- On line 241,
auto params = ...creates an unnecessary copy of the parameters map. - On line 242,
std::find_ifis used to search for a key in astd::unordered_map. Using thefindmember function is much more efficient.
- On line 241,
- Readability & Style:
- Magic numbers like
180.0,360.0, and4095should be replaced with named constants to improve clarity. The value4095is specific to 12-bit resolution Dynamixels and might need to be configurable for other models. tinyxml2provides helper functions likeDoubleValue()which are safer and more expressive than usingstd::atofon an attribute string.- Variables that are not modified should be declared
const.
- Magic numbers like
Here is a suggested replacement that addresses all these points:
const double rising = rising_attr->DoubleValue();
const std::string name = name_attr->Value();
// Find the corresponding GPIO component by name. This is more robust than assuming index correspondence.
auto gpio_it = std::find_if(
info_.gpios.begin(), info_.gpios.end(),
[&name](const hardware_interface::ComponentInfo & gpio) {
return gpio.name == name;
});
if (gpio_it != info_.gpios.end()) {
// Use find() for efficient key lookup in std::unordered_map
if (gpio_it->parameters.find("Homing Offset") == gpio_it->parameters.end()) {
// Use named constants for magic numbers
constexpr double RAD_TO_DEG = 180.0 / M_PI;
constexpr double DXL_RESOLUTION_12BIT = 4095.0;
constexpr double DXL_MAX_DEG = 360.0;
const double homing_offset_steps =
std::round(rising * RAD_TO_DEG / (DXL_MAX_DEG / DXL_RESOLUTION_12BIT));
gpio_it->parameters.emplace("Homing Offset", std::to_string(homing_offset_steps));
}
}
src/dynamixel_hardware_interface.cpp
Outdated
| auto urdf = info_.original_xml; | ||
| tinyxml2::XMLDocument doc; | ||
| if (doc.Parse(urdf.c_str()) != tinyxml2::XML_SUCCESS) { | ||
| RCLCPP_ERROR(logger_, "Failed to parse URDF XML"); | ||
| return hardware_interface::CallbackReturn::ERROR; | ||
| } | ||
| const auto * joint_element = doc.RootElement()->FirstChildElement("joint"); | ||
| while (joint_element != nullptr) { | ||
| const auto * name_attr = joint_element->FindAttribute("name"); | ||
| const auto * calibration_element = joint_element->FirstChildElement("calibration"); | ||
| if (calibration_element != nullptr) { | ||
| const auto * rising_attr = calibration_element->FindAttribute("rising"); | ||
| if ((rising_attr != nullptr) && (name_attr != nullptr)) { | ||
| auto rising = std::atof(calibration_element->Attribute("rising")); | ||
| std::string name = joint_element->Attribute("name"); | ||
| auto itr = std::find_if( | ||
| info_.joints.begin(), info_.joints.end(), | ||
| [&name](const hardware_interface::ComponentInfo & joint) { | ||
| return joint.name == name; | ||
| }); | ||
| if (itr != info_.gpios.end()) { | ||
| auto params = info_.gpios[std::distance(info_.joints.begin(), itr)].parameters; | ||
| if (std::find_if(params.begin(), params.end(), | ||
| [](const std::pair<std::string, std::string> & p) { | ||
| return p.first == "Homing Offset"; | ||
| }) == params.end()) | ||
| { | ||
| info_.gpios[std::distance(info_.joints.begin(), itr)].parameters.emplace( | ||
| "Homing Offset", | ||
| std::to_string(std::round(rising * (180.0 / M_PI) / (360.0 / 4095)))); | ||
| } | ||
| } | ||
| } | ||
| } | ||
| joint_element = joint_element->NextSiblingElement("joint"); | ||
| } |
There was a problem hiding this comment.
This new block of logic for parsing the URDF is quite large and is nested within the on_init function. To improve the readability and modularity of on_init, please consider extracting this entire block into a new private helper function, for example, void DynamixelHardware::updateHomingOffsetsFromURDF(). This will make on_init shorter and easier to understand at a glance.
Signed-off-by: Tatsuro Sakaguchi <[email protected]>
Signed-off-by: Tatsuro Sakaguchi <[email protected]>
Signed-off-by: Tatsuro Sakaguchi <[email protected]>
Signed-off-by: Tatsuro Sakaguchi <[email protected]>
Signed-off-by: Tatsuro Sakaguchi <[email protected]>
|
@Woojin-Crive Could you review it? |
This comment was marked as off-topic.
This comment was marked as off-topic.
Sorry, something went wrong.
|
Hello, i will check this! |
|
Homing Offset has some complexity to implement such logic. |
Signed-off-by: Tatsuro Sakaguchi <[email protected]>
|
@Woojin-Crive As an alternative approach, I would like to propose applying a calibration offset only at the ROS interface level (i.e. on the joint angle), rather than using the Homing Offset in the servo itself. I have created a sample implementation in commit 48d7c4a to demonstrate this idea. |
3 participants
calibration risingtag in URDF joints #96I updated the implementation so that when the URDF is parsed and a joint’s calibration rising value is present, it is applied to the Homing Offset parameter.