Merge branch 'ros-planning:main' into feat/smac_planner_include_orien…

…tation_flexibility

README.md

@@ -27,7 +27,7 @@ If you need professional services related to Nav2, please contact Open Navigatio
 Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. 
 
 <p align="center">
-  <img src="doc/sponsors_may_2023.png" />
+  <img src="doc/sponsors_jan_2024.png" />
 </p>
 
 ### [Dexory](https://www.dexory.com/) develops robotics and AI logistics solutions to drive better business decisions using a digital twin of warehouses to provide inventory insights.

nav2_behavior_tree/CMakeLists.txt

@@ -222,6 +222,17 @@ install(TARGETS ${library_name}
   RUNTIME DESTINATION bin
 )
 
+# we will embed the list of plugin names inside a header file
+set(GENERATED_DIR ${CMAKE_CURRENT_BINARY_DIR}/gen)
+configure_file(plugins_list.hpp.in ${GENERATED_DIR}/plugins_list.hpp)
+
+add_executable(generate_nav2_tree_nodes_xml src/generate_nav2_tree_nodes_xml.cpp)
+ament_target_dependencies(generate_nav2_tree_nodes_xml ${dependencies})
+# allow generate_nav2_tree_nodes_xml to find plugins_list.hpp
+target_include_directories(generate_nav2_tree_nodes_xml PRIVATE ${GENERATED_DIR})
+install(TARGETS generate_nav2_tree_nodes_xml DESTINATION lib/${PROJECT_NAME})
+
+
 install(DIRECTORY include/
   DESTINATION include/
 )
@@ -231,6 +242,7 @@ install(DIRECTORY test/utils/
 )
 
 install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
+install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

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).has_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);
 }