Update force mode controller to use RealtimeThreadSafeBox

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/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,11 @@ 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);
+  if (!force_mode_params_buffer_.try_set(force_mode_parameters)) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not set force mode parameters in realtime buffer.");
+    resp->success = false;
+    return false;
+  }
   force_mode_active_ = true;
   change_requested_ = true;