Skip to content

Commit e7b2926

Browse files
committed
fix issue #57
1 parent e81f588 commit e7b2926

File tree

1 file changed

+22
-3
lines changed

1 file changed

+22
-3
lines changed

behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp

Lines changed: 22 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -522,13 +522,32 @@ inline void RosActionNode<T>::halt()
522522
template <class T>
523523
inline void RosActionNode<T>::cancelGoal()
524524
{
525+
auto& executor = client_instance_->callback_executor;
525526
if(!goal_handle_)
526527
{
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+
}
529549
}
530550

531-
auto& executor = client_instance_->callback_executor;
532551
auto& action_client = client_instance_->action_client;
533552

534553
auto future_result = action_client->async_get_result(goal_handle_);

0 commit comments

Comments
 (0)