Use SCHED_FIFO for controller_manager's main thread (#719)

Previous investigations showed that using FIFO scheduling helps keeping
cycle times also non non-RT kernels. This combined with non-blocking read
can result in a very stable system.

This is, in fact, very close to what the actual controller_manager_node
does except that we always use FIFO scheduling independent of the actual
kernel in use.

Author: Felix Exner (fexner)

ur_robot_driver/src/ur_ros2_control_node.cpp

@@ -41,6 +41,7 @@
 // ROS includes
 #include "controller_manager/controller_manager.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "realtime_tools/thread_priority.hpp"
 
 // code is inspired by
 // https://github.com/ros-controls/ros2_control/blob/master/controller_manager/src/ros2_control_node.cpp
@@ -56,14 +57,32 @@ int main(int argc, char** argv)
 
   // control loop thread
   std::thread control_loop([controller_manager]() {
-    // use fixed time step
-    const rclcpp::Duration dt = rclcpp::Duration::from_seconds(1.0 / controller_manager->get_update_rate());
+    if (!realtime_tools::configure_sched_fifo(50)) {
+      RCLCPP_WARN(controller_manager->get_logger(), "Could not enable FIFO RT scheduling policy");
+    }
+
+    // for calculating sleep time
+    auto const period = std::chrono::nanoseconds(1'000'000'000 / controller_manager->get_update_rate());
+    auto const cm_now = std::chrono::nanoseconds(controller_manager->now().nanoseconds());
+    std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> next_iteration_time{ cm_now };
+
+    // for calculating the measured period of the loop
+    rclcpp::Time previous_time = controller_manager->now();
 
     while (rclcpp::ok()) {
-      // ur client library is blocking and is the one that is controlling time step
-      controller_manager->read(controller_manager->now(), dt);
-      controller_manager->update(controller_manager->now(), dt);
-      controller_manager->write(controller_manager->now(), dt);
+      // calculate measured period
+      auto const current_time = controller_manager->now();
+      auto const measured_period = current_time - previous_time;
+      previous_time = current_time;
+
+      // execute update loop
+      controller_manager->read(controller_manager->now(), measured_period);
+      controller_manager->update(controller_manager->now(), measured_period);
+      controller_manager->write(controller_manager->now(), measured_period);
+
+      // wait until we hit the end of the period
+      next_iteration_time += period;
+      std::this_thread::sleep_until(next_iteration_time);
     }
   });