From b6ab3354151bf646b9ed1ddcfea4d8e4c918381d Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Wed, 30 Oct 2024 21:23:54 +0100
Subject: [PATCH] add thread_priority option to the ros2_control_node (#1820)

---
 controller_manager/src/ros2_control_node.cpp | 8 ++++++--
 doc/release_notes.rst                        | 5 +++++
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
index b2126aef28..f3ec22457c 100644
--- a/controller_manager/src/ros2_control_node.cpp
+++ b/controller_manager/src/ros2_control_node.cpp
@@ -45,13 +45,17 @@ int main(int argc, char ** argv)
   auto cm = std::make_shared<controller_manager::ControllerManager>(executor, manager_node_name);
 
   RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
+  const int thread_priority = cm->get_parameter_or<int>("thread_priority", kSchedPriority);
+  RCLCPP_INFO(
+    cm->get_logger(), "Spawning %s RT thread with scheduler priority: %d", cm->get_name(),
+    thread_priority);
 
   std::thread cm_thread(
-    [cm]()
+    [cm, thread_priority]()
     {
       if (realtime_tools::has_realtime_kernel())
       {
-        if (!realtime_tools::configure_sched_fifo(kSchedPriority))
+        if (!realtime_tools::configure_sched_fifo(thread_priority))
         {
           RCLCPP_WARN(
             cm->get_logger(),
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
index a945ff668e..fa3dc437e0 100644
--- a/doc/release_notes.rst
+++ b/doc/release_notes.rst
@@ -12,3 +12,8 @@ controller_interface
 ********************
 
 * The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 <https://github.com/ros-controls/ros2_control/pull/1775>`_)
+
+controller_manager
+******************
+
+* The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 <https://github.com/ros-controls/ros2_control/pull/1820>`_).