From 432b76d01330fedeba535d2fb3d523b98b3ade53 Mon Sep 17 00:00:00 2001
From: Tony Najjar <tony.najjar@logivations.com>
Date: Mon, 16 May 2022 19:19:46 +0200
Subject: [PATCH] Added behaviour_tree as input to action nodes (#2952)

* added BT as input to action node

* fix format

* Update navigate_to_pose_action.cpp

* Update navigate_through_poses_action.cpp
---
 .../plugins/action/navigate_through_poses_action.hpp            | 1 +
 .../plugins/action/navigate_to_pose_action.hpp                  | 1 +
 nav2_behavior_tree/nav2_tree_nodes.xml                          | 2 ++
 .../plugins/action/navigate_through_poses_action.cpp            | 1 +
 nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp   | 1 +
 5 files changed, 6 insertions(+)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp
index 209a11b6ba..5234dedaec 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp
@@ -56,6 +56,7 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
     return providedBasicPorts(
       {
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goals", "Destinations to plan through"),
+        BT::InputPort<geometry_msgs::msg::PoseStamped>("behavior_tree", "Behavior tree to run"),
       });
   }
 };
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp
index aa5f7a1e82..e8c37af738 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp
@@ -56,6 +56,7 @@ class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPo
     return providedBasicPorts(
       {
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
+        BT::InputPort<geometry_msgs::msg::PoseStamped>("behavior_tree", "Behavior tree to run"),
       });
   }
 };
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
index 66e123beb4..3a22a90083 100644
--- a/nav2_behavior_tree/nav2_tree_nodes.xml
+++ b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -101,12 +101,14 @@
         <input_port name="goal">Goal</input_port>
         <input_port name="service_name">Service name</input_port>
         <input_port name="server_timeout">Server timeout</input_port>
+        <input_port name="behavior_tree">Behavior tree to run</input_port>
     </Action>
 
     <Action ID="NavigateThroughPoses">
         <input_port name="goals">Goals</input_port>
         <input_port name="service_name">Service name</input_port>
         <input_port name="server_timeout">Server timeout</input_port>
+        <input_port name="behavior_tree">Behavior tree to run</input_port>
     </Action>
 
     <Action ID="ReinitializeGlobalLocalization">
diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp
index 657b191491..693fbfa146 100644
--- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp
+++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp
@@ -36,6 +36,7 @@ void NavigateThroughPosesAction::on_tick()
       "NavigateThroughPosesAction: goal not provided");
     return;
   }
+  getInput("behavior_tree", goal_.behavior_tree);
 }
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp
index c932bca257..82cdab44f8 100644
--- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp
+++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp
@@ -36,6 +36,7 @@ void NavigateToPoseAction::on_tick()
       "NavigateToPoseAction: goal not provided");
     return;
   }
+  getInput("behavior_tree", goal_.behavior_tree);
 }
 
 }  // namespace nav2_behavior_tree