fix(admittance): remove real-time memory allocations in update loop #2151
Conversation
github-actions
bot
requested review from
MarqRazz,
arne48,
destogl,
homalozoa and
xguay
| private: | ||
| geometry_msgs::msg::Wrench offsetted_ft_values_; | ||
| rclcpp::Logger logger_ = rclcpp::get_logger("AdmittanceController"); | ||
| }; |
There was a problem hiding this comment.
I don't think this is needed. Wrench has no dynamically allocated variables
There was a problem hiding this comment.
Currently geometry_msgs::msg::Wrench objects are being created on the stack on every cycle in the update while they don't allocate heap memory, promoting them to members, avoid unnecessary stack construction/destruction overhead in the RT loop.
| auto & dst = state_message_.ft_sensor_frame.data; | ||
| const auto & src = admittance_state_.ft_sensor_frame; | ||
| if (src.size() <= dst.capacity()) | ||
| { | ||
| dst.assign(src.c_str(), src.size()); | ||
| } | ||
| else | ||
| { | ||
| dst.assign(src.c_str(), dst.capacity()); // truncate to reserved size | ||
| } |
| if (!success) | ||
| { | ||
| RCLCPP_WARN(logger, "Error while setting command for joint %zu.", joint_ind); | ||
| RCLCPP_WARN(logger_, "Error while setting command for joint %zu.", joint_ind); |
There was a problem hiding this comment.
| RCLCPP_WARN(logger_, "Error while setting command for joint %zu.", joint_ind); | |
| RCLCPP_WARN(get_node()->get_logger(), "Error while setting command for joint %zu.", joint_ind); |
There was a problem hiding this comment.
I thought caching the logger would be a better choice; as normally, calling get_node()->get_logger() will involve pointer derefrencing with potentially, internel mutexes/allocations in rclcpp
@saikishor Thanks for the review. I agree that #2150 is the cleaner solution for the string allocation issue since it handles it in the configuration phase. Regarding the other optimizations (cached logger and stack allocation): I see your points that the overhead is likely negligible. Since the critical real-time violation is being fixed in #2150, I will close this PR. |
2 participants
This PR resolves real-time safety violations in the admittance_controller.
Previously, the update_and_write_commands() loop was creating new geometry_msgs objects and calling get_node()->get_logger() every cycle. These operations involve dynamic memory allocation and potential mutex locking, which are not real-time safe.
Changes:
Testing & Verification
I have verified that these changes maintain strict backward compatibility and pass all existing tests.
Fixes #2130