iron backport: nav2_collision_monitor: collision detector (#3500)

.circleci/config.yml

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

.github/CODEOWNERS

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

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

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

@@ -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 +

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
@@ -203,7 +206,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

@@ -180,7 +180,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;
 }
@@ -191,11 +191,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);
@@ -205,28 +211,54 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
     return false;
   }
 
-  // Create the Behavior Tree from the XML input
-  try {
-    tree_ = bt_->createTreeFromFile(filename, blackboard_);
-  } catch (const std::exception & e) {
-    RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());
-    return false;
+  auto xml_string = std::string(
+    std::istreambuf_iterator<char>(xml_file),
+    std::istreambuf_iterator<char>());
+
+  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.");
@@ -248,11 +280,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

@@ -23,6 +23,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
 {
@@ -101,6 +102,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

@@ -77,14 +77,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(
@@ -134,12 +134,17 @@ class BtServiceNode : public BT::ActionNodeBase
       // 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_) {
-        return BT::NodeStatus::FAILURE;
+        if (should_fail_not_sent_request_) {
+            return BT::NodeStatus::FAILURE;
+        } else {
+            return BT::NodeStatus::SUCCESS;
+        }
       }
 
       future_result_ = service_client_->async_send_request(request_).share();
@@ -256,6 +261,7 @@ class BtServiceNode : public BT::ActionNodeBase
 
   // 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/drive_on_heading_action.hpp

@@ -55,6 +55,8 @@ 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<bool>("free_goal_vel", false, "Don't stop when goal reached"),
+        BT::InputPort<bool>("check_local_costmap", true, "Check local costmap for collisions"),
         BT::OutputPort<Action::Result::_error_code_type>(
           "error_code_id", "The drive on heading behavior server error code")
       });

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

@@ -83,6 +83,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", ""),
         BT::InputPort<std::string>("progress_checker_id", ""),
         BT::OutputPort<ActionResult::_error_code_type>(

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

nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp

@@ -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() );
   }
 
   /**

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -13,6 +13,8 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Server 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>
       <output_port name="error_code_id">"Back up error code"</output_port>
     </Action>
 
@@ -209,6 +211,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"/>

nav2_behavior_tree/package.xml

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