From fbe8f56042a07513630f50b563f5690a30b1346a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 20 Nov 2023 12:41:01 -0800 Subject: [PATCH] Enabling soft realtime prioritization to the Controller Server (#3914) * Enabling soft realtime prioritization to the controller server * abstracting to another function * changing default priorities * linting --- nav2_bringup/params/nav2_params.yaml | 1 + .../nav2_controller/controller_server.hpp | 1 + nav2_controller/src/controller_server.cpp | 22 ++++++---- nav2_util/include/nav2_util/node_thread.hpp | 6 ++- .../nav2_util/simple_action_server.hpp | 41 +++++++++++++++++-- nav2_util/src/node_thread.cpp | 9 ++-- 6 files changed, 64 insertions(+), 16 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index e70fa9216c..356118f19d 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/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: diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 544155ebb3..992c8fab04 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/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_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 0386c06fc7..ea513d1eb2 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/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( @@ -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( - 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( + 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( diff --git a/nav2_util/include/nav2_util/node_thread.hpp b/nav2_util/include/nav2_util/node_thread.hpp index 78d84fb8db..e0c1692792 100644 --- a/nav2_util/include/nav2_util/node_thread.hpp +++ b/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 diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index f05a4602ac..fac67989bf 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/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 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: 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!"); + } + } + } + /** * @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 execution_future_; bool stop_execution_{false}; + bool use_realtime_prioritization_{false}; mutable std::recursive_mutex update_mutex_; bool server_active_{false}; diff --git a/nav2_util/src/node_thread.cpp b/nav2_util/src/node_thread.cpp index c19fbd9847..adcbe50b78 100644 --- a/nav2_util/src/node_thread.cpp +++ b/nav2_util/src/node_thread.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include - #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(); @@ -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([&]() {executor_->spin();}); + thread_ = std::make_unique( + [&]() { + executor_->spin(); + }); } NodeThread::~NodeThread()