diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 7bea9552a8..8265dd91f8 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -41,6 +41,7 @@ #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "realtime_tools/realtime_server_goal_handle.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" @@ -171,7 +172,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Service::SharedPtr query_state_srv_; - std::shared_ptr current_trajectory_ = nullptr; + std::unique_ptr current_trajectory_ = nullptr; realtime_tools::RealtimeBuffer> new_trajectory_msg_; @@ -186,12 +187,18 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; - using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; rclcpp_action::Server::SharedPtr action_server_; - RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. - std::atomic rt_has_pending_goal_{false}; ///< Is there a pending action goal? + // Currently active action goal, if any. Needs to be a shared_ptr for processing the goal inside + // the goal_handle_timer_ in the non-rt loop + realtime_tools::RealtimeThreadSafeBox rt_active_goal_; + // local copy for the RT loop + RealtimeGoalHandlePtr rt_active_goal_local_{nullptr}; + // Is there a pending action goal? + std::atomic rt_has_pending_goal_{false}; + // Timer for processing the goal in the non-rt loop rclcpp::TimerBase::SharedPtr goal_handle_timer_; + // Timer period for goal_handle_timer_ rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); // callback for topic interface @@ -235,7 +242,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // the tolerances from the node parameter SegmentTolerances default_tolerances_; // the tolerances used for the current goal - realtime_tools::RealtimeBuffer active_tolerances_; + realtime_tools::RealtimeThreadSafeBox goal_tolerances_; + // preallocated memory for tolerances used in RT loop + SegmentTolerances active_tol_; void preempt_active_goal(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 3d1f2c1616..3e20355966 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -176,14 +176,15 @@ controller_interface::return_type JointTrajectoryController::update( } // don't update goal after we sampled the trajectory to avoid any racecondition - const auto active_goal = *rt_active_goal_.readFromRT(); + rt_active_goal_.try_get([&](const auto goal) { rt_active_goal_local_ = goal; }); // Check if a new trajectory message has been received from Non-RT threads const auto current_trajectory_msg = current_trajectory_->get_trajectory_msg(); auto new_external_msg = new_trajectory_msg_.readFromRT(); // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) if ( - current_trajectory_msg != *new_external_msg && (rt_has_pending_goal_ && !active_goal) == false) + current_trajectory_msg != *new_external_msg && + (rt_has_pending_goal_ && !rt_active_goal_local_) == false) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); @@ -253,7 +254,16 @@ controller_interface::return_type JointTrajectoryController::update( bool outside_goal_tolerance = false; bool within_goal_time = true; const bool before_last_point = end_segment_itr != current_trajectory_->end(); - auto active_tol = active_tolerances_.readFromRT(); + auto active_tol_op = goal_tolerances_.try_get(); + if (active_tol_op.has_value()) + { + active_tol_ = active_tol_op.value(); + } + else + { + // fallback if try_get fails + active_tol_ = default_tolerances_; + } // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances @@ -278,7 +288,7 @@ controller_interface::return_type JointTrajectoryController::update( if ( (before_last_point || first_sample) && !rt_is_holding_ && !check_state_tolerance_per_joint( - state_error_, index, active_tol->state_tolerance[index], true /* show_errors */)) + state_error_, index, active_tol_.state_tolerance[index], true /* show_errors */)) { tolerance_violated_while_moving = true; } @@ -286,14 +296,14 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && !rt_is_holding_ && !check_state_tolerance_per_joint( - state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */)) + state_error_, index, active_tol_.goal_state_tolerance[index], false /* show_errors */)) { outside_goal_tolerance = true; - if (active_tol->goal_time_tolerance != 0.0) + if (active_tol_.goal_time_tolerance != 0.0) { // if we exceed goal_time_tolerance set it to aborted - if (time_difference > active_tol->goal_time_tolerance) + if (time_difference > active_tol_.goal_time_tolerance) { within_goal_time = false; // print once, goal will be aborted afterwards @@ -363,7 +373,7 @@ controller_interface::return_type JointTrajectoryController::update( last_commanded_time_ = time; } - if (active_goal) + if (rt_active_goal_local_) { // send feedback auto feedback = std::make_shared(); @@ -373,7 +383,7 @@ controller_interface::return_type JointTrajectoryController::update( feedback->actual = state_current_; feedback->desired = state_desired_; feedback->error = state_error_; - active_goal->setFeedback(feedback); + rt_active_goal_local_->setFeedback(feedback); // check abort if (tolerance_violated_while_moving) @@ -381,10 +391,8 @@ controller_interface::return_type JointTrajectoryController::update( auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); result->set__error_string("Aborted due to path tolerance violation"); - active_goal->setAborted(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_local_->setAborted(result); + rt_active_goal_.try_set([](auto goal) { goal = RealtimeGoalHandlePtr(); }); rt_has_pending_goal_ = false; RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); @@ -400,10 +408,8 @@ controller_interface::return_type JointTrajectoryController::update( auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); result->set__error_string("Goal successfully reached!"); - active_goal->setSucceeded(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_local_->setSucceeded(result); + rt_active_goal_.try_set([](auto goal) { goal = RealtimeGoalHandlePtr(); }); rt_has_pending_goal_ = false; RCLCPP_INFO(logger, "Goal reached, success!"); @@ -419,10 +425,8 @@ controller_interface::return_type JointTrajectoryController::update( auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); result->set__error_string(error_string); - active_goal->setAborted(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_local_->setAborted(result); + rt_active_goal_.try_set([](auto goal) { goal = RealtimeGoalHandlePtr(); }); rt_has_pending_goal_ = false; RCLCPP_WARN(logger, "%s", error_string.c_str()); @@ -725,7 +729,7 @@ void JointTrajectoryController::query_state_service( response->success = false; return; } - const auto active_goal = *rt_active_goal_.readFromRT(); + response->name = params_.joints; trajectory_msgs::msg::JointTrajectoryPoint state_requested = state_current_; if (has_active_trajectory()) @@ -925,7 +929,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // parse remaining parameters default_tolerances_ = get_segment_tolerances(logger, params_); - active_tolerances_.initRT(default_tolerances_); + goal_tolerances_.set(default_tolerances_); const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -1140,7 +1144,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } } - current_trajectory_ = std::make_shared(); + current_trajectory_ = std::make_unique(); new_trajectory_msg_.writeFromNonRT(std::shared_ptr()); subscriber_is_active_ = true; @@ -1200,8 +1204,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( const rclcpp_lifecycle::State &) { - const auto active_goal = *rt_active_goal_.readFromNonRT(); auto logger = get_node()->get_logger(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const auto goal) { active_goal = goal; }); if (active_goal) { rt_has_pending_goal_ = false; @@ -1209,7 +1214,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled during deactivate transition."); active_goal->setAborted(action_res); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.try_set([](auto goal) { goal = RealtimeGoalHandlePtr(); }); } for (size_t index = 0; index < num_cmd_joints_; ++index) @@ -1370,7 +1375,8 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const auto goal) { active_goal = goal; }); if (active_goal && active_goal->gh_ == goal_handle) { RCLCPP_INFO( @@ -1380,7 +1386,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback rt_has_pending_goal_ = false; auto action_res = std::make_shared(); active_goal->setCanceled(action_res); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.try_set([](auto goal) { goal = RealtimeGoalHandlePtr(); }); // Enter hold current position mode add_new_trajectory_msg(set_hold_position()); @@ -1408,11 +1414,11 @@ void JointTrajectoryController::goal_accepted_callback( RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); rt_goal->preallocated_feedback_->joint_names = params_.joints; rt_goal->execute(); - rt_active_goal_.writeFromNonRT(rt_goal); + rt_active_goal_.set([&](auto & goal) { goal = rt_goal; }); // Update tolerances if specified in the goal auto logger = this->get_node()->get_logger(); - active_tolerances_.writeFromNonRT(get_segment_tolerances( + goal_tolerances_.set(get_segment_tolerances( logger, default_tolerances_, *(goal_handle->get_goal()), params_.joints)); // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list @@ -1756,14 +1762,15 @@ void JointTrajectoryController::add_new_trajectory_msg( void JointTrajectoryController::preempt_active_goal() { - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const auto goal) { active_goal = goal; }); if (active_goal) { auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled due to new incoming action."); active_goal->setCanceled(action_res); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](auto & goal) { goal = RealtimeGoalHandlePtr(); }); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 0fe3ed69f9..a71e831be6 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -170,7 +170,7 @@ class TestableJointTrajectoryController joint_trajectory_controller::SegmentTolerances get_active_tolerances() { - return *(active_tolerances_.readFromRT()); + return goal_tolerances_.get(); } std::vector get_pids() const { return pids_; }