Merge branch 'main' into behaviortree/generate_xml

Signed-off-by: Davide Faconti <[email protected]>

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v18\
+              - "<< parameters.key >>-v20\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v18\
+              - "<< parameters.key >>-v20\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v18\
+            key: "<< parameters.key >>-v20\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

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.
@@ -70,6 +70,19 @@ If you use **any** of the algorithms in Nav2 or the analysis of the algorithms i
   }
 ```
 
+If you use the Smac Planner (Hybrid A*, State Lattice, 2D), please cite this work in your papers!
+
+- S. Macenski, M. Booker, J. Wallace, [**Open-Source, Cost-Aware Kinematically Feasible Planning for Mobile and Surface Robotics**](https://arxiv.org/abs/2401.13078). 2024.
+
+```bibtex
+@article{macenski2024smac,
+      title={Open-Source, Cost-Aware Kinematically Feasible Planning for Mobile and Surface Robotics}, 
+      author={Steve Macenski and Matthew Booker and Josh Wallace},
+      year={2024},
+      journal = {Arxiv}
+}
+```
+
 If you use the Regulated Pure Pursuit Controller algorithm or software from this repository, please cite this work in your papers!
 
 - S. Macenski, S. Singh, F. Martin, J. Gines, [**Regulated Pure Pursuit for Robot Path Tracking**](https://arxiv.org/abs/2305.20026). Autonomous Robots, 2023.

nav2_amcl/src/amcl_node.cpp

@@ -328,10 +328,12 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   nomotion_update_srv_.reset();
   initial_pose_sub_.reset();
   laser_scan_connection_.disconnect();
+  tf_listener_.reset();  //  listener may access lase_scan_filter_, so it should be reset earlier
   laser_scan_filter_.reset();
   laser_scan_sub_.reset();
 
   // Map
+  map_sub_.reset();  //  map_sub_ may access map_, so it should be reset earlier
   if (map_ != NULL) {
     map_free(map_);
     map_ = nullptr;
@@ -341,7 +343,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 
   // Transforms
   tf_broadcaster_.reset();
-  tf_listener_.reset();
   tf_buffer_.reset();
 
   // PubSub

nav2_behavior_tree/CMakeLists.txt

@@ -204,6 +204,9 @@ list(APPEND plugin_libs nav2_smoother_selector_bt_node)
 add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp)
 list(APPEND plugin_libs nav2_goal_checker_selector_bt_node)
 
+add_library(nav2_progress_checker_selector_bt_node SHARED plugins/action/progress_checker_selector_node.cpp)
+list(APPEND plugin_libs nav2_progress_checker_selector_bt_node)
+
 add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
 list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)
 

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_action_server_impl.hpp

@@ -238,7 +238,7 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
   try {
     tree_ = bt_->createTreeFromFile(filename, blackboard_);
     for (auto & blackboard : tree_.blackboard_stack) {
-      blackboard->set<rclcpp::Node::SharedPtr>("node", client_node_);
+      blackboard->set("node", client_node_);
       blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
       blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
       blackboard->set<std::chrono::milliseconds>(

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/action/progress_checker_selector_node.hpp

@@ -0,0 +1,96 @@
+// Copyright (c) 2024 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "behaviortree_cpp_v3/action_node.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief The ProgressCheckerSelector behavior is used to switch the progress checker
+ * of the controller server. It subscribes to a topic "progress_checker_selector"
+ * to get the decision about what progress_checker must be used. It is usually used before of
+ * the FollowPath. The selected_progress_checker output port is passed to progress_checker_id
+ * input port of the FollowPath
+ */
+class ProgressCheckerSelector : public BT::SyncActionNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::ProgressCheckerSelector
+   *
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param conf  BT node configuration
+   */
+  ProgressCheckerSelector(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<std::string>(
+        "default_progress_checker",
+        "the default progress_checker to use if there is not any external topic message received."),
+
+      BT::InputPort<std::string>(
+        "topic_name",
+        "progress_checker_selector",
+        "the input topic name to select the progress_checker"),
+
+      BT::OutputPort<std::string>(
+        "selected_progress_checker",
+        "Selected progress_checker by subscription")
+    };
+  }
+
+private:
+  /**
+   * @brief Function to perform some user-defined operation on tick
+   */
+  BT::NodeStatus tick() override;
+
+  /**
+   * @brief callback function for the progress_checker_selector topic
+   *
+   * @param msg the message with the id of the progress_checker_selector
+   */
+  void callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg);
+
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr progress_checker_selector_sub_;
+
+  std::string last_selected_progress_checker_;
+
+  rclcpp::Node::SharedPtr node_;
+
+  std::string topic_name_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_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"),
     };
   }