Specifically set RTDE pipeline producer to FIFO scheduling (#139)

Felix Exner (fexner) · web-flow · commit f170fcfdf3a4 · 2023-06-26T16:21:01.000+02:00
* Specifically set RTDE pipeline producer to FIFO scheduling

Regardless of the actual kernel used, setting the producer thread to FIFO
scheduling seems to improve robustness.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -46,6 +46,7 @@ add_library(urcl SHARED
     src/rtde/rtde_writer.cpp
     src/default_log_handler.cpp
     src/log.cpp
+    src/helpers.cpp
 )
 add_library(ur_client_library::urcl ALIAS urcl)
 target_compile_options(urcl PRIVATE -Wall -Wextra -Wno-unused-parameter)
diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h
@@ -22,6 +22,7 @@
 
 #include "ur_client_library/comm/package.h"
 #include "ur_client_library/log.h"
+#include "ur_client_library/helpers.h"
 #include "ur_client_library/queue/readerwriterqueue.h"
 #include <atomic>
 #include <chrono>
@@ -244,9 +245,17 @@ class Pipeline
    * \param consumer The consumer to run in the pipeline
    * \param name The pipeline's name
    * \param notifier The notifier to use
+   * \param producer_fifo_scheduling Should the producer thread use FIFO scheduling?
    */
-  Pipeline(IProducer<T>& producer, IConsumer<T>* consumer, std::string name, INotifier& notifier)
-    : producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
+  Pipeline(IProducer<T>& producer, IConsumer<T>* consumer, std::string name, INotifier& notifier,
+           const bool producer_fifo_scheduling = false)
+    : producer_(producer)
+    , consumer_(consumer)
+    , name_(name)
+    , notifier_(notifier)
+    , queue_{ 32 }
+    , running_{ false }
+    , producer_fifo_scheduling_(producer_fifo_scheduling)
   {
   }
   /*!
@@ -256,9 +265,16 @@ class Pipeline
    * \param producer The producer to run in the pipeline
    * \param name The pipeline's name
    * \param notifier The notifier to use
+   * \param producer_fifo_scheduling Should the producer thread use FIFO scheduling?
    */
-  Pipeline(IProducer<T>& producer, std::string name, INotifier& notifier)
-    : producer_(producer), consumer_(nullptr), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false }
+  Pipeline(IProducer<T>& producer, std::string name, INotifier& notifier, const bool producer_fifo_scheduling = false)
+    : producer_(producer)
+    , consumer_(nullptr)
+    , name_(name)
+    , notifier_(notifier)
+    , queue_{ 32 }
+    , running_{ false }
+    , producer_fifo_scheduling_(producer_fifo_scheduling)
   {
   }
 
@@ -349,61 +365,16 @@ class Pipeline
   moodycamel::BlockingReaderWriterQueue<std::unique_ptr<T>> queue_;
   std::atomic<bool> running_;
   std::thread pThread_, cThread_;
+  bool producer_fifo_scheduling_;
 
   void runProducer()
   {
     URCL_LOG_DEBUG("Starting up producer");
-    std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
-    bool has_realtime;
-    realtime_file >> has_realtime;
-    if (has_realtime)
+    if (producer_fifo_scheduling_)
     {
+      pthread_t this_thread = pthread_self();
       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)
-        {
-          URCL_LOG_ERROR("Unsuccessful in setting producer thread realtime priority. Error code: %d", 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)
-        {
-          URCL_LOG_ERROR("Producer thread: Scheduling is NOT SCHED_FIFO!");
-        }
-        else
-        {
-          URCL_LOG_INFO("Producer thread: SCHED_FIFO OK");
-        }
-
-        // Print thread scheduling priority
-        URCL_LOG_INFO("Thread priority is %d", params.sched_priority);
-      }
-      else
-      {
-        URCL_LOG_ERROR("Could not get maximum thread priority for producer thread");
-      }
-    }
-    else
-    {
-      URCL_LOG_WARN("No realtime capabilities found. Consider using a realtime system for better performance");
+      setFiFoScheduling(this_thread, max_thread_priority);
     }
     std::vector<std::unique_ptr<T>> products;
     while (running_)
diff --git a/include/ur_client_library/helpers.h b/include/ur_client_library/helpers.h
@@ -0,0 +1,38 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2022 FZI Forschungszentrum Informatik
+// Created on behalf of Universal Robots A/S
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Exner exner@fzi.de
+ * \date    2022-12-15
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED
+#define UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED
+
+#include <thread>
+
+namespace urcl
+{
+bool setFiFoScheduling(pthread_t& thread, const int priority);
+}
+#endif  // ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED
diff --git a/src/helpers.cpp b/src/helpers.cpp
@@ -0,0 +1,89 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2022 FZI Forschungszentrum Informatik
+// Created on behalf of Universal Robots A/S
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Exner exner@fzi.de
+ * \date    2022-12-15
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <ur_client_library/helpers.h>
+#include <ur_client_library/log.h>
+
+#include <cstring>
+#include <fstream>
+#include <iostream>
+
+namespace urcl
+{
+bool setFiFoScheduling(pthread_t& thread, const int priority)
+{
+  struct sched_param params;
+  params.sched_priority = priority;
+  int ret = pthread_setschedparam(thread, SCHED_FIFO, &params);
+  if (ret != 0)
+  {
+    switch (ret)
+    {
+      case EPERM:
+      {
+        URCL_LOG_ERROR("Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency "
+                       "kernel with FIFO scheduling. See "
+                       "https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/"
+                       "doc/real_time.md for details.");
+        break;
+      }
+      default:
+
+      {
+        URCL_LOG_ERROR("Unsuccessful in setting thread to FIFO scheduling with priority %i. %s", priority,
+                       strerror(ret));
+      }
+    }
+  }
+  // Now verify the change in thread priority
+  int policy = 0;
+  ret = pthread_getschedparam(thread, &policy, &params);
+  if (ret != 0)
+  {
+    URCL_LOG_ERROR("Couldn't retrieve scheduling parameters");
+    return false;
+  }
+
+  // Check the correct policy was applied
+  if (policy != SCHED_FIFO)
+  {
+    URCL_LOG_ERROR("Scheduling is NOT SCHED_FIFO!");
+    return false;
+  }
+  else
+  {
+    URCL_LOG_INFO("SCHED_FIFO OK, priority %i", params.sched_priority);
+    if (params.sched_priority != priority)
+    {
+      URCL_LOG_ERROR("Thread priority is %i instead of the expected %i", params.sched_priority, priority);
+      return false;
+    }
+  }
+  return true;
+}
+}  // namespace urcl
diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp
@@ -41,7 +41,7 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st
   , input_recipe_(readRecipe(input_recipe_file))
   , parser_(output_recipe_)
   , prod_(stream_, parser_)
-  , pipeline_(prod_, PIPELINE_NAME, notifier)
+  , pipeline_(prod_, PIPELINE_NAME, notifier, true)
   , writer_(&stream_, input_recipe_)
   , max_frequency_(URE_MAX_FREQUENCY)
   , target_frequency_(target_frequency)

Original file line number	Diff line number	Diff line change
`@@ -46,6 +46,7 @@ add_library(urcl SHARED`
`46`	`46`	`src/rtde/rtde_writer.cpp`
`47`	`47`	`src/default_log_handler.cpp`
`48`	`48`	`src/log.cpp`
	`49`	`+ src/helpers.cpp`
`49`	`50`	`)`
`50`	`51`	`add_library(ur_client_library::urcl ALIAS urcl)`
`51`	`52`	`target_compile_options(urcl PRIVATE -Wall -Wextra -Wno-unused-parameter)`