Skip to content

Implement realtime-safe stall detection for parallel gripper controller #1855

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

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Implement realtime-safe stall detection for parallel gripper controller #1855

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3521f5a
Implement realtime-safe stall detection for parallel gripper controller
Aug 7, 2025
8292c80
Use try_set() for RealtimeThreadSafeBox in on_activate()
Aug 7, 2025
c373c15
Make goal state operations realtime-safe in check_for_success
Aug 7, 2025
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
4 changes: 2 additions & 2 deletions ...per_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,8 +147,8 @@ class GripperActionController : public controller_interface::ControllerInterface

void set_hold_position();

rclcpp::Time last_movement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); ///< Store stall time
double computed_command_; ///< Computed command
realtime_tools::RealtimeThreadSafeBox<rclcpp::Time> last_movement_time_; ///< Store stall time
double computed_command_; ///< Computed command

/**
* \brief Check for success and publish appropriate result and feedback.
Expand Down
51 changes: 30 additions & 21 deletions ...ontroller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,7 @@ void GripperActionController::accepted_callback(
pre_alloc_result_->reached_goal = false;
pre_alloc_result_->stalled = false;

last_movement_time_ = get_node()->now();
last_movement_time_.set(get_node()->now());
rt_goal->execute();
rt_active_goal_.set([rt_goal](RealtimeGoalHandlePtr & stored_value) { stored_value = rt_goal; });

Expand Down Expand Up @@ -216,7 +216,7 @@ void GripperActionController::check_for_success(
double current_velocity)
{
RealtimeGoalHandlePtr active_goal;
rt_active_goal_.get([&](const RealtimeGoalHandlePtr & goal) { active_goal = goal; });
rt_active_goal_.try_get([&](const RealtimeGoalHandlePtr & goal) { active_goal = goal; });
if (!active_goal)
{
return;
Expand All @@ -230,34 +230,41 @@ void GripperActionController::check_for_success(
pre_alloc_result_->stalled = false;
RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal.");
active_goal->setSucceeded(pre_alloc_result_);
rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value)
{ stored_value = RealtimeGoalHandlePtr(); });
rt_active_goal_.try_set([](RealtimeGoalHandlePtr & stored_value)
{ stored_value = RealtimeGoalHandlePtr(); });
}
else
{
if (fabs(current_velocity) > params_.stall_velocity_threshold)
{
last_movement_time_ = time;
last_movement_time_.try_set(time);
}
else if ((time - last_movement_time_).seconds() > params_.stall_timeout)
else
{
pre_alloc_result_->state.effort[0] = computed_command_;
pre_alloc_result_->state.position[0] = current_position;
pre_alloc_result_->reached_goal = false;
pre_alloc_result_->stalled = true;

if (params_.allow_stalling)
auto last_time_opt = last_movement_time_.try_get();
if (
last_time_opt.has_value() &&
(time - last_time_opt.value()).seconds() > params_.stall_timeout)
{
RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success.");
active_goal->setSucceeded(pre_alloc_result_);
pre_alloc_result_->state.effort[0] = computed_command_;
pre_alloc_result_->state.position[0] = current_position;
pre_alloc_result_->reached_goal = false;
pre_alloc_result_->stalled = true;

if (params_.allow_stalling)
{
RCLCPP_DEBUG(
get_node()->get_logger(), "Stall detected moving to goal. Returning success.");
active_goal->setSucceeded(pre_alloc_result_);
}
else
{
RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!");
active_goal->setAborted(pre_alloc_result_);
}
rt_active_goal_.try_set([](RealtimeGoalHandlePtr & stored_value)
{ stored_value = RealtimeGoalHandlePtr(); });
}
else
{
RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!");
active_goal->setAborted(pre_alloc_result_);
}
rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value)
{ stored_value = RealtimeGoalHandlePtr(); });
}
}
}
Expand Down Expand Up @@ -378,6 +385,8 @@ controller_interface::CallbackReturn GripperActionController::on_activate(
pre_alloc_result_->reached_goal = false;
pre_alloc_result_->stalled = false;

last_movement_time_.try_set(rclcpp::Time(0, 0, RCL_ROS_TIME));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
last_movement_time_.try_set(rclcpp::Time(0, 0, RCL_ROS_TIME));
last_movement_time_.try_set(rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED));

Sorry, it's just this one. Initializing with ROS time is still a valid time, better set an invalid type

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@saikishor Of course! No worries. I'll commit this suggestion real quick.

May I ask what’s the main reason for this change? You don’t want the time to be valid in on_activate()? It doesn’t really matter here since in accepted_callback() we set it with last_movement_time_.set(get_node()->now());.


// Action interface
action_server_ = rclcpp_action::create_server<control_msgs::action::ParallelGripperCommand>(
get_node(), "~/gripper_cmd",
Expand Down