logivations · tonynajjar · Nov 15, 2022 · Oct 20, 2022 · Oct 20, 2022 · Oct 20, 2022
@@ -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
@@ -199,6 +201,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)

@@ -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(10s)) {
       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 10 s",
         action_name.c_str());
       throw std::runtime_error(std::string("Action server %s not available", action_name.c_str()));
     }

@@ -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,11 @@ class BtActionServer
    */
   void haltTree()
   {
-    tree_.rootNode()->halt();
+    tree_->rootNode()->halt();
   }
 
+  std::map<std::size_t, BT::Tree> cached_trees;
+  std::vector<std::size_t> cached_tree_hashes;
 protected:
   /**
    * @brief Action server callback
@@ -196,7 +198,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_;

@@ -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,17 +178,30 @@ 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;
 }
 
@@ -192,7 +212,7 @@ void BtActionServer<ActionT>::executeCallback()
     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 +234,11 @@ 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, bt_loop_duration_);
 
   // 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.

@@ -71,7 +71,7 @@ class BtServiceNode : public BT::ActionNodeBase
     RCLCPP_DEBUG(
       node_->get_logger(), "Waiting for \"%s\" service",
       service_name_.c_str());
-    service_client_->wait_for_service();
+    service_client_->wait_for_service(std::chrono::seconds(10));
 
     RCLCPP_DEBUG(
       node_->get_logger(), "\"%s\" BtServiceNode initialized",

@@ -55,7 +55,9 @@ 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"),
+        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")
       });
   }
 };

@@ -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<std::string>("free_goal_vel", "Don't stop when goal reached"),
         BT::InputPort<std::string>("goal_checker_id", ""),
       });
   }

@@ -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_;

@@ -46,7 +46,7 @@ class RosTopicLogger : public BT::StatusChangeLogger
     clock_ = node->get_clock();
     logger_ = node->get_logger();
     log_pub_ = node->create_publisher<nav2_msgs::msg::BehaviorTreeLog>(
-      "behavior_tree_log",
+      "~/behavior_tree_log",
       rclcpp::QoS(10));
   }
 
@@ -69,16 +69,18 @@ class RosTopicLogger : public BT::StatusChangeLogger
     // before converting to a msg.
     event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp));
     event.node_name = node.name();
+    event.node_uid = node.UID();
     event.previous_status = toStr(prev_status, false);
     event.current_status = toStr(status, false);
     event_log_.push_back(std::move(event));
 
-    RCLCPP_DEBUG(
-      logger_, "[%.3f]: %25s %s -> %s",
-      std::chrono::duration<double>(timestamp).count(),
-      node.name().c_str(),
-      toStr(prev_status, true).c_str(),
-      toStr(status, true).c_str() );
+    // AMRFM-2554 Remove BT messages about node state transitioning
+    // RCLCPP_DEBUG(
+    //   logger_, "[%.3f]: %25s %s -> %s",
+    //   std::chrono::duration<double>(timestamp).count(),
+    //   node.name().c_str(),
+    //   toStr(prev_status, true).c_str(),
+    //   toStr(status, true).c_str() );
   }
 
   /**

@@ -13,6 +13,8 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <input_port name="free_goal_vel" default="false">Don't stop when goal reached</input_port>
+      <input_port name="check_local_costmap" default="true">Check if there is an obstacle on path</input_port>
     </Action>
 
     <Action ID="DriveOnHeading">
@@ -169,6 +171,8 @@
         <input_port name="goal">Destination</input_port>
         <input_port name="global_frame">Reference frame</input_port>
         <input_port name="robot_base_frame">Robot base frame</input_port>
+        <input_port name="xy_goal_tolerance">xy goal tolerance</input_port>
+        <input_port name="yaw_goal_tolerance">yaw goal tolerance</input_port>
     </Condition>
 
     <Condition ID="IsStuck"/>

@@ -31,6 +31,7 @@
   <build_depend>nav2_util</build_depend>
   <build_depend>lifecycle_msgs</build_depend>
   <build_depend>nav2_common</build_depend>
+  <build_depend>angles</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_action</exec_depend>

@@ -32,13 +32,19 @@ BackUpAction::BackUpAction(
   getInput("backup_speed", speed);
   double time_allowance;
   getInput("time_allowance", time_allowance);
+  bool free_goal_vel;
+  getInput("free_goal_vel", free_goal_vel);
+  bool check_local_costmap;
+  getInput("check_local_costmap", check_local_costmap);
 
   // Populate the input message
   goal_.target.x = dist;
   goal_.target.y = 0.0;
   goal_.target.z = 0.0;
   goal_.speed = speed;
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+  goal_.free_goal_vel = free_goal_vel;
+  goal_.check_local_costmap = check_local_costmap;
 }
 
 void BackUpAction::on_tick()

@@ -32,6 +32,7 @@ void FollowPathAction::on_tick()
 {
   getInput("path", goal_.path);
   getInput("controller_id", goal_.controller_id);
+  getInput("free_goal_vel", goal_.free_goal_vel);
   getInput("goal_checker_id", goal_.goal_checker_id);
 }
 

@@ -18,6 +18,8 @@
 #include "nav2_util/robot_utils.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_util/node_utils.hpp"
+#include "tf2/utils.h"
+#include "angles/angles.h"
 
 #include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
 
@@ -34,6 +36,10 @@ GoalReachedCondition::GoalReachedCondition(
 {
   getInput("global_frame", global_frame_);
   getInput("robot_base_frame", robot_base_frame_);
+  getInput("xy_goal_tolerance", goal_reached_tol_);
+  getInput("yaw_goal_tolerance", goal_reached_tol_yaw_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
 }
 
 GoalReachedCondition::~GoalReachedCondition()
@@ -57,10 +63,10 @@ void GoalReachedCondition::initialize()
 {
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 
-  nav2_util::declare_parameter_if_not_declared(
-    node_, "goal_reached_tol",
-    rclcpp::ParameterValue(0.25));
-  node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
+  //nav2_util::declare_parameter_if_not_declared(
+  //  node_, "goal_reached_tol",
+  //  rclcpp::ParameterValue(0.25));
+  //node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
 
   node_->get_parameter("transform_tolerance", transform_tolerance_);
@@ -81,10 +87,14 @@ bool GoalReachedCondition::isGoalReached()
 
   geometry_msgs::msg::PoseStamped goal;
   getInput("goal", goal);
+
+  double current_yaw = tf2::getYaw(current_pose.pose.orientation);
+  double goal_yaw = tf2::getYaw(goal.pose.orientation);
+  double dangle = fabs(angles::shortest_angular_distance(goal_yaw, current_yaw));
   double dx = goal.pose.position.x - current_pose.pose.position.x;
   double dy = goal.pose.position.y - current_pose.pose.position.y;
 
-  return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_);
+  return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_) && dangle <= goal_reached_tol_yaw_;
 }
 
 }  // namespace nav2_behavior_tree

@@ -26,9 +26,10 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
     return Status::FAILED;
   }
 
-  // Silently ensure that both the speed and direction are negative.
-  command_x_ = -std::fabs(command->target.x);
-  command_speed_ = -std::fabs(command->speed);
+  command_x_ = command->target.x;
+  command_speed_ = std::fabs(command->speed);
+  free_goal_vel = command->free_goal_vel;
+  check_local_costmap = command->check_local_costmap;
   command_time_allowance_ = command->time_allowance;
 
   end_time_ = steady_clock_.now() + command_time_allowance_;