From 11d89f24b60edbd10b4ef82d9db698bb836e5c58 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 30 Oct 2024 19:05:12 +0100 Subject: [PATCH 1/3] add the realtime_tools lock_memory method to prevent page faults --- controller_manager/src/ros2_control_node.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 13af864c2f..ad4850e2a8 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -57,6 +57,12 @@ int main(int argc, char ** argv) auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); + std::string message; + if (!realtime_tools::lock_memory(message)) + { + RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str()); + } + RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); RCLCPP_INFO( From 3d9a4f4702138d7d07fbb3f3866e0508ad2b752c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 31 Oct 2024 09:46:13 +0100 Subject: [PATCH 2/3] Add lock_memory parameter to ros2_control_node --- controller_manager/src/ros2_control_node.cpp | 3 ++- doc/release_notes.rst | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index ad4850e2a8..42987d2b0f 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -57,8 +57,9 @@ int main(int argc, char ** argv) auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); + const bool lock_memory = cm->get_parameter_or("lock_memory", false); std::string message; - if (!realtime_tools::lock_memory(message)) + if (lock_memory && !realtime_tools::lock_memory(message)) { RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str()); } diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a7c1242ffc..9b70016184 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -77,6 +77,7 @@ controller_manager * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). +* The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). hardware_interface ****************** From 48ec00aa4db26b05082c7b96489582b251c3a3d7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 31 Oct 2024 13:41:44 +0100 Subject: [PATCH 3/3] Use lock_memory by default Co-authored-by: Bence Magyar --- controller_manager/src/ros2_control_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 42987d2b0f..ded2f5ea82 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -57,7 +57,7 @@ int main(int argc, char ** argv) auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); - const bool lock_memory = cm->get_parameter_or("lock_memory", false); + const bool lock_memory = cm->get_parameter_or("lock_memory", true); std::string message; if (lock_memory && !realtime_tools::lock_memory(message)) {