Add retry for realtime boxes outside realtime thread.

Author: URJala

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp

@@ -159,6 +159,8 @@ 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_;

ur_controllers/src/passthrough_trajectory_controller.cpp

@@ -54,6 +54,74 @@
 namespace ur_controllers
 {
 
+template <class RTBox>
+auto get_rt_box_from_non_rt(RTBox& rt_box)
+{
+  int tries = 0;
+  auto return_val = rt_box.try_get();
+  while (!return_val.has_value()) {
+    if (tries > 9) {
+      RCLCPP_ERROR(rclcpp::get_logger("PassthroughTrajectoryController"), "Failed to get value from realtime box.");
+      return decltype(return_val){};
+    }
+    RCLCPP_WARN(rclcpp::get_logger("PassthroughTrajectoryController"), "Waiting for value to be set in realtime box, "
+                                                                       "retrying...");
+    rclcpp::sleep_for(std::chrono::milliseconds(50));
+    return_val = rt_box.try_get();
+    tries++;
+  }
+  return return_val;
+}
+
+template <class RTBox, class ValueType>
+bool set_rt_box_from_non_rt(RTBox& rt_box, const ValueType& value)
+{
+  int tries = 0;
+  while (!rt_box.try_set(value)) {
+    if (tries > 9) {
+      RCLCPP_ERROR(rclcpp::get_logger("PassthroughTrajectoryController"), "Failed to get value from realtime box.");
+      return false;
+    }
+    RCLCPP_WARN(rclcpp::get_logger("PassthroughTrajectoryController"), "Waiting for value to be set in realtime box, "
+                                                                       "retrying...");
+    rclcpp::sleep_for(std::chrono::milliseconds(50));
+    tries++;
+  }
+  return true;
+}
+
+std::optional<PassthroughTrajectoryController::RealtimeGoalHandlePtr>
+PassthroughTrajectoryController::get_rt_goal_from_non_rt()
+{
+  RealtimeGoalHandlePtr active_goal = nullptr;
+  int tries = 0;
+  while (!rt_active_goal_.try_get([&active_goal](const RealtimeGoalHandlePtr& goal) { active_goal = goal; })) {
+    if (tries > 9) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Failed to get active goal from realtime box.");
+      return std::nullopt;
+    }
+    RCLCPP_WARN(get_node()->get_logger(), "Waiting for active goal to be read, retrying...");
+    rclcpp::sleep_for(std::chrono::milliseconds(50));
+    tries++;
+  }
+  return active_goal;
+}
+
+bool PassthroughTrajectoryController::set_rt_goal_from_non_rt(RealtimeGoalHandlePtr& rt_goal)
+{
+  int tries = 0;
+  while (!rt_active_goal_.try_set([&rt_goal](RealtimeGoalHandlePtr& goal) { goal = rt_goal; })) {
+    if (tries > 9) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Failed to set active goal in realtime box.");
+      return false;
+    }
+    RCLCPP_WARN(get_node()->get_logger(), "Waiting for active goal to be set, retrying...");
+    rclcpp::sleep_for(std::chrono::milliseconds(50));
+    tries++;
+  }
+  return true;
+}
+
 double duration_to_double(const builtin_interfaces::msg::Duration& duration)
 {
   return duration.sec + (duration.nanosec / 1000000000.0);
@@ -65,14 +133,18 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_init()
   passthrough_params_ = passthrough_param_listener_->get_params();
   current_index_ = 0;
   auto joint_names = passthrough_params_.joints;
-  joint_names_.try_set(joint_names);
+  if (!set_rt_box_from_non_rt(joint_names_, joint_names)) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to set joint names in realtime box.");
+    return controller_interface::CallbackReturn::ERROR;
+  }
   number_of_joints_ = joint_names.size();
   state_interface_types_ = passthrough_params_.state_interfaces;
   scaling_factor_ = 1.0;
   clock_ = get_node()->get_clock();
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
+// Not part of real time loop
 controller_interface::CallbackReturn
 PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& previous_state)
 {
@@ -83,12 +155,11 @@ 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_.try_get();
-  if (!joint_names_internal) {
-    RCLCPP_ERROR(get_node()->get_logger(), "Joint names could not be read, cannot configure controller.");
+  auto joint_names_internal = get_rt_box_from_non_rt(joint_names_);
+  if (!joint_names_internal.has_value()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to get joint names from realtime box.");
     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);
@@ -458,9 +529,9 @@ 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_.try_get();
-  if (!joint_names_internal) {
-    RCLCPP_ERROR(get_node()->get_logger(), "Joint names could not be read, cannot check tolerances.");
+  auto joint_names_internal = get_rt_box_from_non_rt(joint_names_);
+  if (!joint_names_internal.has_value()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to get joint names from realtime box.");
     return false;
   }
 
@@ -567,26 +638,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)
-  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));
+  auto goal = get_rt_goal_from_non_rt();
+  if (!goal.has_value()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to get goal handle.");
+    return rclcpp_action::CancelResponse::REJECT;
   }
+  RealtimeGoalHandlePtr active_goal = goal.value();
 
   if (active_goal && active_goal->gh_ == goal_handle) {
     RCLCPP_INFO(get_node()->get_logger(), "Cancelling active trajectory requested.");
 
+    auto new_goal = RealtimeGoalHandlePtr();
+    if (!set_rt_goal_from_non_rt(new_goal)) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Failed to reset active goal, cancel request failed.");
+      return rclcpp_action::CancelResponse::REJECT;
+    }
+
     // Mark the current goal as canceled
     auto result = std::make_shared<FollowJTrajAction::Result>();
     active_goal->setCanceled(result);
 
-    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;
@@ -601,21 +672,40 @@ void PassthroughTrajectoryController::goal_accepted_callback(
   current_index_ = 0;
 
   // TODO(fexner): Merge goal tolerances with default tolerances
-  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.");
+  auto map = create_joint_mapping(goal_handle->get_goal()->trajectory.joint_names);
+  if (map.empty()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to create joint trajectory mapping, aborting goal.");
+    auto result = std::make_shared<control_msgs::action::FollowJointTrajectory::Result>();
+    result->error_code =
+        control_msgs::action::FollowJointTrajectory::Result::INVALID_JOINTS;  // Not entirely sure, if this is the
+                                                                              // correct error code.
+    result->error_string = "Failed to create joint mapping.";
+    goal_handle->abort(result);
+    return;
+  }
+
+  if (!set_rt_box_from_non_rt(joint_trajectory_mapping_, map)) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to set joint trajectory mapping in realtime box.");
+    auto result = std::make_shared<control_msgs::action::FollowJointTrajectory::Result>();
+    result->error_code = control_msgs::action::FollowJointTrajectory::Result::INVALID_JOINTS;
+    result->error_string = "Failed to set joint trajectory mapping.";
+    goal_handle->abort(result);
     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_.try_get();
-    if (!joint_names_internal) {
-      RCLCPP_ERROR(get_node()->get_logger(), "Joint names could not be read, cannot set goal tolerances.");
+    auto joint_names_internal = get_rt_box_from_non_rt(joint_names_);
+    if (!joint_names_internal.has_value()) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Failed to get joint names, aborting goal.");
+      auto result = std::make_shared<control_msgs::action::FollowJointTrajectory::Result>();
+      result->error_code = control_msgs::action::FollowJointTrajectory::Result::INVALID_JOINTS;
+      result->error_string = "Failed to get joint names.";
+      goal_handle->abort(result);
       return;
     }
+
     std::stringstream ss;
     ss << "Using goal tolerances\n";
     for (auto& joint_name : *joint_names_internal) {
@@ -630,39 +720,52 @@ void PassthroughTrajectoryController::goal_accepted_callback(
     }
     RCLCPP_INFO_STREAM(get_node()->get_logger(), ss.str());
   }
-  write_success = goal_tolerance_.try_set(goal_tolerances);
-  if (!write_success) {
-    RCLCPP_ERROR(get_node()->get_logger(), "Failed to set goal tolerances.");
+
+  if (!set_rt_box_from_non_rt(goal_tolerance_, goal_tolerances)) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to set goal tolerances in realtime box.");
+    auto result = std::make_shared<control_msgs::action::FollowJointTrajectory::Result>();
+    result->error_code = control_msgs::action::FollowJointTrajectory::Result::INVALID_GOAL;
+    result->error_string = "Failed to set goal tolerances.";
+    goal_handle->abort(result);
     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.");
+  if (!set_rt_box_from_non_rt(goal_time_tolerance_, goal_handle->get_goal()->goal_time_tolerance)) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to set goal time tolerance in realtime box.");
+    auto result = std::make_shared<control_msgs::action::FollowJointTrajectory::Result>();
+    result->error_code = control_msgs::action::FollowJointTrajectory::Result::INVALID_GOAL;
+    result->error_string = "Failed to set goal time tolerance.";
+    goal_handle->abort(result);
     return;
   }
+
   RCLCPP_INFO_STREAM(get_node()->get_logger(),
                      "Goal time tolerance: " << duration_to_double(goal_handle->get_goal()->goal_time_tolerance)
                                              << " sec");
 
+  RealtimeGoalHandlePtr rt_goal = std::make_shared<RealtimeGoalHandle>(goal_handle);
+  if (!set_rt_goal_from_non_rt(rt_goal)) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to set active goal, aborting goal.");
+    auto result = std::make_shared<control_msgs::action::FollowJointTrajectory::Result>();
+    result->error_code = control_msgs::action::FollowJointTrajectory::Result::INVALID_GOAL;
+    result->error_string = "Failed to set active goal.";
+    goal_handle->abort(result);
+    return;
+  }
+
   // Action handling will be done from the timer callback to avoid those things in the realtime
   // thread. First, we delete the existing (if any) timer by resetting the pointer and then create a new
   // one.
   //
-  RealtimeGoalHandlePtr rt_goal = std::make_shared<RealtimeGoalHandle>(goal_handle);
   rt_goal->execute();
-  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));
   trajectory_active_ = true;
   return;
 }
 
+// Called from RT thread
 bool PassthroughTrajectoryController::check_goal_tolerance()
 {
   auto goal_tolerance = goal_tolerance_.try_get();
@@ -737,9 +840,10 @@ 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_.try_get();
-  if (!joint_names_internal) {
-    RCLCPP_ERROR(get_node()->get_logger(), "Could not get joint names, cannot create joint mapping.");
+  auto joint_names_internal = get_rt_box_from_non_rt(joint_names_);
+  if (!joint_names_internal.has_value()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Failed to get joint names from realtime box.");
+    return joint_mapping;
   }
 
   for (auto& joint_name : *joint_names_internal) {
@@ -750,6 +854,7 @@ PassthroughTrajectoryController::create_joint_mapping(const std::vector<std::str
   }
   return joint_mapping;
 }
+
 }  // namespace ur_controllers
 
 #include "pluginlib/class_list_macros.hpp"