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 de94e697e6e..2de0593b6f0 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 @@ -232,6 +232,9 @@ class BtActionNode : public BT::ActionNodeBase } setStatus(BT::NodeStatus::IDLE); + if (goal_handle_) { + goal_handle_.reset(); + } } protected: @@ -242,7 +245,7 @@ class BtActionNode : public BT::ActionNodeBase bool should_cancel_goal() { // Shut the node down if it is currently running - if (status() != BT::NodeStatus::RUNNING) { + if (status() != BT::NodeStatus::RUNNING || !goal_handle_) { return false; } @@ -266,7 +269,7 @@ class BtActionNode : public BT::ActionNodeBase // 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) { + if (!this->goal_handle_ || (this->goal_handle_->get_goal_id() == result.goal_id)) { goal_result_available_ = true; result_ = result; }