From 8c8325065ebc15623beb6a1d80d7a99588b3ed21 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Sat, 8 Apr 2023 12:11:27 +0200 Subject: [PATCH 1/4] added minimal_scheduling_real_time_tutorial.cpp Signed-off-by: Jan Staschulat --- minimal_scheduling/CMakeLists.txt | 4 + minimal_scheduling/README.md | 19 +++ .../minimal_scheduling_real_time_tutorial.cpp | 153 ++++++++++++++++++ minimal_scheduling/package.xml | 1 + 4 files changed, 177 insertions(+) create mode 100644 minimal_scheduling/minimal_scheduling_real_time_tutorial.cpp diff --git a/minimal_scheduling/CMakeLists.txt b/minimal_scheduling/CMakeLists.txt index d8a54c1..14a00b4 100644 --- a/minimal_scheduling/CMakeLists.txt +++ b/minimal_scheduling/CMakeLists.txt @@ -26,11 +26,15 @@ ament_target_dependencies(minimal_scheduling_spin_thread rclcpp std_msgs) add_executable(minimal_scheduling_callback_group minimal_scheduling_callback_group.cpp) ament_target_dependencies(minimal_scheduling_callback_group rclcpp std_msgs) +add_executable(minimal_scheduling_real_time_tutorial minimal_scheduling_real_time_tutorial.cpp) +ament_target_dependencies(minimal_scheduling_real_time_tutorial rclcpp std_msgs) + install(TARGETS minimal_scheduling_main_thread minimal_scheduling_middleware_threads minimal_scheduling_spin_thread minimal_scheduling_callback_group + minimal_scheduling_real_time_tutorial DESTINATION lib/${PROJECT_NAME} ) diff --git a/minimal_scheduling/README.md b/minimal_scheduling/README.md index 19523a3..e2c7aca 100644 --- a/minimal_scheduling/README.md +++ b/minimal_scheduling/README.md @@ -202,6 +202,25 @@ $ ps -C minimal_schedul -L -o tid,comm,rtprio,cls,psr + +### minimal_scheduling_real_time_tutorial + +Example to demonstrate mixed system, with a real-time thread and non real-time thread. Two publisher callbacks are created. The first one is executed in the real-time thread, the other one is executed in a non real-time thread. Output: No preemptions of the real-time publisher and multiple preemptions of the non real-time publisher. + +```bash +$ ros2 run minimal_scheduling minimal_scheduling_real_time_tutorial +``` + +Output: real-time subscriber is not preempted by other kernel processes, but normal subscriber is. +```bash +[WARN] [1680947876.099416572] [minimal_sub1]: [sub] Involuntary context switches: '8' +[INFO] [1680947876.099471567] [minimal_sub2]: [sub] Involuntary context switches: '0' +[WARN] [1680947876.599197932] [minimal_sub1]: [sub] Involuntary context switches: '49' +[INFO] [1680947876.599202498] [minimal_sub2]: [sub] Involuntary context switches: '0' +[WARN] [1680947877.101378852] [minimal_sub1]: [sub] Involuntary context switches: '25' +[INFO] [1680947877.101372018] [minimal_sub2]: [sub] Involuntary context switches: '0' +``` + ## Resources - [https://www.man7.org/linux/man-pages/man7/sched.7.html](https://www.man7.org/linux/man-pages/man7/sched.7.html) diff --git a/minimal_scheduling/minimal_scheduling_real_time_tutorial.cpp b/minimal_scheduling/minimal_scheduling_real_time_tutorial.cpp new file mode 100644 index 0000000..1bc4152 --- /dev/null +++ b/minimal_scheduling/minimal_scheduling_real_time_tutorial.cpp @@ -0,0 +1,153 @@ +// Copyright 2023 +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +#include "rusage_utils.hpp" +#include "sched_utils.hpp" +#include "command_line_options.hpp" +#include "burn_cpu_cycles.hpp" + +/* For this example, we will be using two Executors running in different threads to separate + * callback processing, so it is possible to spin them in threads with different scheduling + * priorities. + */ + +using namespace std::chrono_literals; + +static ContextSwitchesCounter context_switches_counter(RUSAGE_THREAD); +static ContextSwitchesCounter context_switches_counter_rt(RUSAGE_THREAD); + +class MinimalPublisher : public rclcpp::Node +{ +public: + MinimalPublisher() + : Node("minimal_pub"), count1_(0), count2_(0) + { + publisher1_ = this->create_publisher("topic", 10); + auto timer_callback1 = + [this]() -> void { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(this->count1_++); + this->publisher1_->publish(message); + }; + timer1_ = this->create_wall_timer(500ms, timer_callback1); + + publisher2_ = this->create_publisher("topic_rt", 10); + auto timer_callback2 = + [this]() -> void { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(this->count2_++); + this->publisher2_->publish(message); + }; + timer2_ = this->create_wall_timer(500ms, timer_callback2); + } + +private: + rclcpp::TimerBase::SharedPtr timer1_, timer2_; + rclcpp::Publisher::SharedPtr publisher1_, publisher2_; + size_t count1_, count2_; +}; + +class MinimalSubscriber : public rclcpp::Node +{ +public: + MinimalSubscriber(const std::string & node_name, const std::string & topic_name) + : Node(node_name), context_switches_counter_(RUSAGE_THREAD) + { + subscription1_ = this->create_subscription( + topic_name, + 10, + [this](std_msgs::msg::String::UniquePtr) { + auto context_switches = context_switches_counter_.get(); + if (context_switches > 0L) { + RCLCPP_WARN( + this->get_logger(), "[sub] Involuntary context switches: '%lu'", + context_switches); + } else { + RCLCPP_INFO( + this->get_logger(), "[sub] Involuntary context switches: '%lu'", + context_switches); + } + burn_cpu_cycles(200ms); + }); + } + +private: + rclcpp::Subscription::SharedPtr subscription1_; + ContextSwitchesCounter context_switches_counter_; +}; + + +int main(int argc, char * argv[]) +{ + // Force flush of the stdout buffer. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + auto options_reader = SchedOptionsReader(); + if (!options_reader.read_options(argc, argv)) { + options_reader.print_usage(); + return 0; + } + auto options = options_reader.get_options(); + + rclcpp::init(argc, argv); + + auto node_pub = std::make_shared(); + auto node_sub = std::make_shared("minimal_sub1", "topic"); + auto node_sub_rt = std::make_shared("minimal_sub2", "topic_rt"); + + rclcpp::executors::StaticSingleThreadedExecutor default_executor; + rclcpp::executors::StaticSingleThreadedExecutor realtime_executor; + + // the publisher and non real-time subscriber are processed by default_executor + default_executor.add_node(node_pub); + default_executor.add_node(node_sub); + + // real-time subscriber is processed by realtime_executor. + realtime_executor.add_node(node_sub_rt); + + // middleware threads are not configured with real-time priority + // because scheduling configuration of main thread is not real-time. + + // Note: you can only add complete nodes to an Executor. That is: + // If a node has multiple entities (e.g. subscribers) then the corresponding + // callbacks are all entities processed with the same scheduling configuration + // of the thread, in which the Executor is spinning. + + // spin non real-time tasks in a separate thread + auto default_thread = std::thread( + [&]() { + default_executor.spin(); + }); + + // spin real-time tasks in a separate thread + auto realtime_thread = std::thread( + [&]() { + realtime_executor.spin(); + }); + + set_thread_scheduling(realtime_thread.native_handle(), options.policy, options.priority); + + default_thread.join(); + realtime_thread.join(); + + rclcpp::shutdown(); + return 0; +} diff --git a/minimal_scheduling/package.xml b/minimal_scheduling/package.xml index 5094038..3057753 100644 --- a/minimal_scheduling/package.xml +++ b/minimal_scheduling/package.xml @@ -7,6 +7,7 @@ Carlos San Vicente Apache License 2.0 Carlos San Vicente + Jan Staschulat ament_cmake From c09aa1f26b249b21366288f76986d7fd13e7a808 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Sat, 8 Apr 2023 12:19:58 +0200 Subject: [PATCH 2/4] updated README Signed-off-by: Jan Staschulat --- minimal_scheduling/README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/minimal_scheduling/README.md b/minimal_scheduling/README.md index e2c7aca..8e19a72 100644 --- a/minimal_scheduling/README.md +++ b/minimal_scheduling/README.md @@ -213,6 +213,15 @@ $ ros2 run minimal_scheduling minimal_scheduling_real_time_tutorial Output: real-time subscriber is not preempted by other kernel processes, but normal subscriber is. ```bash +ros2 run minimal_scheduling minimal_scheduling_real_time_tutorial +[WARN] [1680948979.971439054] [minimal_sub1]: [sub] Involuntary context switches: '25' +[WARN] [1680948979.971483357] [minimal_sub2]: [sub] Involuntary context switches: '20' +[WARN] [1680948980.473828433] [minimal_sub1]: [sub] Involuntary context switches: '23' +[WARN] [1680948980.473872245] [minimal_sub2]: [sub] Involuntary context switches: '21' +[WARN] [1680948980.972909968] [minimal_sub1]: [sub] Involuntary context switches: '26' +[WARN] [1680948980.973096277] [minimal_sub2]: [sub] Involuntary context switches: '15' + +ros2 run minimal_scheduling minimal_scheduling_real_time_tutorial --sched SCHED_FIFO --priority 80 [WARN] [1680947876.099416572] [minimal_sub1]: [sub] Involuntary context switches: '8' [INFO] [1680947876.099471567] [minimal_sub2]: [sub] Involuntary context switches: '0' [WARN] [1680947876.599197932] [minimal_sub1]: [sub] Involuntary context switches: '49' From 35e1dc91fa3fd00afce31897d8ebd6ddba87aad9 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Mon, 22 May 2023 14:58:36 +0200 Subject: [PATCH 3/4] updated ressources Signed-off-by: Jan Staschulat --- minimal_scheduling/README.md | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/minimal_scheduling/README.md b/minimal_scheduling/README.md index 8e19a72..baf0767 100644 --- a/minimal_scheduling/README.md +++ b/minimal_scheduling/README.md @@ -232,12 +232,16 @@ ros2 run minimal_scheduling minimal_scheduling_real_time_tutorial --sched SCHED_ ## Resources -- [https://www.man7.org/linux/man-pages/man7/sched.7.html](https://www.man7.org/linux/man-pages/man7/sched.7.html) -- [https://wiki.linuxfoundation.org/realtime/documentation/technical_basics/sched_policy_prio/start](https://wiki.linuxfoundation.org/realtime/documentation/technical_basics/sched_policy_prio/start) -- [https://wiki.linuxfoundation.org/realtime/documentation/howto/applications/application_base](https://wiki.linuxfoundation.org/realtime/documentation/howto/applications/application_base) -- [https://linux.die.net/man/3/pthread_setschedparam](https://linux.die.net/man/3/pthread_setschedparam) -- [https://linux.die.net/man/3/pthread_create](https://linux.die.net/man/3/pthread_create) -- [https://access.redhat.com/documentation/en-us/red_hat_enterprise_linux/6/html/performance_tuning_guide/s-cpu-scheduler](https://access.redhat.com/documentation/en-us/red_hat_enterprise_linux/6/html/performance_tuning_guide/s-cpu-scheduler) -- Callback groups executor - Ralph Lange [video](https://www.youtube.com/watch?v=5Sd5bvvZeb0), [slides](https://www.apex.ai/_files/ugd/984e93_f3791ae0711042a883bfc40f827d6268.pdf) -- [https://github.com/ros2/examples/tree/master/rclcpp/executors/cbg_executor](https://github.com/ros2/examples/tree/master/rclcpp/executors/cbg_executor) -- [Basic investigation of priority of DDS-generated threads.](https://discourse.ros.org/uploads/short-url/p2fAAbrKHkrSqZ9oJkZNwOOf2Hi.pdf) +- Linux scheduling and multi-threading + - [https://www.man7.org/linux/man-pages/man7/sched.7.html](https://www.man7.org/linux/man-pages/man7/sched.7.html) + - [https://wiki.linuxfoundation.org/realtime/documentation/technical_basics/sched_policy_prio/start](https://wiki.linuxfoundation.org/realtime/documentation/technical_basics/sched_policy_prio/start) + - [https://wiki.linuxfoundation.org/realtime/documentation/howto/applications/application_base](https://wiki.linuxfoundation.org/realtime/documentation/howto/applications/application_base) + - [https://linux.die.net/man/3/pthread_setschedparam](https://linux.die.net/man/3/pthread_setschedparam) + - [https://linux.die.net/man/3/pthread_create](https://linux.die.net/man/3/pthread_create) + - [https://access.redhat.com/documentation/en-us/red_hat_enterprise_linux/6/html/performance_tuning_guide/s-cpu-scheduler](https://access.redhat.com/documentation/en-us/red_hat_enterprise_linux/6/html/performance_tuning_guide/s-cpu-scheduler) +- ROS 2 Execution Management + - ROS 2 documentation [About-Executors](https://docs.ros.org/en/humble/Concepts/About-Executors.html) + - ROS 2 documentation [Using-callback-groups](https://docs.ros.org/en/humble/How-To-Guides/Using-callback-groups.html) + - Talk "Callback group level executor", Ralph Lange, ROS 2 Executor Workshop, 2021. [video](https://www.youtube.com/watch?v=5Sd5bvvZeb0), [slides](https://www.apex.ai/_files/ugd/984e93_f3791ae0711042a883bfc40f827d6268.pdf) + - Code example with callback group level executor [https://github.com/ros2/examples/tree/master/rclcpp/executors/cbg_executor](https://github.com/ros2/examples/tree/master/rclcpp/executors/cbg_executor) + - [Basic investigation of priority of DDS-generated threads.](https://discourse.ros.org/uploads/short-url/p2fAAbrKHkrSqZ9oJkZNwOOf2Hi.pdf) From 4a8fb1b47ac7ae9f69da3255951bfffe29b7b34b Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Mon, 5 Jun 2023 17:39:35 +0200 Subject: [PATCH 4/4] changed filename Signed-off-by: Jan Staschulat --- minimal_scheduling/CMakeLists.txt | 6 +++--- minimal_scheduling/README.md | 8 ++++---- ...rial.cpp => minimal_scheduling_real_time_executor.cpp} | 0 3 files changed, 7 insertions(+), 7 deletions(-) rename minimal_scheduling/{minimal_scheduling_real_time_tutorial.cpp => minimal_scheduling_real_time_executor.cpp} (100%) diff --git a/minimal_scheduling/CMakeLists.txt b/minimal_scheduling/CMakeLists.txt index 14a00b4..f83eb48 100644 --- a/minimal_scheduling/CMakeLists.txt +++ b/minimal_scheduling/CMakeLists.txt @@ -26,15 +26,15 @@ ament_target_dependencies(minimal_scheduling_spin_thread rclcpp std_msgs) add_executable(minimal_scheduling_callback_group minimal_scheduling_callback_group.cpp) ament_target_dependencies(minimal_scheduling_callback_group rclcpp std_msgs) -add_executable(minimal_scheduling_real_time_tutorial minimal_scheduling_real_time_tutorial.cpp) -ament_target_dependencies(minimal_scheduling_real_time_tutorial rclcpp std_msgs) +add_executable(minimal_scheduling_real_time_executor minimal_scheduling_real_time_executor.cpp) +ament_target_dependencies(minimal_scheduling_real_time_executor rclcpp std_msgs) install(TARGETS minimal_scheduling_main_thread minimal_scheduling_middleware_threads minimal_scheduling_spin_thread minimal_scheduling_callback_group - minimal_scheduling_real_time_tutorial + minimal_scheduling_real_time_executor DESTINATION lib/${PROJECT_NAME} ) diff --git a/minimal_scheduling/README.md b/minimal_scheduling/README.md index baf0767..7e8cbf8 100644 --- a/minimal_scheduling/README.md +++ b/minimal_scheduling/README.md @@ -203,17 +203,17 @@ $ ps -C minimal_schedul -L -o tid,comm,rtprio,cls,psr -### minimal_scheduling_real_time_tutorial +### minimal_scheduling_real_time_executor Example to demonstrate mixed system, with a real-time thread and non real-time thread. Two publisher callbacks are created. The first one is executed in the real-time thread, the other one is executed in a non real-time thread. Output: No preemptions of the real-time publisher and multiple preemptions of the non real-time publisher. ```bash -$ ros2 run minimal_scheduling minimal_scheduling_real_time_tutorial +$ ros2 run minimal_scheduling minimal_scheduling_real_time_executor ``` Output: real-time subscriber is not preempted by other kernel processes, but normal subscriber is. ```bash -ros2 run minimal_scheduling minimal_scheduling_real_time_tutorial +ros2 run minimal_scheduling minimal_scheduling_real_time_executor [WARN] [1680948979.971439054] [minimal_sub1]: [sub] Involuntary context switches: '25' [WARN] [1680948979.971483357] [minimal_sub2]: [sub] Involuntary context switches: '20' [WARN] [1680948980.473828433] [minimal_sub1]: [sub] Involuntary context switches: '23' @@ -221,7 +221,7 @@ ros2 run minimal_scheduling minimal_scheduling_real_time_tutorial [WARN] [1680948980.972909968] [minimal_sub1]: [sub] Involuntary context switches: '26' [WARN] [1680948980.973096277] [minimal_sub2]: [sub] Involuntary context switches: '15' -ros2 run minimal_scheduling minimal_scheduling_real_time_tutorial --sched SCHED_FIFO --priority 80 +ros2 run minimal_scheduling minimal_scheduling_real_time_executor --sched SCHED_FIFO --priority 80 [WARN] [1680947876.099416572] [minimal_sub1]: [sub] Involuntary context switches: '8' [INFO] [1680947876.099471567] [minimal_sub2]: [sub] Involuntary context switches: '0' [WARN] [1680947876.599197932] [minimal_sub1]: [sub] Involuntary context switches: '49' diff --git a/minimal_scheduling/minimal_scheduling_real_time_tutorial.cpp b/minimal_scheduling/minimal_scheduling_real_time_executor.cpp similarity index 100% rename from minimal_scheduling/minimal_scheduling_real_time_tutorial.cpp rename to minimal_scheduling/minimal_scheduling_real_time_executor.cpp