diff --git a/minimal_scheduling/minimal_scheduling_real_time_executor.cpp b/minimal_scheduling/minimal_scheduling_real_time_executor.cpp index 1bc4152..7ec689d 100644 --- a/minimal_scheduling/minimal_scheduling_real_time_executor.cpp +++ b/minimal_scheduling/minimal_scheduling_real_time_executor.cpp @@ -17,6 +17,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "rcpputils/thread.hpp" #include "std_msgs/msg/string.hpp" #include "rusage_utils.hpp" @@ -113,8 +114,14 @@ int main(int argc, char * argv[]) 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; + + rcpputils::ThreadAttribute default_attr; + rcpputils::ThreadAttribute realtime_attr; + realtime_attr.set_sched_policy(rcpputils::SchedPolicy::fifo); + realtime_attr.set_priority(options.priority); + + rclcpp::executors::SingleThreadedExecutor default_executor(rclcpp::ExecutorOptions(), default_attr); + rclcpp::executors::SingleThreadedExecutor realtime_executor(rclcpp::ExecutorOptions(), realtime_attr); // the publisher and non real-time subscriber are processed by default_executor default_executor.add_node(node_pub); @@ -132,21 +139,19 @@ int main(int argc, char * argv[]) // of the thread, in which the Executor is spinning. // spin non real-time tasks in a separate thread - auto default_thread = std::thread( + auto default_executor_thread = std::thread( [&]() { default_executor.spin(); }); // spin real-time tasks in a separate thread - auto realtime_thread = std::thread( + auto realtime_executor_thread = std::thread( [&]() { realtime_executor.spin(); }); - set_thread_scheduling(realtime_thread.native_handle(), options.policy, options.priority); - - default_thread.join(); - realtime_thread.join(); + default_executor_thread.join(); + realtime_executor_thread.join(); rclcpp::shutdown(); return 0; diff --git a/minimal_scheduling/minimal_scheduling_spin_thread.cpp b/minimal_scheduling/minimal_scheduling_spin_thread.cpp index eea69da..1b5892b 100644 --- a/minimal_scheduling/minimal_scheduling_spin_thread.cpp +++ b/minimal_scheduling/minimal_scheduling_spin_thread.cpp @@ -17,6 +17,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "rcpputils/thread.hpp" #include "std_msgs/msg/string.hpp" #include "rusage_utils.hpp" @@ -85,12 +86,15 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); auto node = std::make_shared(); - auto spin_thread = std::thread( + rcpputils::ThreadAttribute attr; + attr.set_sched_policy(rcpputils::SchedPolicy::fifo); + attr.set_priority(options.priority); + auto spin_thread = rcpputils::Thread( + attr, [&]() { rclcpp::spin(node); }); - set_thread_scheduling(spin_thread.native_handle(), options.policy, options.priority); spin_thread.join(); rclcpp::shutdown(); diff --git a/minimal_scheduling/package.xml b/minimal_scheduling/package.xml index 3057753..daa3395 100644 --- a/minimal_scheduling/package.xml +++ b/minimal_scheduling/package.xml @@ -12,9 +12,11 @@ ament_cmake rclcpp + rcpputils std_msgs rclcpp + rcpputils std_msgs ament_lint_auto