Skip to content

JTC: Use std::atomic<bool> (backport #1720) #1722

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Jun 5, 2025
+20 −22
Merged

JTC: Use std::atomic<bool> (backport #1720) #1722

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#ifndef JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_
#define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_

#include <atomic>
#include <chrono>
#include <functional> // for std::reference_wrapper
#include <memory>
Expand Down Expand Up @@ -160,7 +161,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
// Timeout to consider commands old
double cmd_timeout_;
// True if holding position or repeating last trajectory point in case of success
realtime_tools::RealtimeBuffer<bool> rt_is_holding_;
std::atomic<bool> rt_is_holding_;
// TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported
bool subscriber_is_active_ = false;
rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr joint_command_subscriber_ =
Expand Down Expand Up @@ -192,7 +193,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa

rclcpp_action::Server<FollowJTrajAction>::SharedPtr action_server_;
RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any.
realtime_tools::RealtimeBuffer<bool> rt_has_pending_goal_; ///< Is there a pending action goal?
std::atomic<bool> rt_has_pending_goal_; ///< Is there a pending action goal?
rclcpp::TimerBase::SharedPtr goal_handle_timer_;
rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms);

Expand Down
37 changes: 17 additions & 20 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -171,9 +171,7 @@ controller_interface::return_type JointTrajectoryController::update(
auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg();
auto new_external_msg = traj_msg_external_point_ptr_.readFromRT();
// Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_)
if (
current_external_msg != *new_external_msg &&
(*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false)
if (current_external_msg != *new_external_msg && (rt_has_pending_goal_ && !active_goal) == false)
{
fill_partial_goal(*new_external_msg);
sort_to_local_joint_order(*new_external_msg);
Expand Down Expand Up @@ -242,7 +240,7 @@ controller_interface::return_type JointTrajectoryController::update(
// have we reached the end, are not holding position, and is a timeout configured?
// Check independently of other tolerances
if (
!before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 &&
!before_last_point && !rt_is_holding_ && cmd_timeout_ > 0.0 &&
time_difference > cmd_timeout_)
{
RCLCPP_WARN(logger, "Aborted due to command timeout");
Expand All @@ -260,15 +258,15 @@ controller_interface::return_type JointTrajectoryController::update(
// is the last point
// print output per default, goal will be aborted afterwards
if (
(before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false &&
(before_last_point || first_sample) && !rt_is_holding_ &&
!check_state_tolerance_per_joint(
state_error_, index, active_tol->state_tolerance[index], true /* show_errors */))
{
tolerance_violated_while_moving = true;
}
// past the final point, check that we end up inside goal tolerance
if (
!before_last_point && *(rt_is_holding_.readFromRT()) == false &&
!before_last_point && !rt_is_holding_ &&
!check_state_tolerance_per_joint(
state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */))
{
Expand Down Expand Up @@ -355,7 +353,7 @@ controller_interface::return_type JointTrajectoryController::update(
// 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_has_pending_goal_.writeFromNonRT(false);
rt_has_pending_goal_ = false;

RCLCPP_WARN(logger, "Aborted due to state tolerance violation");

Expand All @@ -374,7 +372,7 @@ controller_interface::return_type JointTrajectoryController::update(
// 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_has_pending_goal_.writeFromNonRT(false);
rt_has_pending_goal_ = false;

RCLCPP_INFO(logger, "Goal reached, success!");

Expand All @@ -393,7 +391,7 @@ controller_interface::return_type JointTrajectoryController::update(
// 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_has_pending_goal_.writeFromNonRT(false);
rt_has_pending_goal_ = false;

RCLCPP_WARN(logger, "%s", error_string.c_str());

Expand All @@ -402,16 +400,15 @@ controller_interface::return_type JointTrajectoryController::update(
}
}
}
else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false)
else if (tolerance_violated_while_moving && !rt_has_pending_goal_)
{
// we need to ensure that there is no pending goal -> we get a race condition otherwise
RCLCPP_ERROR(logger, "Holding position due to state tolerance violation");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
}
else if (
!before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false)
else if (!before_last_point && !within_goal_time && !rt_has_pending_goal_)
{
RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position...");

Expand Down Expand Up @@ -1014,7 +1011,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(

// The controller should start by holding position at the beginning of active state
add_new_trajectory_msg(set_hold_position());
rt_is_holding_.writeFromNonRT(true);
rt_is_holding_ = true;

// parse timeout parameter
if (params_.cmd_timeout > 0.0)
Expand Down Expand Up @@ -1046,7 +1043,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
const auto active_goal = *rt_active_goal_.readFromNonRT();
if (active_goal)
{
rt_has_pending_goal_.writeFromNonRT(false);
rt_has_pending_goal_ = false;
auto action_res = std::make_shared<FollowJTrajAction::Result>();
action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL);
action_res->set__error_string("Current goal cancelled during deactivate transition.");
Expand Down Expand Up @@ -1217,7 +1214,7 @@ void JointTrajectoryController::topic_callback(
if (subscriber_is_active_)
{
add_new_trajectory_msg(msg);
rt_is_holding_.writeFromNonRT(false);
rt_is_holding_ = false;
}
};

Expand Down Expand Up @@ -1256,7 +1253,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback
get_node()->get_logger(), "Canceling active action goal because cancel callback received.");

// Mark the current goal as canceled
rt_has_pending_goal_.writeFromNonRT(false);
rt_has_pending_goal_ = false;
auto action_res = std::make_shared<FollowJTrajAction::Result>();
active_goal->setCanceled(action_res);
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
Expand All @@ -1271,7 +1268,7 @@ void JointTrajectoryController::goal_accepted_callback(
std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle)
{
// mark a pending goal
rt_has_pending_goal_.writeFromNonRT(true);
rt_has_pending_goal_ = true;

// Update new trajectory
{
Expand All @@ -1280,7 +1277,7 @@ void JointTrajectoryController::goal_accepted_callback(
std::make_shared<trajectory_msgs::msg::JointTrajectory>(goal_handle->get_goal()->trajectory);

add_new_trajectory_msg(traj_msg);
rt_is_holding_.writeFromNonRT(false);
rt_is_holding_ = false;
}

// Update the active goal
Expand Down Expand Up @@ -1593,7 +1590,7 @@ JointTrajectoryController::set_hold_position()
hold_position_msg_ptr_->points[0].positions = state_current_.positions;

// set flag, otherwise tolerances will be checked with holding position too
rt_is_holding_.writeFromNonRT(true);
rt_is_holding_ = true;

return hold_position_msg_ptr_;
}
Expand All @@ -1607,7 +1604,7 @@ JointTrajectoryController::set_success_trajectory_point()
hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0);

// set flag, otherwise tolerances will be checked with success_trajectory_point too
rt_is_holding_.writeFromNonRT(true);
rt_is_holding_ = true;

return hold_position_msg_ptr_;
}
Expand Down
Loading