File tree Expand file tree Collapse file tree 1 file changed +22
-3
lines changed
behaviortree_ros2/include/behaviortree_ros2 Expand file tree Collapse file tree 1 file changed +22
-3
lines changed Original file line number Diff line number Diff line change @@ -522,13 +522,32 @@ inline void RosActionNode<T>::halt()
522
522
template <class T >
523
523
inline void RosActionNode<T>::cancelGoal()
524
524
{
525
+ auto & executor = client_instance_->callback_executor ;
525
526
if (!goal_handle_)
526
527
{
527
- RCLCPP_WARN (logger (), " cancelGoal called on an empty goal_handle" );
528
- return ;
528
+ if (future_goal_handle_.valid ())
529
+ {
530
+ // Here the discussion is if we should block or put a timer for the waiting
531
+ auto ret =
532
+ executor.spin_until_future_complete (future_goal_handle_, server_timeout_);
533
+ if (ret != rclcpp::FutureReturnCode::SUCCESS)
534
+ {
535
+ // In that case the goal was not accepted or timed out so probably we should do nothing.
536
+ return ;
537
+ }
538
+ else
539
+ {
540
+ goal_handle_ = future_goal_handle_.get ();
541
+ future_goal_handle_ = {};
542
+ }
543
+ }
544
+ else
545
+ {
546
+ RCLCPP_WARN (logger (), " cancelGoal called on an empty goal_handle" );
547
+ return ;
548
+ }
529
549
}
530
550
531
- auto & executor = client_instance_->callback_executor ;
532
551
auto & action_client = client_instance_->action_client ;
533
552
534
553
auto future_result = action_client->async_get_result (goal_handle_);
You can’t perform that action at this time.
0 commit comments