Attempt to use SCHED_FIFO for Servo regardless of RT kernel (#2653)

* Attempt to use SCHED_FIFO for Servo regardless of RT kernel * Update warning message if Servo fails to use SCHED_FIFO Co-authored-by: AndyZe <[email protected]> * Update moveit_ros/moveit_servo/src/servo_node.cpp Co-authored-by: Sebastian Castro <[email protected]> --------- Co-authored-by: AndyZe <[email protected]> Co-authored-by: Sebastian Castro <[email protected]>
moveit · Jan 26, 2024 · d56a8b9 · d56a8b9
1 parent 4e58ff1
commit d56a8b9
Showing 1 changed file with 9 additions and 10 deletions.
diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp
@@ -75,19 +75,18 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options)
                        "in the launch file");
   }
 
-  // Check if a realtime kernel is available
-  if (realtime_tools::has_realtime_kernel())
+  // Configure SCHED_FIFO and priority
+  if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority))
   {
-    if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority))
-    {
-      RCLCPP_INFO_STREAM(node_->get_logger(), "Realtime kernel available, higher thread priority has been set.");
-    }
-    else
-    {
-      RCLCPP_WARN_STREAM(node_->get_logger(), "Could not enable FIFO RT scheduling policy.");
-    }
+    RCLCPP_INFO_STREAM(node_->get_logger(), "Enabled SCHED_FIFO and higher thread priority.");
   }
   else
+  {
+    RCLCPP_WARN_STREAM(node_->get_logger(), "Could not enable FIFO RT scheduling policy. Continuing with the default.");
+  }
+
+  // Check if a realtime kernel is available
+  if (!realtime_tools::has_realtime_kernel())
   {
     RCLCPP_WARN_STREAM(node_->get_logger(), "Realtime kernel is recommended for better performance.");
   }