From c79c5674127e7817639435a91a016875c5d314c2 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Wed, 8 May 2019 17:47:44 +0200
Subject: [PATCH] start main and producer thread with realtime priority

---
 .../include/ur_rtde_driver/comm/pipeline.h    | 54 +++++++++++++++++++
 .../src/ros/hardware_interface_node.cpp       | 49 +++++++++++++++++
 2 files changed, 103 insertions(+)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h b/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h
index 315bf6a..22a17dc 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h
@@ -27,6 +27,7 @@
 #include <chrono>
 #include <thread>
 #include <vector>
+#include <fstream>
 
 namespace ur_driver
 {
@@ -198,6 +199,59 @@ private:
 
   void runProducer()
   {
+    LOG_INFO("Starting up producer");
+    std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
+    bool has_realtime;
+    realtime_file >> has_realtime;
+    if (has_realtime)
+    {
+      const int max_thread_priority = sched_get_priority_max(SCHED_FIFO);
+      if (max_thread_priority != -1)
+      {
+        // We'll operate on the currently running thread.
+        pthread_t this_thread = pthread_self();
+
+        // struct sched_param is used to store the scheduling priority
+        struct sched_param params;
+
+        // We'll set the priority to the maximum.
+        params.sched_priority = max_thread_priority;
+
+        int ret = pthread_setschedparam(this_thread, SCHED_FIFO, &params);
+        if (ret != 0)
+        {
+          ROS_ERROR_STREAM("Unsuccessful in setting producer thread realtime priority. Error code: " << ret);
+        }
+        // Now verify the change in thread priority
+        int policy = 0;
+        ret = pthread_getschedparam(this_thread, &policy, &params);
+        if (ret != 0)
+        {
+          std::cout << "Couldn't retrieve real-time scheduling paramers" << std::endl;
+        }
+
+        // Check the correct policy was applied
+        if (policy != SCHED_FIFO)
+        {
+          ROS_ERROR("Producer thread: Scheduling is NOT SCHED_FIFO!");
+        }
+        else
+        {
+          ROS_INFO("Producer thread: SCHED_FIFO OK");
+        }
+
+        // Print thread scheduling priority
+        ROS_INFO_STREAM("Thread priority is " << params.sched_priority);
+      }
+      else
+      {
+        ROS_ERROR("Could not get maximum thread priority for producer thread");
+      }
+    }
+    else
+    {
+      LOG_WARN("No realtime capabilities found. Consider using a realtime system for better performance");
+    }
     std::vector<std::unique_ptr<_package_type>> products;
     while (running_)
     {
diff --git a/ur_rtde_driver/src/ros/hardware_interface_node.cpp b/ur_rtde_driver/src/ros/hardware_interface_node.cpp
index d936a4e..7621d0e 100644
--- a/ur_rtde_driver/src/ros/hardware_interface_node.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface_node.cpp
@@ -60,6 +60,55 @@ int main(int argc, char** argv)
 
   ros::Rate control_rate(static_cast<double>(hw_interface.getControlFrequency()));
 
+  std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
+  bool has_realtime;
+  realtime_file >> has_realtime;
+  if (has_realtime)
+  {
+    const int max_thread_priority = sched_get_priority_max(SCHED_FIFO);
+    if (max_thread_priority != -1)
+    {
+      // We'll operate on the currently running thread.
+      pthread_t this_thread = pthread_self();
+
+      // struct sched_param is used to store the scheduling priority
+      struct sched_param params;
+
+      // We'll set the priority to the maximum.
+      params.sched_priority = max_thread_priority;
+
+      int ret = pthread_setschedparam(this_thread, SCHED_FIFO, &params);
+      if (ret != 0)
+      {
+        ROS_ERROR_STREAM("Unsuccessful in setting main thread realtime priority. Error code: " << ret);
+      }
+      // Now verify the change in thread priority
+      int policy = 0;
+      ret = pthread_getschedparam(this_thread, &policy, &params);
+      if (ret != 0)
+      {
+        std::cout << "Couldn't retrieve real-time scheduling paramers" << std::endl;
+      }
+
+      // Check the correct policy was applied
+      if (policy != SCHED_FIFO)
+      {
+        ROS_ERROR("Main thread: Scheduling is NOT SCHED_FIFO!");
+      }
+      else
+      {
+        ROS_INFO("Main thread: SCHED_FIFO OK");
+      }
+
+      // Print thread scheduling priority
+      ROS_INFO_STREAM("Main thread priority is " << params.sched_priority);
+    }
+    else
+    {
+      ROS_ERROR("Could not get maximum thread priority for main thread");
+    }
+  }
+
   // Run as fast as possible
   while (ros::ok())
   {