Update passthrough controller to use RealtimeThreadSafeBox

Author: URJala

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_;
@@ -162,8 +162,8 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI
 
   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/src/passthrough_trajectory_controller.cpp

@@ -65,7 +65,7 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_init()
   passthrough_params_ = passthrough_param_listener_->get_params();
   current_index_ = 0;
   auto joint_names = passthrough_params_.joints;
-  joint_names_.writeFromNonRT(joint_names);
+  joint_names_.try_set(joint_names);
   number_of_joints_ = joint_names.size();
   state_interface_types_ = passthrough_params_.state_interfaces;
   scaling_factor_ = 1.0;
@@ -83,7 +83,12 @@ PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& pre
 
   joint_state_interface_names_.reserve(number_of_joints_ * state_interface_types_.size());
 
-  auto joint_names_internal = joint_names_.readFromRT();
+  auto joint_names_internal = joint_names_.try_get();
+  if (!joint_names_internal) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Joint names could not be read, cannot configure controller.");
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
   for (const auto& joint_name : *joint_names_internal) {
     for (const auto& interface_type : state_interface_types_) {
       joint_state_interface_names_.emplace_back(joint_name + "/" + interface_type);
@@ -228,12 +233,22 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_deactiv
     return controller_interface::CallbackReturn::ERROR;
   }
   if (trajectory_active_) {
-    const auto active_goal = *rt_active_goal_.readFromRT();
+    RealtimeGoalHandlePtr active_goal = nullptr;
+    bool success = rt_active_goal_.try_get([&active_goal](const RealtimeGoalHandlePtr& goal) { active_goal = goal; });
+    if (!success) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Could not read active goal from realtime buffer, deactivation of "
+                                             "controller failed.");
+      return controller_interface::CallbackReturn::ERROR;
+    }
     std::shared_ptr<control_msgs::action::FollowJointTrajectory::Result> result =
         std::make_shared<control_msgs::action::FollowJointTrajectory::Result>();
     result->set__error_string("Aborting current goal, since the controller is being deactivated.");
     active_goal->setAborted(result);
-    rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
+    success = rt_active_goal_.try_set([](RealtimeGoalHandlePtr& goal) { goal = RealtimeGoalHandlePtr(); });
+    if (!success) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Failed to set active goal, deactivation of controller failed.");
+      return controller_interface::CallbackReturn::ERROR;
+    }
     end_goal();
   }
   return CallbackReturn::SUCCESS;
@@ -242,7 +257,13 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_deactiv
 controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/,
                                                                           const rclcpp::Duration& period)
 {
-  const auto active_goal = *rt_active_goal_.readFromRT();
+  RealtimeGoalHandlePtr active_goal = nullptr;
+  const bool read_success =
+      rt_active_goal_.try_get([&active_goal](const RealtimeGoalHandlePtr& goal) { active_goal = goal; });
+  if (!read_success) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not read active goal from realtime buffer, skipping cycle.");
+    return controller_interface::return_type::OK;
+  }
 
   const auto current_transfer_state = transfer_command_interface_->get().get_optional().value_or(TRANSFER_STATE_IDLE);
 
@@ -272,8 +293,18 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
       write_success &=
           trajectory_size_command_interface_->get().set_value(static_cast<double>(active_joint_traj_.points.size()));
     }
-    auto active_goal_time_tol = goal_time_tolerance_.readFromRT();
-    auto joint_mapping = joint_trajectory_mapping_.readFromRT();
+
+    auto active_goal_time_tol = goal_time_tolerance_.try_get();
+    if (!active_goal_time_tol) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Could not read goal time tolerance, skipping cycle.");
+      return controller_interface::return_type::OK;
+    }
+
+    auto joint_mapping = joint_trajectory_mapping_.try_get();
+    if (!joint_mapping) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Could not read joint trajectory mapping, skipping cycle.");
+      return controller_interface::return_type::OK;
+    }
 
     // Write a new point to the command interface, if the previous point has been read by the hardware interface.
     if (current_transfer_state == TRANSFER_STATE_WAITING_FOR_POINT) {
@@ -283,7 +314,12 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
             duration_to_double(active_joint_traj_.points[current_index_].time_from_start));
 
         // Write the positions for each joint of the robot
-        auto joint_names_internal = joint_names_.readFromRT();
+        auto joint_names_internal = joint_names_.try_get();
+        if (!joint_names_internal) {
+          RCLCPP_ERROR(get_node()->get_logger(), "Could not read joint names, skipping cycle.");
+          return controller_interface::return_type::OK;
+        }
+
         // We've added the joint interfaces matching the order of the joint names so we can safely access
         // them by the index.
         for (size_t i = 0; i < number_of_joints_; i++) {
@@ -422,7 +458,12 @@ bool PassthroughTrajectoryController::check_goal_tolerances(
     std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal)
 {
   auto& tolerances = goal->goal_tolerance;
-  auto joint_names_internal = joint_names_.readFromRT();
+  auto joint_names_internal = joint_names_.try_get();
+  if (!joint_names_internal) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Joint names could not be read, cannot check tolerances.");
+    return false;
+  }
+
   if (!tolerances.empty()) {
     for (auto& tol : tolerances) {
       auto found_it = std::find(joint_names_internal->begin(), joint_names_internal->end(), tol.name);
@@ -526,14 +567,26 @@ rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_ca
     const std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::FollowJointTrajectory>> goal_handle)
 {
   // Check that cancel request refers to currently active goal (if any)
-  const auto active_goal = *rt_active_goal_.readFromNonRT();
+  RealtimeGoalHandlePtr active_goal = nullptr;
+  while (!rt_active_goal_.try_get([&active_goal](const RealtimeGoalHandlePtr& goal) { active_goal = goal; })) {
+    // Wait until we can read the active goal from the realtime buffer
+    RCLCPP_WARN(get_node()->get_logger(), "Waiting for active goal to be set, retrying cancel request.");
+    rclcpp::sleep_for(std::chrono::milliseconds(50));
+  }
+
   if (active_goal && active_goal->gh_ == goal_handle) {
     RCLCPP_INFO(get_node()->get_logger(), "Cancelling active trajectory requested.");
 
     // Mark the current goal as canceled
     auto result = std::make_shared<FollowJTrajAction::Result>();
     active_goal->setCanceled(result);
-    rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
+
+    while (!rt_active_goal_.try_set([](RealtimeGoalHandlePtr& goal) { goal = RealtimeGoalHandlePtr(); })) {
+      // Wait until we can set the active goal to nullptr
+      RCLCPP_WARN(get_node()->get_logger(), "Waiting for active goal to be set, retrying cancel request.");
+      rclcpp::sleep_for(std::chrono::milliseconds(50));
+    }
+
     trajectory_active_ = false;
   }
   return rclcpp_action::CancelResponse::ACCEPT;
@@ -548,13 +601,21 @@ void PassthroughTrajectoryController::goal_accepted_callback(
   current_index_ = 0;
 
   // TODO(fexner): Merge goal tolerances with default tolerances
-
-  joint_trajectory_mapping_.writeFromNonRT(create_joint_mapping(goal_handle->get_goal()->trajectory.joint_names));
+  bool write_success =
+      joint_trajectory_mapping_.try_set(create_joint_mapping(goal_handle->get_goal()->trajectory.joint_names));
+  if (!write_success) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to set joint trajectory mapping.");
+    return;
+  }
 
   // sort goal tolerances to match internal joint order
   std::vector<control_msgs::msg::JointTolerance> goal_tolerances;
   if (!goal_handle->get_goal()->goal_tolerance.empty()) {
-    auto joint_names_internal = joint_names_.readFromRT();
+    auto joint_names_internal = joint_names_.try_get();
+    if (!joint_names_internal) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Joint names could not be read, cannot set goal tolerances.");
+      return;
+    }
     std::stringstream ss;
     ss << "Using goal tolerances\n";
     for (auto& joint_name : *joint_names_internal) {
@@ -569,8 +630,17 @@ void PassthroughTrajectoryController::goal_accepted_callback(
     }
     RCLCPP_INFO_STREAM(get_node()->get_logger(), ss.str());
   }
-  goal_tolerance_.writeFromNonRT(goal_tolerances);
-  goal_time_tolerance_.writeFromNonRT(goal_handle->get_goal()->goal_time_tolerance);
+  write_success = goal_tolerance_.try_set(goal_tolerances);
+  if (!write_success) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to set goal tolerances.");
+    return;
+  }
+
+  write_success = goal_time_tolerance_.try_set(goal_handle->get_goal()->goal_time_tolerance);
+  if (!write_success) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to set goal time tolerance.");
+    return;
+  }
   RCLCPP_INFO_STREAM(get_node()->get_logger(),
                      "Goal time tolerance: " << duration_to_double(goal_handle->get_goal()->goal_time_tolerance)
                                              << " sec");
@@ -581,7 +651,11 @@ void PassthroughTrajectoryController::goal_accepted_callback(
   //
   RealtimeGoalHandlePtr rt_goal = std::make_shared<RealtimeGoalHandle>(goal_handle);
   rt_goal->execute();
-  rt_active_goal_.writeFromNonRT(rt_goal);
+  bool write_success;
+  write_success = rt_active_goal_.try_set([&rt_goal](RealtimeGoalHandlePtr& goal) { goal = rt_goal; });
+  if (!write_success) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to set active goal.");
+  }
   goal_handle_timer_.reset();
   goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_.to_chrono<std::chrono::nanoseconds>(),
                                                      std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal));
@@ -591,9 +665,22 @@ void PassthroughTrajectoryController::goal_accepted_callback(
 
 bool PassthroughTrajectoryController::check_goal_tolerance()
 {
-  auto goal_tolerance = goal_tolerance_.readFromRT();
-  auto joint_mapping = joint_trajectory_mapping_.readFromRT();
-  auto joint_names_internal = joint_names_.readFromRT();
+  auto goal_tolerance = goal_tolerance_.try_get();
+  if (!goal_tolerance) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not get goal tolerance, cannot check goal tolerance.");
+    return false;
+  }
+  auto joint_mapping = joint_trajectory_mapping_.try_get();
+  if (!joint_mapping) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not get joint mapping, cannot check goal tolerance.");
+    return false;
+  }
+  auto joint_names_internal = joint_names_.try_get();
+  if (!joint_names_internal) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not get joint names, cannot check goal tolerance.");
+    return false;
+  }
+
   if (goal_tolerance->empty()) {
     return true;
   }
@@ -650,7 +737,11 @@ std::unordered_map<std::string, size_t>
 PassthroughTrajectoryController::create_joint_mapping(const std::vector<std::string>& joint_names) const
 {
   std::unordered_map<std::string, size_t> joint_mapping;
-  auto joint_names_internal = joint_names_.readFromNonRT();
+  auto joint_names_internal = joint_names_.try_get();
+  if (!joint_names_internal) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not get joint names, cannot create joint mapping.");
+  }
+
   for (auto& joint_name : *joint_names_internal) {
     auto found_it = std::find(joint_names.begin(), joint_names.end(), joint_name);
     if (found_it != joint_names.end()) {