Realtime tools migration (#1474)

Use RealtimeThreadSafeBox instead of RealTimeBuffer.

Also made it so that any reading or writing done outside of real-time threads will retry 10 times before failing in non-RT contexts.

Author: URJala

ur_controllers/include/ur_controllers/force_mode_controller.hpp

@@ -41,7 +41,7 @@
 #include <controller_interface/controller_interface.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <realtime_tools/realtime_buffer.hpp>
+#include <realtime_tools/realtime_thread_safe_box.hpp>
 #include <std_srvs/srv/trigger.hpp>
 #if __has_include(<tf2_ros/buffer.hpp>)
 #include <tf2_ros/buffer.hpp>
@@ -98,7 +98,7 @@ struct ForceModeParameters
   std::array<double, 6> task_frame;
   std::array<double, 6> selection_vec;
   std::array<double, 6> limits;
-  geometry_msgs::msg::Wrench wrench;
+  geometry_msgs::msg::Wrench wrench{};
   double type;
   double damping_factor;
   double gain_scaling;
@@ -137,7 +137,7 @@ class ForceModeController : public controller_interface::ControllerInterface
   std::shared_ptr<force_mode_controller::ParamListener> param_listener_;
   force_mode_controller::Params params_;
 
-  realtime_tools::RealtimeBuffer<ForceModeParameters> force_mode_params_buffer_;
+  realtime_tools::RealtimeThreadSafeBox<ForceModeParameters> force_mode_params_buffer_;
   std::atomic<bool> force_mode_active_;
   std::atomic<bool> change_requested_;
   std::atomic<double> async_state_;

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp

@@ -51,7 +51,7 @@
 #include <vector>
 
 #include <controller_interface/controller_interface.hpp>
-#include <realtime_tools/realtime_buffer.hpp>
+#include <realtime_tools/realtime_thread_safe_box.hpp>
 #include <realtime_tools/realtime_server_goal_handle.hpp>
 #include <rclcpp_action/server.hpp>
 #include <rclcpp_action/create_server.hpp>
@@ -114,11 +114,11 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI
   using FollowJTrajAction = control_msgs::action::FollowJointTrajectory;
   using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<FollowJTrajAction>;
   using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
-  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
+  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeThreadSafeBox<RealtimeGoalHandlePtr>;
 
   RealtimeGoalHandleBuffer rt_active_goal_;         ///< Currently active action goal, if any.
   rclcpp::TimerBase::SharedPtr goal_handle_timer_;  ///< Timer to frequently check on the running goal
-  realtime_tools::RealtimeBuffer<std::unordered_map<std::string, size_t>> joint_trajectory_mapping_;
+  realtime_tools::RealtimeThreadSafeBox<std::unordered_map<std::string, size_t>> joint_trajectory_mapping_;
 
   rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms);
 
@@ -147,7 +147,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI
   void goal_accepted_callback(
       std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::FollowJointTrajectory>> goal_handle);
 
-  realtime_tools::RealtimeBuffer<std::vector<std::string>> joint_names_;
+  realtime_tools::RealtimeThreadSafeBox<std::vector<std::string>> joint_names_;
   std::vector<std::string> state_interface_types_;
 
   std::vector<std::string> joint_state_interface_names_;
@@ -159,11 +159,13 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI
   bool check_goal_positions(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
   bool check_goal_velocities(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
   bool check_goal_accelerations(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
+  std::optional<RealtimeGoalHandlePtr> get_rt_goal_from_non_rt();
+  bool set_rt_goal_from_non_rt(RealtimeGoalHandlePtr& rt_goal);
 
   trajectory_msgs::msg::JointTrajectory active_joint_traj_;
   // std::vector<control_msgs::msg::JointTolerance> path_tolerance_;
-  realtime_tools::RealtimeBuffer<std::vector<control_msgs::msg::JointTolerance>> goal_tolerance_;
-  realtime_tools::RealtimeBuffer<rclcpp::Duration> goal_time_tolerance_{ rclcpp::Duration(0, 0) };
+  realtime_tools::RealtimeThreadSafeBox<std::vector<control_msgs::msg::JointTolerance>> goal_tolerance_;
+  realtime_tools::RealtimeThreadSafeBox<rclcpp::Duration> goal_time_tolerance_{ rclcpp::Duration(0, 0) };
 
   std::atomic<size_t> current_index_;
   std::atomic<bool> trajectory_active_;

ur_controllers/include/ur_controllers/tool_contact_controller.hpp

@@ -53,8 +53,9 @@
 #include <rclcpp_action/server_goal_handle.hpp>
 #include <rclcpp/duration.hpp>
 
-#include <realtime_tools/realtime_buffer.hpp>
+#include <realtime_tools/realtime_thread_safe_box.hpp>
 #include <realtime_tools/realtime_server_goal_handle.hpp>
+#include <realtime_tools/lock_free_queue.hpp>
 
 #include <ur_msgs/action/tool_contact.hpp>
 #include "ur_controllers/tool_contact_controller_parameters.hpp"
@@ -86,11 +87,13 @@ class ToolContactController : public controller_interface::ControllerInterface
 private:
   using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<ur_msgs::action::ToolContact>;
   using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
-  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
+  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeThreadSafeBox<RealtimeGoalHandlePtr>;
 
   RealtimeGoalHandleBuffer rt_active_goal_;                     ///< Currently active action goal, if any.
   ur_msgs::action::ToolContact::Feedback::SharedPtr feedback_;  ///< preallocated feedback
   rclcpp::TimerBase::SharedPtr goal_handle_timer_;              ///< Timer to frequently check on the running goal
+  std::optional<RealtimeGoalHandlePtr> get_rt_goal_from_non_rt();
+  bool set_rt_goal_from_non_rt(const RealtimeGoalHandlePtr& goal_handle);
 
   // non-rt function that will be called with action_monitor_period to monitor the rt action
   rclcpp::Duration action_monitor_period_ = rclcpp::Duration::from_seconds(0.05);

ur_controllers/include/ur_controllers/ur_configuration_controller.hpp

@@ -44,7 +44,7 @@
 #include <memory>
 
 #include <controller_interface/controller_interface.hpp>
-#include <realtime_tools/realtime_box.hpp>
+#include <realtime_tools/realtime_thread_safe_box.hpp>
 
 #include "ur_msgs/srv/get_robot_software_version.hpp"
 #include "ur_controllers/ur_configuration_controller_parameters.hpp"
@@ -85,7 +85,7 @@ class URConfigurationController : public controller_interface::ControllerInterfa
   CallbackReturn on_init() override;
 
 private:
-  realtime_tools::RealtimeBox<std::shared_ptr<VersionInformation>> robot_software_version_{
+  realtime_tools::RealtimeThreadSafeBox<std::shared_ptr<VersionInformation>> robot_software_version_{
     std::make_shared<VersionInformation>()
   };
   std::atomic<bool> robot_software_version_set_{ false };

ur_controllers/src/force_mode_controller.cpp

@@ -180,7 +180,12 @@ controller_interface::return_type ur_controllers::ForceModeController::update(co
   if (change_requested_) {
     bool write_successful = true;
     if (force_mode_active_) {
-      const auto force_mode_parameters = force_mode_params_buffer_.readFromRT();
+      const auto force_mode_parameters = force_mode_params_buffer_.try_get();
+      if (!force_mode_parameters) {
+        RCLCPP_WARN(get_node()->get_logger(), "Could not get force mode parameters from realtime buffer. Retrying in "
+                                              "next update cycle.");
+        return controller_interface::return_type::OK;
+      }
       write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(
           force_mode_parameters->task_frame[0]);
       write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(
@@ -357,7 +362,17 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request
   }
   force_mode_parameters.gain_scaling = req->gain_scaling;
 
-  force_mode_params_buffer_.writeFromNonRT(force_mode_parameters);
+  int tries = 0;
+  while (!force_mode_params_buffer_.try_set(force_mode_parameters)) {
+    if (tries > 10) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Could not set force mode parameters in realtime buffer.");
+      resp->success = false;
+      return false;
+    }
+    rclcpp::sleep_for(std::chrono::milliseconds(50));
+    tries++;
+  }
+
   force_mode_active_ = true;
   change_requested_ = true;