From f94f69ecf713df96938e245ae8a4a1fd95e2030b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 21 Nov 2023 14:09:03 -0800 Subject: [PATCH] adding soft realtime prioritization for collision monitor and velocity smoother (#3979) * adding soft realtime prioritization for collision monitor and velocity smoother * refactor simple action server to use new utils API --- .../src/collision_monitor_node.cpp | 14 ++++++++++++++ nav2_util/include/nav2_util/node_utils.hpp | 8 ++++++++ .../include/nav2_util/simple_action_server.hpp | 14 +++----------- nav2_util/src/node_utils.cpp | 14 ++++++++++++++ nav2_velocity_smoother/src/velocity_smoother.cpp | 12 ++++++++++++ 5 files changed, 51 insertions(+), 11 deletions(-) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 481cafbb65..29e3f3031d 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/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( "~/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; } diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index d0bb65cf41..a6a511a89c 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/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_ diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index fac67989bf..c74dcf55f2 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/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: hard rtprio 99 and " - " 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!"); } } diff --git a/nav2_util/src/node_utils.cpp b/nav2_util/src/node_utils.cpp index e5415b81ad..993eaf53b6 100644 --- a/nav2_util/src/node_utils.cpp +++ b/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: hard rtprio 99 and " + " soft rtprio 99 in /etc/security/limits.conf to enable " + "realtime prioritization! Error: "); + throw std::runtime_error(errmsg + std::strerror(errno)); + } +} + } // namespace nav2_util diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 1c86659aae..d3dd5c2a99 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/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; }