adding soft realtime prioritization for collision monitor and velocit…

…y smoother (ros-navigation#3979)

* adding soft realtime prioritization for collision monitor and velocity smoother

* refactor simple action server to use new utils API

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -77,6 +77,20 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
   collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
     "~/collision_points_marker", 1);
 
+  auto node = shared_from_this();
+  nav2_util::declare_parameter_if_not_declared(
+    node, "use_realtime_priority", rclcpp::ParameterValue(false));
+  bool use_realtime_priority = false;
+  node->get_parameter("use_realtime_priority", use_realtime_priority);
+  if (use_realtime_priority) {
+    try {
+      nav2_util::setSoftRealTimePriority();
+    } catch (const std::runtime_error & e) {
+      RCLCPP_ERROR(get_logger(), "%s", e.what());
+      return nav2_util::CallbackReturn::FAILURE;
+    }
+  }
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 

nav2_util/include/nav2_util/node_utils.hpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2023 Open Navigation LLC
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -171,6 +172,13 @@ void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child)
   }
 }
 
+/**
+ * @brief Sets the caller thread to have a soft-realtime prioritization by
+ * increasing the priority level of the host thread.
+ * May throw exception if unable to set prioritization successfully
+ */
+void setSoftRealTimePriority();
+
 }  // namespace nav2_util
 
 #endif  // NAV2_UTIL__NODE_UTILS_HPP_

nav2_util/include/nav2_util/simple_action_server.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "nav2_util/node_thread.hpp"
+#include "nav2_util/node_utils.hpp"
 
 namespace nav2_util
 {
@@ -184,17 +185,8 @@ class SimpleActionServer
   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!");
-      }
+      nav2_util::setSoftRealTimePriority();
+      debug_msg("Soft realtime prioritization successfully set!");
     }
   }
 

nav2_util/src/node_utils.cpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2023 Open Navigation LLC
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -89,4 +90,17 @@ rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix)
   return rclcpp::Node::make_shared("_", options);
 }
 
+void setSoftRealTimePriority()
+{
+  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));
+  }
+}
+
 }  // namespace nav2_util

nav2_velocity_smoother/src/velocity_smoother.cpp

@@ -133,6 +133,18 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &)
     "cmd_vel", rclcpp::QoS(1),
     std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1));
 
+  declare_parameter_if_not_declared(node, "use_realtime_priority", rclcpp::ParameterValue(false));
+  bool use_realtime_priority = false;
+  node->get_parameter("use_realtime_priority", use_realtime_priority);
+  if (use_realtime_priority) {
+    try {
+      nav2_util::setSoftRealTimePriority();
+    } catch (const std::runtime_error & e) {
+      RCLCPP_ERROR(get_logger(), "%s", e.what());
+      return nav2_util::CallbackReturn::FAILURE;
+    }
+  }
+
   return nav2_util::CallbackReturn::SUCCESS;
 }