From bd15f2f60b8c3d077219fb0033ce9774b6ef58bd Mon Sep 17 00:00:00 2001
From: vicmassy <victor@unmanned.life>
Date: Wed, 10 Apr 2024 14:49:51 +0200
Subject: [PATCH] Fix cancel goal with emopty goal handle

---
 .../include/behaviortree_ros2/bt_action_node.hpp            | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp
index c6b0c97..e125afc 100644
--- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp
+++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp
@@ -433,6 +433,12 @@ template<class T> inline
 template<class T> inline
   void RosActionNode<T>::cancelGoal()
 {
+  if (!goal_handle_)
+  {
+    RCLCPP_WARN( node_->get_logger(), "cancelGoal called on an empty goal_handle");
+    return;
+  }
+
   auto future_result = action_client_->async_get_result(goal_handle_);
   auto future_cancel = action_client_->async_cancel_goal(goal_handle_);