Added behaviour_tree as input to action nodes (ros-navigation#2952)

* added BT as input to action node * fix format * Update navigate_to_pose_action.cpp * Update navigate_through_poses_action.cpp
EnjoyRobotics · Jun 30, 2022 · 0105004 · 0105004
1 parent 45e58eb
commit 0105004
Show file tree

Hide file tree

Showing 5 changed files with 6 additions and 0 deletions.
diff --git a/...behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/...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
@@ -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
@@ -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
@@ -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
@@ -36,6 +36,7 @@ void NavigateToPoseAction::on_tick()
       "NavigateToPoseAction: goal not provided");
     return;
   }
+  getInput("behavior_tree", goal_.behavior_tree);
 }
 
 }  // namespace nav2_behavior_tree