Humble

.github/CODEOWNERS

@@ -0,0 +1 @@
+* @tonynajjar @jplapp @andriimaistruk

nav2_behavior_tree/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(angles REQUIRED)
 
 nav2_package()
 
@@ -41,6 +42,7 @@ set(dependencies
   std_msgs
   std_srvs
   nav2_util
+  angles
 )
 
 add_library(${library_name} SHARED
@@ -208,6 +210,8 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
+install(FILES test/test_action_server.hpp test/test_service.hpp DESTINATION include/nav2_behavior_tree)
+
 install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include "rclcpp/rclcpp.hpp"
 
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
@@ -61,6 +62,7 @@ class BehaviorTreeEngine
     BT::Tree * tree,
     std::function<void()> onLoop,
     std::function<bool()> cancelRequested,
+    rclcpp::Logger logger,
     std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
 
   /**

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -47,7 +47,7 @@ class BtActionNode : public BT::ActionNodeBase
     const std::string & xml_tag_name,
     const std::string & action_name,
     const BT::NodeConfiguration & conf)
-  : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
+  : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name), should_send_goal_(true)
   {
     node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
     callback_group_ = node_->create_callback_group(
@@ -93,9 +93,9 @@ class BtActionNode : public BT::ActionNodeBase
 
     // Make sure the server is actually there before continuing
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
-    if (!action_client_->wait_for_action_server(1s)) {
+    if (!action_client_->wait_for_action_server(20s)) {
       RCLCPP_ERROR(
-        node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
+        node_->get_logger(), "\"%s\" action server not available after waiting for 20 s",
         action_name.c_str());
       throw std::runtime_error(
               std::string("Action server ") + action_name +
@@ -190,9 +190,15 @@ class BtActionNode : public BT::ActionNodeBase
       // setting the status to RUNNING to notify the BT Loggers (if any)
       setStatus(BT::NodeStatus::RUNNING);
 
-      // user defined callback
+      // reset the flag to send the goal or not, allowing the user the option to set it in on_tick
+      should_send_goal_ = true;
+
+      // user defined callback, may modify "should_send_goal_".
       on_tick();
 
+      if (!should_send_goal_) {
+        return BT::NodeStatus::FAILURE;
+      }
       send_new_goal();
     }
 
@@ -225,7 +231,8 @@ class BtActionNode : public BT::ActionNodeBase
         feedback_.reset();
 
         auto goal_status = goal_handle_->get_status();
-        if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
+        if (goal_updated_ &&
+          (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
           goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
         {
           goal_updated_ = false;
@@ -446,6 +453,9 @@ class BtActionNode : public BT::ActionNodeBase
   std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
   future_goal_handle_;
   rclcpp::Time time_goal_sent_;
+
+  // Can be set in on_tick or on_wait_for_result to indicate if a goal should be sent.
+  bool should_send_goal_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -169,7 +169,7 @@ class BtActionServer
    * @brief Getter function for the current BT tree
    * @return BT::Tree Current behavior tree
    */
-  const BT::Tree & getTree() const
+  const BT::Tree* getTree() const
   {
     return tree_;
   }
@@ -180,9 +180,12 @@ class BtActionServer
    */
   void haltTree()
   {
-    tree_.rootNode()->halt();
+    tree_->rootNode()->halt();
   }
 
+  void resetBlackboard();
+  std::map<std::size_t, BT::Tree> cached_trees;
+  std::vector<std::size_t> cached_tree_hashes;
 protected:
   /**
    * @brief Action server callback
@@ -196,7 +199,7 @@ class BtActionServer
   std::shared_ptr<ActionServer> action_server_;
 
   // Behavior Tree to be executed when goal is received
-  BT::Tree tree_;
+  BT::Tree* tree_;
 
   // The blackboard shared by all of the nodes in the tree
   BT::Blackboard::Ptr blackboard_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -21,6 +21,7 @@
 #include <set>
 #include <exception>
 #include <vector>
+#include <algorithm>
 
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 #include "nav2_behavior_tree/bt_action_server.hpp"
@@ -142,7 +143,7 @@ bool BtActionServer<ActionT>::on_cleanup()
   plugin_lib_names_.clear();
   current_bt_xml_filename_.clear();
   blackboard_.reset();
-  bt_->haltAllActions(tree_.rootNode());
+  bt_->haltAllActions(tree_->rootNode());
   bt_.reset();
   return true;
 }
@@ -153,11 +154,17 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
   // Empty filename is default for backward compatibility
   auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;
 
+  // This is removed as part of the changes about BT hashing as we still want to check for
+  // changes in the xml file even if current_bt_xml_filename_ == filename
+
+  /*
   // Use previous BT if it is the existing one
   if (current_bt_xml_filename_ == filename) {
     RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded");
     return true;
   }
+  */
+
 
   // Read the input BT XML from the specified file into a string
   std::ifstream xml_file(filename);
@@ -171,28 +178,50 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
     std::istreambuf_iterator<char>(xml_file),
     std::istreambuf_iterator<char>());
 
-  // Create the Behavior Tree from the XML input
-  try {
-    tree_ = bt_->createTreeFromText(xml_string, blackboard_);
-  } catch (const std::exception & e) {
-    RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());
-    return false;
+  std::hash<std::string> hasher;
+
+  std::size_t tree_hash = hasher(xml_string);
+
+  // if a tree was used before we fetch it from the cached trees not to create it one more time
+  if (std::find(cached_tree_hashes.begin(), cached_tree_hashes.end(), tree_hash) != cached_tree_hashes.end())
+  {
+    RCLCPP_DEBUG(logger_, "BT will not be loaded as it exists in cache");
+    tree_ = &cached_trees[tree_hash];
+  }
+  else
+  {
+    RCLCPP_DEBUG(logger_, "BT will be loaded as it doesn't exist in cache");
+
+    // Create the Behavior Tree from the XML input
+    cached_trees[tree_hash] = bt_->createTreeFromText(xml_string, blackboard_);
+    cached_tree_hashes.push_back(tree_hash);
+    tree_ = &cached_trees[tree_hash];
   }
 
-  topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
+  topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, *tree_);
 
   current_bt_xml_filename_ = filename;
+
   return true;
 }
 
+template<class ActionT>
+void BtActionServer<ActionT>::resetBlackboard()
+{
+  blackboard_->clear();
+  blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_);  // NOLINT
+  blackboard_->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);  // NOLINT
+  blackboard_->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);  // NOLINT
+}
+
 template<class ActionT>
 void BtActionServer<ActionT>::executeCallback()
 {
   if (!on_goal_received_callback_(action_server_->get_current_goal())) {
     action_server_->terminate_current();
     return;
   }
-
+  RCLCPP_INFO(logger_, "Action server name is %s", action_name_.c_str());
   auto is_canceling = [&]() {
       if (action_server_ == nullptr) {
         RCLCPP_DEBUG(logger_, "Action server unavailable. Canceling.");
@@ -214,11 +243,14 @@ void BtActionServer<ActionT>::executeCallback()
     };
 
   // Execute the BT that was previously created in the configure step
-  nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling, bt_loop_duration_);
+  nav2_behavior_tree::BtStatus rc = bt_->run(tree_, on_loop, is_canceling, logger_, bt_loop_duration_);
 
+  // send remaining logs
+  topic_logger_->flush();
+
   // Make sure that the Bt is not in a running state from a previous execution
   // note: if all the ControlNodes are implemented correctly, this is not needed.
-  bt_->haltAllActions(tree_.rootNode());
+  bt_->haltAllActions(tree_->rootNode());
 
   // Give server an opportunity to populate the result message or simple give
   // an indication that the action is complete.

nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp

@@ -22,6 +22,7 @@
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
 
 namespace BT
 {
@@ -100,6 +101,34 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
   }
 }
 
+/**
+ * @brief Parse XML string to geometry_msgs::msg::TransformStamped
+ * @param key XML string
+ * @return geometry_msgs::msg::TransformStamped
+ */
+template<>
+inline geometry_msgs::msg::TransformStamped convertFromString(const StringView key)
+{
+  // 7 real numbers separated by semicolons
+  auto parts = BT::splitString(key, ';');
+  if (parts.size() != 10) {
+    throw std::runtime_error("invalid number of fields for TransformStamped attribute)");
+  } else {
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    transform_stamped.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0]));
+    transform_stamped.header.frame_id = BT::convertFromString<std::string>(parts[1]);
+    transform_stamped.child_frame_id = BT::convertFromString<std::string>(parts[2]);
+    transform_stamped.transform.translation.x = BT::convertFromString<double>(parts[3]);
+    transform_stamped.transform.translation.y = BT::convertFromString<double>(parts[4]);
+    transform_stamped.transform.translation.z = BT::convertFromString<double>(parts[5]);
+    transform_stamped.transform.rotation.x = BT::convertFromString<double>(parts[6]);
+    transform_stamped.transform.rotation.y = BT::convertFromString<double>(parts[7]);
+    transform_stamped.transform.rotation.z = BT::convertFromString<double>(parts[8]);
+    transform_stamped.transform.rotation.w = BT::convertFromString<double>(parts[9]);
+    return transform_stamped;
+  }
+}
+
 /**
  * @brief Parse XML string to std::chrono::milliseconds
  * @param key XML string

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -74,14 +74,14 @@ class BtServiceNode : public BT::ActionNodeBase
     RCLCPP_DEBUG(
       node_->get_logger(), "Waiting for \"%s\" service",
       service_name_.c_str());
-    if (!service_client_->wait_for_service(1s)) {
+    if (!service_client_->wait_for_service(10s)) {
       RCLCPP_ERROR(
-        node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
-        service_node_name.c_str());
+        node_->get_logger(), "\"%s\" service server not available after waiting for 10 s",
+        service_name_.c_str());
       throw std::runtime_error(
               std::string(
                 "Service server %s not available",
-                service_node_name.c_str()));
+                service_name_.c_str()));
     }
 
     RCLCPP_DEBUG(
@@ -128,7 +128,22 @@ class BtServiceNode : public BT::ActionNodeBase
   BT::NodeStatus tick() override
   {
     if (!request_sent_) {
+      // reset the flag to send the request or not,
+      // allowing the user the option to set it in on_tick
+      should_send_request_ = true;
+      should_fail_not_sent_request_ = true;
+
+      // user defined callback, may modify "should_send_request_".
       on_tick();
+
+      if (!should_send_request_) {
+        if (should_fail_not_sent_request_) {
+            return BT::NodeStatus::FAILURE;
+        } else {
+            return BT::NodeStatus::SUCCESS;
+        }
+      }
+
       future_result_ = service_client_->async_send_request(request_).share();
       sent_time_ = node_->now();
       request_sent_ = true;
@@ -240,6 +255,10 @@ class BtServiceNode : public BT::ActionNodeBase
   std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;
   bool request_sent_{false};
   rclcpp::Time sent_time_;
+
+  // Can be set in on_tick or on_wait_for_result to indicate if a request should be sent.
+  bool should_send_request_;
+  bool should_fail_not_sent_request_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

@@ -55,7 +55,7 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
       {
         BT::InputPort<double>("backup_dist", 0.15, "Distance to backup"),
         BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup"),
-        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing")
+        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing"),
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp

@@ -50,7 +50,9 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
       {
         BT::InputPort<double>("dist_to_travel", 0.15, "Distance to travel"),
         BT::InputPort<double>("speed", 0.025, "Speed at which to travel"),
-        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading")
+        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading"),
+        BT::InputPort<bool>("free_goal_vel", false, "Don't stop when goal reached"),
+        BT::InputPort<bool>("check_local_costmap", true, "Check local costmap for collisions")
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp

@@ -64,6 +64,7 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
       {
         BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
         BT::InputPort<std::string>("controller_id", ""),
+        BT::InputPort<bool>("free_goal_vel", "Don't stop when goal reached"),
         BT::InputPort<std::string>("goal_checker_id", ""),
       });
   }

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp

@@ -74,6 +74,8 @@ class GoalReachedCondition : public BT::ConditionNode
     return {
       BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination"),
       BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
+      BT::InputPort<double>("xy_goal_tolerance", 0.1, "xy goal tolerance"),
+      BT::InputPort<double>("yaw_goal_tolerance", 0.1, "yaw goal tolerance"),
       BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
     };
   }
@@ -91,6 +93,7 @@ class GoalReachedCondition : public BT::ConditionNode
 
   bool initialized_;
   double goal_reached_tol_;
+  double goal_reached_tol_yaw_;
   std::string global_frame_;
   std::string robot_base_frame_;
   double transform_tolerance_;