[behavior_tree] support both ports and blackboard (ros-navigation#4060)

* check result of blackboard->get and use halTree

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

* remove unused header

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

* BT: add port inputs when missing and use getInputPortOrBlackboard

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

* add description

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

* change return value of getInputPortOrBlackboard

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

* updated tree XML

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

---------

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -23,7 +23,6 @@
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "behaviortree_cpp_v3/xml_parsing.h"
-#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
 
 #include "rclcpp/rclcpp.hpp"
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp

@@ -132,20 +132,27 @@ inline std::set<int> convertFromString(StringView key)
 }
 
 /**
- * @brief Return parameter value from behavior tree node or ros2 parameter file
+ * @brief Return parameter value from behavior tree node or ros2 parameter file.
  * @param node rclcpp::Node::SharedPtr
  * @param param_name std::string
  * @param behavior_tree_node T2
  * @return <T1>
  */
-template<typename T1, typename T2>
+template<typename T1, typename T2 = BT::TreeNode>
 T1 deconflictPortAndParamFrame(
   rclcpp::Node::SharedPtr node,
   std::string param_name,
-  T2 * behavior_tree_node)
+  const T2 * behavior_tree_node)
 {
   T1 param_value;
-  if (!behavior_tree_node->getInput(param_name, param_value)) {
+  bool param_from_input = behavior_tree_node->getInput(param_name, param_value);
+
+  if constexpr (std::is_same_v<T1, std::string>) {
+    // not valid if port doesn't exist or it is an empty string
+    param_from_input &= !param_value.empty();
+  }
+
+  if (!param_from_input) {
     RCLCPP_DEBUG(
       node->get_logger(),
       "Parameter '%s' not provided by behavior tree xml file, "
@@ -162,6 +169,37 @@ T1 deconflictPortAndParamFrame(
   }
 }
 
+/**
+ * @brief Try reading an import port first, and if that doesn't work
+ * fallback to reading directly the blackboard.
+ * The blackboard must be passed explitly because config() is private in BT.CPP 4.X
+ *
+ * @param bt_node node
+ * @param blackboard the blackboard ovtained with node->config().blackboard
+ * @param param_name std::string
+ * @param behavior_tree_node the node
+ * @return <T>
+ */
+template<typename T> inline
+bool getInputPortOrBlackboard(
+  const BT::TreeNode & bt_node,
+  const BT::Blackboard & blackboard,
+  const std::string & param_name,
+  T & value)
+{
+  if (bt_node.getInput<T>(param_name, value)) {
+    return true;
+  }
+  if (blackboard.get<T>(param_name, value)) {
+    return true;
+  }
+  return false;
+}
+
+// Macro to remove boiler plate when using getInputPortOrBlackboard
+#define getInputOrBlackboard(name, value) \
+  getInputPortOrBlackboard(*this, *(this->config().blackboard), name, value);
+
 }  // namespace BT
 
 #endif  // NAV2_BEHAVIOR_TREE__BT_UTILS_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp

@@ -57,7 +57,12 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode
    */
   static BT::PortsList providedPorts()
   {
-    return {};
+    return {
+      BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+        "goals", "Vector of navigation goals"),
+      BT::InputPort<geometry_msgs::msg::PoseStamped>(
+        "goal", "Navigation goal"),
+    };
   }
 
 private:

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp

@@ -54,7 +54,12 @@ class GoalUpdatedCondition : public BT::ConditionNode
    */
   static BT::PortsList providedPorts()
   {
-    return {};
+    return {
+      BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+        "goals", "Vector of navigation goals"),
+      BT::InputPort<geometry_msgs::msg::PoseStamped>(
+        "goal", "Navigation goal"),
+    };
   }
 
 private:

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp

@@ -15,6 +15,7 @@
 #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
 #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
 
+#include <string>
 #include "behaviortree_cpp_v3/behavior_tree.h"
 
 namespace nav2_behavior_tree
@@ -23,7 +24,21 @@ namespace nav2_behavior_tree
  * @brief A BT::ConditionNode that returns SUCCESS if initial pose
  * has been received and FAILURE otherwise
  */
-BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node);
-}
+class InitialPoseReceived : public BT::ConditionNode
+{
+public:
+  InitialPoseReceived(
+    const std::string & name,
+    const BT::NodeConfiguration & config);
+
+  static BT::PortsList providedPorts()
+  {
+    return {BT::InputPort<bool>("initial_pose_received")};
+  }
+
+  BT::NodeStatus tick() override;
+};
+
+}  // namespace nav2_behavior_tree
 
 #endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp

@@ -48,7 +48,12 @@ class GoalUpdatedController : public BT::DecoratorNode
    */
   static BT::PortsList providedPorts()
   {
-    return {};
+    return {
+      BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+        "goals", "Vector of navigation goals"),
+      BT::InputPort<geometry_msgs::msg::PoseStamped>(
+        "goal", "Navigation goal"),
+    };
   }
 
 private:

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp

@@ -57,6 +57,10 @@ class SpeedController : public BT::DecoratorNode
       BT::InputPort<double>("max_rate", 1.0, "Maximum rate"),
       BT::InputPort<double>("min_speed", 0.0, "Minimum speed"),
       BT::InputPort<double>("max_speed", 0.5, "Maximum speed"),
+      BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+        "goals", "Vector of navigation goals"),
+      BT::InputPort<geometry_msgs::msg::PoseStamped>(
+        "goal", "Navigation goal"),
     };
   }
 

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -224,9 +224,15 @@
       <input_port name="parent">Parent frame for transform</input_port>
     </Condition>
 
-    <Condition ID="GoalUpdated"/>
+    <Condition ID="GoalUpdated">
+      <input_port name="goal">Vector of navigation goals</input_port>
+      <input_port name="goals">Navigation goal</input_port>
+    </Condition>
 
-    <Condition ID="GlobalUpdatedGoal"/>
+    <Condition ID="GlobalUpdatedGoal">
+      <input_port name="goal">Vector of navigation goals</input_port>
+      <input_port name="goals">Navigation goal</input_port>
+    </Condition>
 
     <Condition ID="IsBatteryLow">
       <input_port name="min_battery">Min battery % or voltage before triggering</input_port>
@@ -255,6 +261,7 @@
     </Condition>
 
     <Condition ID="InitialPoseReceived">
+      <input_port name="initial_pose_received">SUCCESS if initial_pose_received true</input_port>
     </Condition>
 
     <Condition ID="IsPathValid">
@@ -313,6 +320,8 @@
       <input_port name="max_rate">Maximum rate</input_port>
       <input_port name="min_speed">Minimum speed</input_port>
       <input_port name="max_speed">Maximum speed</input_port>
+      <input_port name="goal">Vector of navigation goals</input_port>
+      <input_port name="goals">Navigation goal</input_port>
     </Decorator>
 
     <Decorator ID="PathLongerOnApproach">
@@ -322,6 +331,8 @@
     </Decorator>
 
     <Decorator ID="GoalUpdatedController">
+      <input_port name="goal">Vector of navigation goals</input_port>
+      <input_port name="goals">Navigation goal</input_port>
     </Decorator>
 
   </TreeNodesModel>

nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

@@ -41,7 +41,7 @@ void RemovePassedGoals::initialize()
   auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   node->get_parameter("transform_tolerance", transform_tolerance_);
 
-  robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string, RemovePassedGoals>(
+  robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
     node, "robot_base_frame", this);
 }
 

nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp

@@ -43,9 +43,9 @@ void DistanceTraveledCondition::initialize()
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
   node_->get_parameter("transform_tolerance", transform_tolerance_);
 
-  global_frame_ = BT::deconflictPortAndParamFrame<std::string, DistanceTraveledCondition>(
+  global_frame_ = BT::deconflictPortAndParamFrame<std::string>(
     node_, "global_frame", this);
-  robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string, DistanceTraveledCondition>(
+  robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
     node_, "robot_base_frame", this);
   initialized_ = true;
 }

nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp

@@ -16,6 +16,7 @@
 #include <string>
 
 #include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -33,15 +34,15 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick()
 {
   if (first_time) {
     first_time = false;
-    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
-    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    BT::getInputOrBlackboard("goals", goals_);
+    BT::getInputOrBlackboard("goal", goal_);
     return BT::NodeStatus::SUCCESS;
   }
 
   std::vector<geometry_msgs::msg::PoseStamped> current_goals;
-  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  BT::getInputOrBlackboard("goals", current_goals);
   geometry_msgs::msg::PoseStamped current_goal;
-  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
+  BT::getInputOrBlackboard("goal", current_goal);
 
   if (goal_ != current_goal || goals_ != current_goals) {
     goal_ = current_goal;

nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

@@ -33,7 +33,7 @@ GoalReachedCondition::GoalReachedCondition(
 {
   auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 
-  robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string, GoalReachedCondition>(
+  robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
     node, "robot_base_frame", this);
 }
 

nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp

@@ -15,6 +15,7 @@
 #include <string>
 #include <vector>
 #include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -28,15 +29,15 @@ GoalUpdatedCondition::GoalUpdatedCondition(
 BT::NodeStatus GoalUpdatedCondition::tick()
 {
   if (status() == BT::NodeStatus::IDLE) {
-    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
-    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    BT::getInputOrBlackboard("goals", goals_);
+    BT::getInputOrBlackboard("goal", goal_);
     return BT::NodeStatus::FAILURE;
   }
 
   std::vector<geometry_msgs::msg::PoseStamped> current_goals;
-  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
   geometry_msgs::msg::PoseStamped current_goal;
-  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
+  BT::getInputOrBlackboard("goals", current_goals);
+  BT::getInputOrBlackboard("goal", current_goal);
 
   if (goal_ != current_goal || goals_ != current_goals) {
     goal_ = current_goal;

nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp

@@ -13,13 +13,21 @@
 // limitations under the License.
 
 #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {
+InitialPoseReceived::InitialPoseReceived(
+  const std::string & name,
+  const BT::NodeConfiguration & config)
+: BT::ConditionNode(name, config)
+{
+}
 
-BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node)
+BT::NodeStatus InitialPoseReceived::tick()
 {
-  auto initPoseReceived = tree_node.config().blackboard->get<bool>("initial_pose_received");
+  bool initPoseReceived = false;
+  BT::getInputOrBlackboard("initial_pose_received", initPoseReceived);
   return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
 }
 
@@ -28,7 +36,5 @@ BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node)
 #include "behaviortree_cpp_v3/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
-  factory.registerSimpleCondition(
-    "InitialPoseReceived",
-    std::bind(&nav2_behavior_tree::initialPoseReceived, std::placeholders::_1));
+  factory.registerNodeType<nav2_behavior_tree::InitialPoseReceived>("InitialPoseReceived");
 }

nav2_behavior_tree/plugins/decorator/distance_controller.cpp

@@ -43,9 +43,9 @@ DistanceController::DistanceController(
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
   node_->get_parameter("transform_tolerance", transform_tolerance_);
 
-  global_frame_ = BT::deconflictPortAndParamFrame<std::string, DistanceController>(
+  global_frame_ = BT::deconflictPortAndParamFrame<std::string>(
     node_, "global_frame", this);
-  robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string, DistanceController>(
+  robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
     node_, "robot_base_frame", this);
 }
 

nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp

@@ -19,7 +19,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "behaviortree_cpp_v3/decorator_node.h"
 #include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"
-
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -37,18 +37,18 @@ BT::NodeStatus GoalUpdatedController::tick()
     // Reset since we're starting a new iteration of
     // the goal updated controller (moving from IDLE to RUNNING)
 
-    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
-    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    BT::getInputOrBlackboard("goals", goals_);
+    BT::getInputOrBlackboard("goal", goal_);
 
     goal_was_updated_ = true;
   }
 
   setStatus(BT::NodeStatus::RUNNING);
 
   std::vector<geometry_msgs::msg::PoseStamped> current_goals;
-  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  BT::getInputOrBlackboard("goals", current_goals);
   geometry_msgs::msg::PoseStamped current_goal;
-  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
+  BT::getInputOrBlackboard("goal", current_goal);
 
   if (goal_ != current_goal || goals_ != current_goals) {
     goal_ = current_goal;

nav2_behavior_tree/plugins/decorator/speed_controller.cpp

@@ -19,6 +19,7 @@
 #include "nav2_util/geometry_utils.hpp"
 
 #include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -61,17 +62,17 @@ inline BT::NodeStatus SpeedController::tick()
   if (status() == BT::NodeStatus::IDLE) {
     // Reset since we're starting a new iteration of
     // the speed controller (moving from IDLE to RUNNING)
-    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
-    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    BT::getInputOrBlackboard("goals", goals_);
+    BT::getInputOrBlackboard("goal", goal_);
     period_ = 1.0 / max_rate_;
     start_ = node_->now();
     first_tick_ = true;
   }
 
   std::vector<geometry_msgs::msg::PoseStamped> current_goals;
-  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  BT::getInputOrBlackboard("goals", current_goals);
   geometry_msgs::msg::PoseStamped current_goal;
-  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
+  BT::getInputOrBlackboard("goal", current_goal);
 
   if (goal_ != current_goal || goals_ != current_goals) {
     // Reset state and set period to max since we have a new goal