Merge pull request #47 from matthias-mayr/pr_realtime

Feature: Introduces realtime support.
epfl-lasa · Nov 14, 2022 · 6baddce · 6baddce
2 parents d55884c + 26b8a49
commit 6baddce
Showing 1 changed file with 38 additions and 2 deletions.
diff --git a/iiwa_driver/src/iiwa.cpp b/iiwa_driver/src/iiwa.cpp
@@ -36,6 +36,33 @@
 #include <thread>
 
 namespace iiwa_ros {
+    bool has_realtime_kernel() {
+        std::ifstream realtime("/sys/kernel/realtime", std::ios_base::in);
+        bool is_realtime;
+        realtime >> is_realtime;
+        return is_realtime;
+    }
+
+    bool set_thread_to_highest_priority(std::string& error_message) {
+        const int thread_priority = sched_get_priority_max(SCHED_FIFO);
+        if (thread_priority == -1) {
+            if (error_message.empty()) {
+                error_message = std::string("Unable to get maximum possible thread priority: ") + std::strerror(errno);
+            }
+            return false;
+        }
+
+        sched_param thread_param{};
+        thread_param.sched_priority = thread_priority;
+        if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &thread_param) != 0) {
+            if (error_message.empty()) {
+                error_message = std::string("Unable to set realtime scheduling: ") + std::strerror(errno);
+            }
+            return false;
+        }
+        return true;
+    }
+
     Iiwa::Iiwa(ros::NodeHandle& nh)
     {
         init(nh);
@@ -59,6 +86,17 @@ namespace iiwa_ros {
         _commanding_status_pub = _nh.advertise<std_msgs::Bool>("commanding_status", 100);
         _controller_manager.reset(new controller_manager::ControllerManager(this, _nh));
 
+        if (has_realtime_kernel()) {
+            std::string error_message;
+            if (!set_thread_to_highest_priority(error_message)) {
+                ROS_ERROR_STREAM(error_message);
+            } else {
+                ROS_INFO_STREAM("Initializing with realtime scheduling support.");
+            }
+        } else {
+            ROS_INFO_STREAM("Initializing without realtime scheduling support.");
+        }
+
         if (_init_fri())
             _initialized = true;
         else
@@ -207,7 +245,6 @@ namespace iiwa_ros {
 
     void Iiwa::_ctrl_loop()
     {
-        static ros::Rate rate(_control_freq);
         while (ros::ok()) {
             ros::Time time = ros::Time::now();
 
@@ -230,7 +267,6 @@ namespace iiwa_ros {
             }
 
             _publish();
-            rate.sleep();
         }
     }