From e546db76445e691de8571cb0c3e66f8e21197c78 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 14 Aug 2022 14:05:16 -0400 Subject: [PATCH 1/4] set empty path on black on failure --- .../plugins/action/compute_path_to_pose_action.hpp | 7 +++++++ .../plugins/action/compute_path_to_pose_action.cpp | 14 ++++++++++++++ 2 files changed, 21 insertions(+) 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 105b52fbf6..d08c7f04b8 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,13 @@ class ComputePathToPoseAction : public BtActionNode Date: Sun, 14 Aug 2022 14:13:35 -0400 Subject: [PATCH 2/4] docs --- .../plugins/action/compute_path_to_pose_action.hpp | 3 +++ 1 file changed, 3 insertions(+) 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 d08c7f04b8..f87435995c 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 @@ -56,6 +56,9 @@ class ComputePathToPoseAction : public BtActionNode Date: Sun, 21 Aug 2022 16:08:33 -0400 Subject: [PATCH 3/4] switched to correct message type --- .../plugins/action/compute_path_to_pose_action.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 c18b90419b..e6d315e43a 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 @@ -45,14 +45,14 @@ BT::NodeStatus ComputePathToPoseAction::on_success() BT::NodeStatus ComputePathToPoseAction::on_aborted() { - geometry_msgs::msg::PoseStamped empty_path; + nav_msgs::msg::Path empty_path; setOutput("path", empty_path); return BT::NodeStatus::FAILURE; } BT::NodeStatus ComputePathToPoseAction::on_cancelled() { - geometry_msgs::msg::PoseStamped empty_path; + nav_msgs::msg::Path empty_path; setOutput("path", empty_path); return BT::NodeStatus::SUCCESS; } From a11106ac4813d0f6c99ee3039a2b6b0ea7381a6c Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 21 Aug 2022 16:17:22 -0400 Subject: [PATCH 4/4] set empty path for compute_path_through_poses --- .../action/compute_path_through_poses_action.hpp | 10 ++++++++++ .../action/compute_path_through_poses_action.cpp | 12 ++++++++++++ 2 files changed, 22 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 f9f233c233..f7610696f8 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/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index f699b9fc0b..7dc5a6f2ba 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"