set rt priority

diff --git a/src/hw_node.cpp b/src/hw_node.cpp
index 7f3cfabba51eddce90f334dc36cca0d402259fef..0ac70c4cc19696eb892689071561bca30b693cd7 100644 (file)
--- a/src/hw_node.cpp
+++ b/src/hw_node.cpp
@@ -367,7 +367,18 @@ class A4wd3 : public rclcpp::Node {
 
 int main(int argc, char ** argv) {
        rclcpp::init(argc, argv);
-       rclcpp::spin(std::make_shared<A4wd3>());
+       auto node = std::make_shared<A4wd3>();
+       auto spin_thread = std::thread(
+               [&]() {
+                       rclcpp::spin(node);
+               });
+       struct sched_param param;
+       param.sched_priority = 10;
+       if  (pthread_setschedparam(spin_thread.native_handle(), SCHED_FIFO, &param) != 0) {
+               perror("Unable to set realtime priority");
+       }
+       spin_thread.join();
+
        rclcpp::shutdown();
 
        return 0;