Implement RT priority assignment

kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp

@@ -50,6 +50,58 @@ int main(int argc, char** argv)
 
   ros::NodeHandle nh;
 
+  std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
+  bool has_realtime = false;
+  if (realtime_file.is_open())
+  {
+    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)
+      {
+        ROS_ERROR_STREAM("Couldn't retrieve real-time scheduling paramers");
+      }
+
+      // 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");
+    }
+  }
+
   kuka_rsi_hw_interface::KukaHardwareInterface kuka_rsi_hw_interface;
   kuka_rsi_hw_interface.configure();