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.

Author: fmauch

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)

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>
@@ -245,8 +246,15 @@ class Pipeline
    * \param name The pipeline's name
    * \param notifier The notifier to use
    */
-  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)
   {
   }
   /*!
@@ -257,8 +265,14 @@ class Pipeline
    * \param name The pipeline's name
    * \param notifier The notifier to use
    */
-  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,62 +363,18 @@ 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::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
     std::vector<std::unique_ptr<T>> products;
     while (running_)
     {

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)