Enabling soft realtime prioritization to the Controller Server (ros-navigation#3914)

* Enabling soft realtime prioritization to the controller server

* abstracting to another function

* changing default priorities

* linting

Signed-off-by: enricosutera <[email protected]>

Author: SteveMacenski

nav2_bringup/params/nav2_params.yaml

@@ -122,6 +122,7 @@ controller_server:
     progress_checker_plugins: ["progress_checker"]
     goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
     controller_plugins: ["FollowPath"]
+    use_realtime_priority: false
 
     # Progress checker parameters
     progress_checker:

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -265,6 +265,7 @@ class ControllerServer : public nav2_util::LifecycleNode
   double min_theta_velocity_threshold_;
 
   double failure_tolerance_;
+  bool use_realtime_priority_;
 
   // Whether we've published the single controller warning yet
   geometry_msgs::msg::PoseStamped end_pose_;

nav2_controller/src/controller_server.cpp

@@ -62,6 +62,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
   declare_parameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit"));
 
   declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0));
+  declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false));
 
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
@@ -126,6 +127,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   std::string speed_limit_topic;
   get_parameter("speed_limit_topic", speed_limit_topic);
   get_parameter("failure_tolerance", failure_tolerance_);
+  get_parameter("use_realtime_priority", use_realtime_priority_);
 
   costmap_ros_->configure();
   // Launch a thread to run the costmap node
@@ -221,13 +223,19 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout);
 
   // Create the action server that we implement with our followPath method
-  action_server_ = std::make_unique<ActionServer>(
-    shared_from_this(),
-    "follow_path",
-    std::bind(&ControllerServer::computeControl, this),
-    nullptr,
-    std::chrono::milliseconds(500),
-    true, server_options);
+  // This may throw due to real-time prioritzation if user doesn't have real-time permissions
+  try {
+    action_server_ = std::make_unique<ActionServer>(
+      shared_from_this(),
+      "follow_path",
+      std::bind(&ControllerServer::computeControl, this),
+      nullptr,
+      std::chrono::milliseconds(500),
+      true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/);
+  } catch (const std::runtime_error & e) {
+    RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what());
+    return nav2_util::CallbackReturn::FAILURE;
+  }
 
   // Set subscribtion to the speed limiting topic
   speed_limit_sub_ = create_subscription<nav2_msgs::msg::SpeedLimit>(

nav2_util/include/nav2_util/node_thread.hpp

@@ -32,13 +32,15 @@ class NodeThread
    * @brief A background thread to process node callbacks constructor
    * @param node_base Interface to Node to spin in thread
    */
-  explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base);
+  explicit NodeThread(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base);
 
   /**
    * @brief A background thread to process executor's callbacks constructor
    * @param executor Interface to executor to spin in thread
    */
-  explicit NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor);
+  explicit NodeThread(
+    rclcpp::executors::SingleThreadedExecutor::SharedPtr executor);
 
   /**
    * @brief A background thread to process node callbacks constructor

nav2_util/include/nav2_util/simple_action_server.hpp

@@ -57,6 +57,8 @@ class SimpleActionServer
    * @param server_timeout Timeout to to react to stop or preemption requests
    * @param spin_thread Whether to spin with a dedicated thread internally
    * @param options Options to pass to the underlying rcl_action_server_t
+   * @param realtime Whether the action server's worker thread should have elevated
+   * prioritization (soft realtime)
    */
   template<typename NodeT>
   explicit SimpleActionServer(
@@ -66,13 +68,15 @@ class SimpleActionServer
     CompletionCallback completion_callback = nullptr,
     std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500),
     bool spin_thread = false,
-    const rcl_action_server_options_t & options = rcl_action_server_get_default_options())
+    const rcl_action_server_options_t & options = rcl_action_server_get_default_options(),
+    const bool realtime = false)
   : SimpleActionServer(
       node->get_node_base_interface(),
       node->get_node_clock_interface(),
       node->get_node_logging_interface(),
       node->get_node_waitables_interface(),
-      action_name, execute_callback, completion_callback, server_timeout, spin_thread, options)
+      action_name, execute_callback, completion_callback,
+      server_timeout, spin_thread, options, realtime)
   {}
 
   /**
@@ -83,6 +87,8 @@ class SimpleActionServer
    * @param server_timeout Timeout to to react to stop or preemption requests
    * @param spin_thread Whether to spin with a dedicated thread internally
    * @param options Options to pass to the underlying rcl_action_server_t
+   * @param realtime Whether the action server's worker thread should have elevated
+   * prioritization (soft realtime)
    */
   explicit SimpleActionServer(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
@@ -94,7 +100,8 @@ class SimpleActionServer
     CompletionCallback completion_callback = nullptr,
     std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500),
     bool spin_thread = false,
-    const rcl_action_server_options_t & options = rcl_action_server_get_default_options())
+    const rcl_action_server_options_t & options = rcl_action_server_get_default_options(),
+    const bool realtime = false)
   : node_base_interface_(node_base_interface),
     node_clock_interface_(node_clock_interface),
     node_logging_interface_(node_logging_interface),
@@ -106,6 +113,7 @@ class SimpleActionServer
     spin_thread_(spin_thread)
   {
     using namespace std::placeholders;  // NOLINT
+    use_realtime_prioritization_ = realtime;
     if (spin_thread_) {
       callback_group_ = node_base_interface->create_callback_group(
         rclcpp::CallbackGroupType::MutuallyExclusive, false);
@@ -170,6 +178,26 @@ class SimpleActionServer
     return rclcpp_action::CancelResponse::ACCEPT;
   }
 
+  /**
+   * @brief Sets thread priority level
+   */
+  void setSoftRealTimePriority()
+  {
+    if (use_realtime_prioritization_) {
+      sched_param sch;
+      sch.sched_priority = 49;
+      if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) {
+        std::string errmsg(
+          "Cannot set as real-time thread. Users must set: <username> hard rtprio 99 and "
+          "<username> soft rtprio 99 in /etc/security/limits.conf to enable "
+          "realtime prioritization! Error: ");
+        throw std::runtime_error(errmsg + std::strerror(errno));
+      } else {
+        debug_msg("Soft realtime prioritization successfully set!");
+      }
+    }
+  }
+
   /**
    * @brief Handles accepted goals and adds to preempted queue to switch to
    * @param Goal A server goal handle to cancel
@@ -202,7 +230,11 @@ class SimpleActionServer
 
       // Return quickly to avoid blocking the executor, so spin up a new thread
       debug_msg("Executing goal asynchronously.");
-      execution_future_ = std::async(std::launch::async, [this]() {work();});
+      execution_future_ = std::async(
+        std::launch::async, [this]() {
+          setSoftRealTimePriority();
+          work();
+        });
     }
   }
 
@@ -509,6 +541,7 @@ class SimpleActionServer
   CompletionCallback completion_callback_;
   std::future<void> execution_future_;
   bool stop_execution_{false};
+  bool use_realtime_prioritization_{false};
 
   mutable std::recursive_mutex update_mutex_;
   bool server_active_{false};

nav2_util/src/node_thread.cpp

@@ -13,13 +13,13 @@
 // limitations under the License.
 
 #include <memory>
-
 #include "nav2_util/node_thread.hpp"
 
 namespace nav2_util
 {
 
-NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base)
+NodeThread::NodeThread(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base)
 : node_(node_base)
 {
   executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
@@ -35,7 +35,10 @@ NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nod
 NodeThread::NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor)
 : executor_(executor)
 {
-  thread_ = std::make_unique<std::thread>([&]() {executor_->spin();});
+  thread_ = std::make_unique<std::thread>(
+    [&]() {
+      executor_->spin();
+    });
 }
 
 NodeThread::~NodeThread()