From dc568b1e35f16b5302bb92b058bec8c6edbe4c51 Mon Sep 17 00:00:00 2001
From: Joshua Wallace <47819219+jwallace42@users.noreply.github.com>
Date: Mon, 22 Aug 2022 12:26:56 -0400
Subject: [PATCH] ComputePathToPose Sets empty path on blackboard if action is
 aborted or cancelled (#3114)

* set empty path on black on failure

* docs

* switched to correct message type

* set empty path for compute_path_through_poses
---
 .../action/compute_path_through_poses_action.hpp   | 10 ++++++++++
 .../plugins/action/compute_path_to_pose_action.hpp | 10 ++++++++++
 .../action/compute_path_through_poses_action.cpp   | 12 ++++++++++++
 .../plugins/action/compute_path_to_pose_action.cpp | 14 ++++++++++++++
 4 files changed, 46 insertions(+)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp
index f9f233c2337..f7610696f85 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp
@@ -54,6 +54,16 @@ class ComputePathThroughPosesAction
    */
   BT::NodeStatus on_success() override;
 
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancelation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp
index 105b52fbf6c..f87435995c5 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp
@@ -51,6 +51,16 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
    */
   BT::NodeStatus on_success() override;
 
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancelation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp
index f699b9fc0bc..7dc5a6f2ba6 100644
--- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp
+++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp
@@ -44,6 +44,18 @@ BT::NodeStatus ComputePathThroughPosesAction::on_success()
   return BT::NodeStatus::SUCCESS;
 }
 
+BT::NodeStatus ComputePathThroughPosesAction::on_aborted() {
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() {
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"
diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
index 83b29dfe3a3..e6d315e43ad 100644
--- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
+++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
@@ -43,6 +43,20 @@ BT::NodeStatus ComputePathToPoseAction::on_success()
   return BT::NodeStatus::SUCCESS;
 }
 
+BT::NodeStatus ComputePathToPoseAction::on_aborted()
+{
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus ComputePathToPoseAction::on_cancelled()
+{
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"