diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index a8f8c079c3..75a98d19dd 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -61,10 +61,13 @@ int main(int argc, char ** argv) const bool has_realtime = realtime_tools::has_realtime_kernel(); const bool lock_memory = cm->get_parameter_or("lock_memory", has_realtime); - std::string message; - if (lock_memory && !realtime_tools::lock_memory(message)) + if (lock_memory) { - RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str()); + const auto lock_result = realtime_tools::lock_memory(); + if (!lock_result.first) + { + RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory: '%s'", lock_result.second.c_str()); + } } rclcpp::Parameter cpu_affinity_param;