From df6c15fd08036c6d0512df631a83b445dca07c43 Mon Sep 17 00:00:00 2001 From: URJala <159417921+URJala@users.noreply.github.com> Date: Tue, 16 Sep 2025 10:38:07 +0200 Subject: [PATCH 1/2] 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. (cherry picked from commit 76b3e3767f7a9c2cf25378dc3c52448bace1c494) # Conflicts: # ur_controllers/include/ur_controllers/force_mode_controller.hpp --- .../ur_controllers/force_mode_controller.hpp | 9 +- .../passthrough_trajectory_controller.hpp | 14 +- .../tool_contact_controller.hpp | 7 +- .../ur_configuration_controller.hpp | 4 +- ur_controllers/src/force_mode_controller.cpp | 19 +- .../src/passthrough_trajectory_controller.cpp | 238 ++++++++++++++++-- .../src/tool_contact_controller.cpp | 87 ++++++- .../src/ur_configuration_controller.cpp | 13 +- 8 files changed, 340 insertions(+), 51 deletions(-) diff --git a/ur_controllers/include/ur_controllers/force_mode_controller.hpp b/ur_controllers/include/ur_controllers/force_mode_controller.hpp index 5ce586f56..e7a062558 100644 --- a/ur_controllers/include/ur_controllers/force_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/force_mode_controller.hpp @@ -42,8 +42,13 @@ #include #include +<<<<<<< HEAD #include #include +======= +#include +#include +>>>>>>> 76b3e37 (Realtime tools migration (#1474)) #include #include @@ -93,7 +98,7 @@ struct ForceModeParameters std::array task_frame; std::array selection_vec; std::array limits; - geometry_msgs::msg::Wrench wrench; + geometry_msgs::msg::Wrench wrench{}; double type; double damping_factor; double gain_scaling; @@ -132,7 +137,7 @@ class ForceModeController : public controller_interface::ControllerInterface std::shared_ptr param_listener_; force_mode_controller::Params params_; - realtime_tools::RealtimeBuffer force_mode_params_buffer_; + realtime_tools::RealtimeThreadSafeBox force_mode_params_buffer_; std::atomic force_mode_active_; std::atomic change_requested_; std::atomic async_state_; diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 8e34503a1..568aab166 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -51,7 +51,7 @@ #include #include -#include +#include #include #include #include @@ -114,11 +114,11 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; - using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeThreadSafeBox; 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> joint_trajectory_mapping_; + realtime_tools::RealtimeThreadSafeBox> 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> goal_handle); - realtime_tools::RealtimeBuffer> joint_names_; + realtime_tools::RealtimeThreadSafeBox> joint_names_; std::vector state_interface_types_; std::vector joint_state_interface_names_; @@ -159,11 +159,13 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI bool check_goal_positions(std::shared_ptr goal); bool check_goal_velocities(std::shared_ptr goal); bool check_goal_accelerations(std::shared_ptr goal); + std::optional 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 path_tolerance_; - realtime_tools::RealtimeBuffer> goal_tolerance_; - realtime_tools::RealtimeBuffer goal_time_tolerance_{ rclcpp::Duration(0, 0) }; + realtime_tools::RealtimeThreadSafeBox> goal_tolerance_; + realtime_tools::RealtimeThreadSafeBox goal_time_tolerance_{ rclcpp::Duration(0, 0) }; std::atomic current_index_; std::atomic trajectory_active_; diff --git a/ur_controllers/include/ur_controllers/tool_contact_controller.hpp b/ur_controllers/include/ur_controllers/tool_contact_controller.hpp index 809628e13..c7d96c0f4 100644 --- a/ur_controllers/include/ur_controllers/tool_contact_controller.hpp +++ b/ur_controllers/include/ur_controllers/tool_contact_controller.hpp @@ -53,8 +53,9 @@ #include #include -#include +#include #include +#include #include #include "ur_controllers/tool_contact_controller_parameters.hpp" @@ -86,11 +87,13 @@ class ToolContactController : public controller_interface::ControllerInterface private: using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; - using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeThreadSafeBox; 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 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); diff --git a/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp index 389933792..3151274c2 100644 --- a/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp +++ b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp @@ -44,7 +44,7 @@ #include #include -#include +#include #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> robot_software_version_{ + realtime_tools::RealtimeThreadSafeBox> robot_software_version_{ std::make_shared() }; std::atomic robot_software_version_set_{ false }; diff --git a/ur_controllers/src/force_mode_controller.cpp b/ur_controllers/src/force_mode_controller.cpp index 64747c7d7..56930d538 100644 --- a/ur_controllers/src/force_mode_controller.cpp +++ b/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; diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 7e7aa0d4d..d5aa757ba 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -54,6 +54,74 @@ namespace ur_controllers { +template +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 +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::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,7 +133,10 @@ 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); + 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; @@ -73,6 +144,7 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() return controller_interface::CallbackReturn::SUCCESS; } +// Not part of real time loop controller_interface::CallbackReturn PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& previous_state) { @@ -83,7 +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_.readFromRT(); + 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); @@ -228,12 +304,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 result = std::make_shared(); 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 +328,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 +364,18 @@ controller_interface::return_type PassthroughTrajectoryController::update(const write_success &= trajectory_size_command_interface_->get().set_value(static_cast(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 +385,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 +529,12 @@ bool PassthroughTrajectoryController::check_goal_tolerances( std::shared_ptr goal) { auto& tolerances = goal->goal_tolerance; - auto joint_names_internal = joint_names_.readFromRT(); + 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; + } + if (!tolerances.empty()) { for (auto& tol : tolerances) { auto found_it = std::find(joint_names_internal->begin(), joint_names_internal->end(), tol.name); @@ -526,14 +638,26 @@ rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_ca const std::shared_ptr> goal_handle) { // Check that cancel request refers to currently active goal (if any) - const auto active_goal = *rt_active_goal_.readFromNonRT(); + 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(); active_goal->setCanceled(result); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + trajectory_active_ = false; } return rclcpp_action::CancelResponse::ACCEPT; @@ -548,13 +672,40 @@ void PassthroughTrajectoryController::goal_accepted_callback( current_index_ = 0; // TODO(fexner): Merge goal tolerances with default tolerances + 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(); + 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; + } - joint_trajectory_mapping_.writeFromNonRT(create_joint_mapping(goal_handle->get_goal()->trajectory.joint_names)); + 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(); + 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 goal_tolerances; if (!goal_handle->get_goal()->goal_tolerance.empty()) { - auto joint_names_internal = joint_names_.readFromRT(); + 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(); + 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) { @@ -569,19 +720,44 @@ 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); + + 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(); + result->error_code = control_msgs::action::FollowJointTrajectory::Result::INVALID_GOAL; + result->error_string = "Failed to set goal tolerances."; + goal_handle->abort(result); + return; + } + + 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(); + 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(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(); + 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(goal_handle); rt_goal->execute(); - rt_active_goal_.writeFromNonRT(rt_goal); goal_handle_timer_.reset(); goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_.to_chrono(), std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); @@ -589,11 +765,25 @@ void PassthroughTrajectoryController::goal_accepted_callback( return; } +// Called from RT thread 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 +840,12 @@ std::unordered_map PassthroughTrajectoryController::create_joint_mapping(const std::vector& joint_names) const { std::unordered_map joint_mapping; - auto joint_names_internal = joint_names_.readFromNonRT(); + 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) { auto found_it = std::find(joint_names.begin(), joint_names.end(), joint_name); if (found_it != joint_names.end()) { @@ -659,6 +854,7 @@ PassthroughTrajectoryController::create_joint_mapping(const std::vectorget_logger(), "Failed to read active goal, deactivation of controller failed."); + return controller_interface::CallbackReturn::ERROR; + } + if (active_goal) { RCLCPP_INFO(get_node()->get_logger(), "Aborting tool contact, as controller has been deactivated."); // Mark the current goal as abort @@ -210,8 +215,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_goal_from_non_rt(); + + if (!active_goal.has_value()) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to read if active goal is present, 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; } @@ -224,9 +235,15 @@ void ToolContactController::goal_accepted_callback( RCLCPP_INFO(get_node()->get_logger(), "Goal accepted."); tool_contact_enable_ = true; tool_contact_abort_ = false; + RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); + if (!set_rt_goal_from_non_rt(rt_goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to set active goal in realtime box, aborting goal."); + goal_handle->abort(std::make_shared()); + return; + } + rt_goal->execute(); - rt_active_goal_.writeFromNonRT(rt_goal); goal_handle_timer_.reset(); auto period = std::chrono::duration_cast( std::chrono::nanoseconds(action_monitor_period_.nanoseconds())); @@ -236,14 +253,19 @@ 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_goal_from_non_rt(); + 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()); - should_reset_goal = false; + if (set_rt_goal_from_non_rt(RealtimeGoalHandlePtr())) { + should_reset_goal = false; + } } } } @@ -252,13 +274,18 @@ rclcpp_action::CancelResponse ToolContactController::goal_canceled_callback( const std::shared_ptr> 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_goal_from_non_rt(); + 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(); - active_goal->setCanceled(result); + active_goal.value()->setCanceled(result); should_reset_goal = true; tool_contact_abort_ = true; tool_contact_enable_ = false; @@ -282,7 +309,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(); + RealtimeGoalHandlePtr active_goal; + if (!rt_active_goal_.try_get([&active_goal](const RealtimeGoalHandlePtr& goal) { active_goal = goal; })) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to read active goal, skipping update cycle."); + return controller_interface::return_type::OK; + } + std::optional state_optional = tool_contact_state_interface_->get().get_optional(); if (!state_optional) { @@ -379,6 +411,35 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti return controller_interface::return_type::OK; } +std::optional ToolContactController::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 read active goal."); + return std::nullopt; + } + rclcpp::sleep_for(std::chrono::milliseconds(50)); + tries++; + } + return active_goal; +} + +bool ToolContactController::set_rt_goal_from_non_rt(const RealtimeGoalHandlePtr& goal_handle) +{ + int tries = 0; + while (!rt_active_goal_.try_set([&goal_handle](RealtimeGoalHandlePtr& goal) { goal = goal_handle; })) { + if (tries > 9) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to set active goal."); + return false; + } + rclcpp::sleep_for(std::chrono::milliseconds(50)); + tries++; + } + return true; +} + } // namespace ur_controllers #include "pluginlib/class_list_macros.hpp" diff --git a/ur_controllers/src/ur_configuration_controller.cpp b/ur_controllers/src/ur_configuration_controller.cpp index 6320fcb79..6fb29b8ea 100644 --- a/ur_controllers/src/ur_configuration_controller.cpp +++ b/ur_controllers/src/ur_configuration_controller.cpp @@ -39,7 +39,6 @@ //---------------------------------------------------------------------- #include -#include namespace ur_controllers { @@ -120,12 +119,20 @@ bool URConfigurationController::getRobotSoftwareVersion( RCLCPP_WARN(get_node()->get_logger(), "Robot software version not set yet."); return false; } - return robot_software_version_.try_get([resp](const std::shared_ptr ptr) { + int tries = 0; + while (!robot_software_version_.try_get([resp](const std::shared_ptr ptr) { resp->major = ptr->major; resp->minor = ptr->minor; resp->build = ptr->build; resp->bugfix = ptr->bugfix; - }); + })) { + if (tries > 10) { + return false; + } + rclcpp::sleep_for(std::chrono::milliseconds(50)); + tries++; + } + return true; } } // namespace ur_controllers From 0194384ea382f76427f09512b9a467be0d7b55ad Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 16 Sep 2025 10:44:21 +0200 Subject: [PATCH 2/2] Resolve conflict --- .../include/ur_controllers/force_mode_controller.hpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/ur_controllers/include/ur_controllers/force_mode_controller.hpp b/ur_controllers/include/ur_controllers/force_mode_controller.hpp index e7a062558..c13e8c2bc 100644 --- a/ur_controllers/include/ur_controllers/force_mode_controller.hpp +++ b/ur_controllers/include/ur_controllers/force_mode_controller.hpp @@ -42,13 +42,8 @@ #include #include -<<<<<<< HEAD -#include -#include -======= #include #include ->>>>>>> 76b3e37 (Realtime tools migration (#1474)) #include #include