From dc597153236cd2eaa48b11e45f9eb3ae7c447af0 Mon Sep 17 00:00:00 2001 From: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Date: Thu, 18 Jun 2020 14:00:23 -0400 Subject: [PATCH] a workaround patch for #1652 (#1807) Signed-off-by: Daisuke Sato --- .../include/nav2_behavior_tree/bt_action_node.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 0f58ffe998..8a8aa9a746 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -214,7 +214,10 @@ class BtActionNode : public BT::ActionNodeBase auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); send_goal_options.result_callback = [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { - if (result.code != rclcpp_action::ResultCode::ABORTED) { + // TODO(#1652): a work around until rcl_action interface is updated + // if goal ids are not matched, the older goal call this callback so ignore the result + // if matched, it must be processed (including aborted) + if (this->goal_handle_->get_goal_id() == result.goal_id) { goal_result_available_ = true; result_ = result; }