Update tool contact controller to use RealtimeThreadSafeBox

Author: URJala

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,12 @@ 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_buffer();
 
   // 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/src/tool_contact_controller.cpp

@@ -177,12 +177,18 @@ controller_interface::CallbackReturn
 ToolContactController::on_deactivate(const rclcpp_lifecycle::State& /* previous_state */)
 {
   // Abort active goal (if any)
-  const auto active_goal = *rt_active_goal_.readFromRT();
-  if (active_goal) {
+  const auto active_goal = get_rt_buffer();
+
+  if (!active_goal.has_value()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to read active goal, aborting deactivation of controller.");
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
+  if (active_goal.value()) {
     RCLCPP_INFO(get_node()->get_logger(), "Aborting tool contact, as controller has been deactivated.");
     // Mark the current goal as abort
     auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
-    active_goal->setAborted(result);
+    active_goal.value()->setAborted(result);
     should_reset_goal = true;
   }
   if (tool_contact_active_) {
@@ -210,8 +216,14 @@ rclcpp_action::GoalResponse ToolContactController::goal_received_callback(
     return rclcpp_action::GoalResponse::REJECT;
   }
 
-  const auto active_goal = *rt_active_goal_.readFromNonRT();
-  if (active_goal) {
+  const auto active_goal = get_rt_buffer();
+
+  if (!active_goal.has_value()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to read active goal, rejecting goal.");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+
+  if (active_goal.value()) {
     RCLCPP_ERROR(get_node()->get_logger(), "Tool contact already active, rejecting goal.");
     return rclcpp_action::GoalResponse::REJECT;
   }
@@ -226,7 +238,11 @@ void ToolContactController::goal_accepted_callback(
   tool_contact_abort_ = false;
   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();
   auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
       std::chrono::nanoseconds(action_monitor_period_.nanoseconds()));
@@ -236,13 +252,17 @@ void ToolContactController::goal_accepted_callback(
 
 void ToolContactController::action_handler()
 {
-  const auto active_goal = *rt_active_goal_.readFromNonRT();
-  if (active_goal) {
+  const auto active_goal = get_rt_buffer();
+  if (!active_goal.has_value()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to read active goal, aborting action handler.");
+    return;
+  }
+  if (active_goal.value()) {
     // Allow the goal to handle any actions it needs to perform
-    active_goal->runNonRealtime();
+    active_goal.value()->runNonRealtime();
     // If one of the goal ending conditions were met, reset our active goal pointer
     if (should_reset_goal) {
-      rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
+      rt_active_goal_.try_set([](RealtimeGoalHandlePtr& goal) { goal = RealtimeGoalHandlePtr(); });
       should_reset_goal = false;
     }
   }
@@ -252,13 +272,18 @@ rclcpp_action::CancelResponse ToolContactController::goal_canceled_callback(
     const std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::ToolContact>> goal_handle)
 {
   // Check that cancel request refers to currently active goal (if any)
-  const auto active_goal = *rt_active_goal_.readFromNonRT();
-  if (active_goal && active_goal->gh_ == goal_handle) {
+  const auto active_goal = get_rt_buffer();
+  if (!active_goal.has_value()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to read active goal, rejecting cancel.");
+    return rclcpp_action::CancelResponse::REJECT;
+  }
+
+  if (active_goal.value() && active_goal.value()->gh_ == goal_handle) {
     RCLCPP_INFO(get_node()->get_logger(), "Cancel tool contact requested.");
 
     // Mark the current goal as canceled
     auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
-    active_goal->setCanceled(result);
+    active_goal.value()->setCanceled(result);
     should_reset_goal = true;
     tool_contact_abort_ = true;
     tool_contact_enable_ = false;
@@ -282,7 +307,12 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
     write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_BEGIN);
   }
 
-  const auto active_goal = *rt_active_goal_.readFromRT();
+  const auto active_goal = get_rt_buffer();
+  if (!active_goal.has_value()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to read active goal, skipping update cycle.");
+    return controller_interface::return_type::OK;
+  }
+
   std::optional<double> state_optional = tool_contact_state_interface_->get().get_optional();
 
   if (!state_optional) {
@@ -311,7 +341,7 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
         write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_END);
         if (active_goal) {
           auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
-          active_goal->setSucceeded(result);
+          active_goal.value()->setSucceeded(result);
           should_reset_goal = true;
         }
       } else if (result.value() == 1.0) {
@@ -321,7 +351,7 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
         write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
         if (active_goal) {
           auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
-          active_goal->setAborted(result);
+          active_goal.value()->setAborted(result);
           should_reset_goal = true;
         }
       }
@@ -335,7 +365,7 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
 
       if (active_goal) {
         auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
-        active_goal->setAborted(result);
+        active_goal.value()->setAborted(result);
         should_reset_goal = true;
       }
     } break;
@@ -358,7 +388,7 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
 
       if (active_goal) {
         auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
-        active_goal->setAborted(result);
+        active_goal.value()->setAborted(result);
         should_reset_goal = true;
       }
     } break;
@@ -368,8 +398,8 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
     default:
       break;
   }
-  if (active_goal) {
-    active_goal->setFeedback(feedback_);
+  if (active_goal.value()) {
+    active_goal.value()->setFeedback(feedback_);
   }
 
   if (!write_success) {
@@ -379,6 +409,17 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
   return controller_interface::return_type::OK;
 }
 
+std::optional<ToolContactController::RealtimeGoalHandlePtr> ToolContactController::get_rt_buffer()
+{
+  RealtimeGoalHandlePtr active_goal = nullptr;
+  const bool read_success =
+      rt_active_goal_.try_get([&active_goal](const RealtimeGoalHandlePtr& goal) { active_goal = goal; });
+  if (!read_success) {
+    return std::nullopt;
+  }
+  return active_goal;
+}
+
 }  // namespace ur_controllers
 
 #include "pluginlib/class_list_macros.hpp"