adding soft realtime prioritization for collision monitor and velocity smoother

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;
 }