Add DistanceController decorator node (#1699)

* Add DistanceController decorator node

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Update documentation and rename behavior tree XMLs

* Add tests for distance controller

* Fix test

* Update readme and add BT image

* Rename test files

* Remove protected setStatus calls

nav2_behavior_tree/CMakeLists.txt

@@ -89,6 +89,9 @@ list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node)
 add_library(nav2_rate_controller_bt_node SHARED plugins/decorator/rate_controller.cpp)
 list(APPEND plugin_libs nav2_rate_controller_bt_node)
 
+add_library(nav2_distance_controller_bt_node SHARED plugins/decorator/distance_controller.cpp)
+list(APPEND plugin_libs nav2_distance_controller_bt_node)
+
 add_library(nav2_recovery_node_bt_node SHARED plugins/control/recovery_node.cpp)
 list(APPEND plugin_libs nav2_recovery_node_bt_node)
 
@@ -123,9 +126,13 @@ install(DIRECTORY include/
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
 endif()
 
-ament_export_include_directories(include)
+ament_export_include_directories(
+  include
+)
 
 ament_export_libraries(
   ${library_name}

nav2_behavior_tree/README.md

@@ -74,6 +74,7 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 | GoalUpdated | Condition | Checks if the global navigation goal has changed in the blackboard. Returns failure if the goal is the same, if it changes, it returns success. |
 | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. |
 | RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. |
+| DistanceController | Decorator | A node that controls the tick rate for its child based on the distance traveled. The distance to be traveled before replanning can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `DistanceController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. |
 | RandomCrawl | Action | This BT action invokes the RandomCrawl ROS2 action server, which is implemented by the nav2_experimental/nav2_rl/nav2_turtlebot3_rl (in the nav2_experimental branch) experimental module. The RandomCrawl action server will direct the robot to randomly navigate its environment without hitting any obstacles. |
 | RecoveryNode | Control | The RecoveryNode is a control flow node with two children.  It returns SUCCESS if and only if the first child returns SUCCESS. The second child will be executed only if the first child returns FAILURE. If the second child SUCCEEDS, then the first child will be executed again. The user can specify how many times the recovery actions should be taken before returning FAILURE. In nav2, the RecoveryNode is included in Behavior Trees to implement recovery actions upon failures.
 | Spin | Action | Invokes the Spin ROS2 action server, which is implemented by the nav2_recoveries module. This action is using in nav2 Behavior Trees as a recovery behavior. |

nav2_behavior_tree/plugins/decorator/distance_controller.cpp

+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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.
+
+#include <chrono>
+#include <string>
+#include <memory>
+#include <cmath>
+
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_ros/buffer.h"
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+#include "distance_controller.hpp"
+
+namespace nav2_behavior_tree
+{
+
+DistanceController::DistanceController(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::DecoratorNode(name, conf),
+  distance_(1.0),
+  first_time_(false)
+{
+  getInput("distance", distance_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+}
+
+inline BT::NodeStatus DistanceController::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    // Reset the starting position since we're starting a new iteration of
+    // the distance controller (moving from IDLE to RUNNING)
+    if (!nav2_util::getCurrentPose(start_pose_, *tf_)) {
+      RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+      return BT::NodeStatus::FAILURE;
+    }
+    first_time_ = true;
+  }
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  // Determine distance travelled since we've started this iteration
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(current_pose, *tf_)) {
+    RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Get euclidean distance
+  auto travelled = nav2_util::geometry_utils::euclideanDistance(
+    start_pose_.pose.position, current_pose.pose.position);
+
+  // The child gets ticked the first time through and every time the threshold
+  // distance is crossed. In addition, once the child begins to run, it is
+  // ticked each time 'til completion
+  if (first_time_ || (child_node_->status() == BT::NodeStatus::RUNNING) ||
+    travelled >= distance_)
+  {
+    first_time_ = false;
+    const BT::NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state) {
+      case BT::NodeStatus::RUNNING:
+        return BT::NodeStatus::RUNNING;
+
+      case BT::NodeStatus::SUCCESS:
+        if (!nav2_util::getCurrentPose(start_pose_, *tf_)) {
+          RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+          return BT::NodeStatus::FAILURE;
+        }
+        return BT::NodeStatus::SUCCESS;
+
+      case BT::NodeStatus::FAILURE:
+      default:
+        return BT::NodeStatus::FAILURE;
+    }
+  }
+
+  return status();
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::DistanceController>("DistanceController");
+}

nav2_behavior_tree/plugins/decorator/distance_controller.hpp

+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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__DECORATOR__DISTANCE_CONTROLLER_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_ros/buffer.h"
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+namespace nav2_behavior_tree
+{
+
+class DistanceController : public BT::DecoratorNode
+{
+public:
+  DistanceController(
+    const std::string & name,
+    const BT::NodeConfiguration & conf);
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<double>("distance", 1.0, "Distance")
+    };
+  }
+
+private:
+  BT::NodeStatus tick() override;
+
+  rclcpp::Node::SharedPtr node_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+
+  geometry_msgs::msg::PoseStamped start_pose_;
+  double distance_;
+  bool first_time_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_

nav2_behavior_tree/test/CMakeLists.txt

+ament_add_gtest(test_decorator_distance_controller
+  plugins/decorator/test_distance_controller.cpp
+)
+
+target_link_libraries(test_decorator_distance_controller
+  nav2_distance_controller_bt_node
+)
+
+ament_target_dependencies(test_decorator_distance_controller
+  ${dependencies}
+)

nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp

+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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.
+
+#include <gtest/gtest.h>
+#include <cmath>
+#include <memory>
+#include <set>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+#include "../../test_transform_handler.hpp"
+#include "../../test_dummy_tree_node.hpp"
+#include "../../../plugins/decorator/distance_controller.hpp"
+
+class DistanceControllerTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    transform_handler_ = new nav2_behavior_tree::TransformHandler();
+    config_ = new BT::NodeConfiguration();
+    dummy_node_ = new nav2_behavior_tree::DummyNode();
+
+    // Create the blackboard that will be shared by all of the nodes in the tree
+    config_->blackboard = BT::Blackboard::create();
+    // Put items on the blackboard
+    config_->blackboard->set<rclcpp::Node::SharedPtr>(
+      "node",
+      rclcpp::Node::SharedPtr(transform_handler_));
+    config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(
+      "tf_buffer",
+      transform_handler_->getBuffer());
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<bool>("path_updated", false);
+    config_->blackboard->set<bool>("initial_pose_received", false);
+
+    transform_handler_->activate();
+    transform_handler_->waitForTransform();
+  }
+
+  static void TearDownTestCase()
+  {
+    transform_handler_->deactivate();
+    delete transform_handler_;
+    delete config_;
+    delete dummy_node_;
+    transform_handler_ = nullptr;
+    config_ = nullptr;
+    dummy_node_ = nullptr;
+  }
+
+  void SetUp()
+  {
+    node_ = new nav2_behavior_tree::DistanceController("distance_controller", *config_);
+    node_->setChild(dummy_node_);
+  }
+
+  void TearDown()
+  {
+    node_ = nullptr;
+  }
+
+protected:
+  static nav2_behavior_tree::TransformHandler * transform_handler_;
+  static BT::NodeConfiguration * config_;
+  static nav2_behavior_tree::DistanceController * node_;
+  static nav2_behavior_tree::DummyNode * dummy_node_;
+};
+
+nav2_behavior_tree::TransformHandler * DistanceControllerTestFixture::transform_handler_ = nullptr;
+BT::NodeConfiguration * DistanceControllerTestFixture::config_ = nullptr;
+nav2_behavior_tree::DistanceController * DistanceControllerTestFixture::node_ = nullptr;
+nav2_behavior_tree::DummyNode * DistanceControllerTestFixture::dummy_node_ = nullptr;
+
+TEST_F(DistanceControllerTestFixture, test_behavior)
+{
+  EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE);
+
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
+
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = 0;
+  pose.pose.position.y = 0;
+  pose.pose.orientation.w = 1;
+
+  double traveled = 0;
+  for (int i = 1; i <= 20; i++) {
+    pose.pose.position.x = i * 0.51;
+    transform_handler_->updateRobotPose(pose.pose);
+
+    // Wait for transforms to actually update
+    // updated pose is i * 0.55
+    // we wait fot the traveled distance to reach a value > i * 0.5
+    // we can assume the current transform has been updated at this point
+    while (traveled < i * 0.5) {
+      if (nav2_util::getCurrentPose(pose, *transform_handler_->getBuffer())) {
+        traveled = std::sqrt(
+          pose.pose.position.x * pose.pose.position.x +
+          pose.pose.position.y * pose.pose.position.y);
+      }
+    }
+
+    dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+
+    if (i % 2) {
+      EXPECT_EQ(node_->executeTick(), BT::NodeStatus::RUNNING);
+    } else {
+      EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
+      EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
+    }
+  }
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

nav2_behavior_tree/test/test_dummy_tree_node.hpp

+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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 TEST_DUMMY_TREE_NODE_HPP_
+#define TEST_DUMMY_TREE_NODE_HPP_
+
+#include <behaviortree_cpp_v3/basic_types.h>
+#include <behaviortree_cpp_v3/action_node.h>
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A Dummy TreeNode to be used as a child for testing nodes
+ * Returns the current status on tick without any execution logic
+ */
+class DummyNode : public BT::ActionNodeBase
+{
+public:
+  DummyNode()
+  : BT::ActionNodeBase("dummy", {})
+  {
+  }
+
+  void changeStatus(BT::NodeStatus status)
+  {
+    setStatus(status);
+  }
+
+  BT::NodeStatus executeTick() override
+  {
+    return tick();
+  }
+
+  BT::NodeStatus tick() override
+  {
+    return status();
+  }
+
+  void halt() override
+  {
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // TEST_DUMMY_TREE_NODE_HPP_

nav2_behavior_tree/test/test_transform_handler.hpp

+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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 TEST_TRANSFORM_HANDLER_HPP_
+#define TEST_TRANSFORM_HANDLER_HPP_
+
+#include <memory>
+#include <string>
+#include <thread>
+#include <chrono>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "tf2_msgs/msg/tf_message.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.h"
+
+using namespace std::chrono_literals; // NOLINT
+using namespace std::chrono;  // NOLINT
+
+namespace nav2_behavior_tree
+{
+class TransformHandler : public rclcpp::Node
+{
+public:
+  TransformHandler()
+  : Node("TransformHandler"),
+    is_active_(false),
+    base_transform_(nullptr),
+    tf_broadcaster_(nullptr)
+  {
+    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  }
+
+  ~TransformHandler()
+  {
+    if (is_active_) {
+      deactivate();
+    }
+  }
+
+  // Activate the tester before running tests
+  void activate()
+  {
+    if (is_active_) {
+      throw std::runtime_error("Trying to activate while already active");
+    }
+    is_active_ = true;
+
+    // Launch a thread to process the messages for this node
+    spin_thread_ = std::make_unique<nav2_util::NodeThread>(this);
+
+    startRobotTransform();
+  }
+
+  void deactivate()
+  {
+    if (!is_active_) {
+      throw std::runtime_error("Trying to deactivate while already inactive");
+    }
+    is_active_ = false;
+    spin_thread_.reset();
+    tf_broadcaster_.reset();
+    tf_buffer_.reset();
+    tf_listener_.reset();
+  }
+
+  std::shared_ptr<tf2_ros::Buffer> getBuffer()
+  {
+    return tf_buffer_;
+  }
+
+  void waitForTransform()
+  {
+    if (is_active_) {
+      while (!tf_buffer_->canTransform("map", "base_link", rclcpp::Time(0))) {
+        std::this_thread::sleep_for(100ms);
+      }
+      RCLCPP_INFO(get_logger(), "Transforms are available now!");
+      return;
+    }
+    throw std::runtime_error("Trying to deactivate while already inactive");
+  }
+
+  void updateRobotPose(const geometry_msgs::msg::Pose & pose)
+  {
+    // Update base transform to publish
+    base_transform_->transform.translation.x = pose.position.x;
+    base_transform_->transform.translation.y = pose.position.y;
+    base_transform_->transform.translation.z = pose.position.z;
+    base_transform_->transform.rotation.x = pose.orientation.x;
+    base_transform_->transform.rotation.y = pose.orientation.y;
+    base_transform_->transform.rotation.z = pose.orientation.z;
+    base_transform_->transform.rotation.w = pose.orientation.w;
+    publishRobotTransform();
+  }
+
+private:
+  void publishRobotTransform()
+  {
+    base_transform_->header.stamp = now();
+    tf_broadcaster_->sendTransform(*base_transform_);
+  }
+
+  void startRobotTransform()
+  {
+    // Provide the robot pose transform
+    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+
+    if (!base_transform_) {
+      base_transform_ = std::make_unique<geometry_msgs::msg::TransformStamped>();
+      base_transform_->header.frame_id = "map";
+      base_transform_->child_frame_id = "base_link";
+    }
+
+    // Set an initial pose
+    geometry_msgs::msg::Pose robot_pose;
+    robot_pose.position.x = 0;
+    robot_pose.position.y = 0;
+    robot_pose.orientation.w = 1;
+    updateRobotPose(robot_pose);
+
+    // Publish the transform periodically
+    transform_timer_ = create_wall_timer(
+      100ms, std::bind(&TransformHandler::publishRobotTransform, this));
+  }
+
+  bool is_active_;
+
+  // A thread for spinning the ROS node
+  std::unique_ptr<nav2_util::NodeThread> spin_thread_;
+
+  // Subscriber
+
+  // The tester must provide the robot pose through a transform
+  std::unique_ptr<geometry_msgs::msg::TransformStamped> base_transform_;
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  rclcpp::TimerBase::SharedPtr transform_timer_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // TEST_TRANSFORM_HANDLER_HPP_

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -63,6 +63,7 @@ bt_navigator:
     - nav2_initial_pose_received_condition_bt_node
     - nav2_reinitialize_global_localization_service_bt_node
     - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node
     - nav2_transform_available_condition_bt_node

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -63,6 +63,7 @@ bt_navigator:
     - nav2_initial_pose_received_condition_bt_node
     - nav2_reinitialize_global_localization_service_bt_node
     - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node
     - nav2_transform_available_condition_bt_node

nav2_bringup/bringup/params/nav2_params.yaml

@@ -63,6 +63,7 @@ bt_navigator:
     - nav2_initial_pose_received_condition_bt_node
     - nav2_reinitialize_global_localization_service_bt_node
     - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node
     - nav2_round_robin_node_bt_node

nav2_bt_navigator/README.md

@@ -24,30 +24,23 @@ A Behavior Tree consists of control flow nodes, such as fallback, sequence, para
 
 ## Navigation Behavior Trees
 
-The BT Navigator package has two sample XML-based descriptions of BTs.  These trees are [navigate_w_replanning.xml](behavior_trees/navigate_w_replanning.xml) and [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml).  The user may use any of these sample trees or develop a more complex tree which could better suit the user's needs.
-
-### Navigate with Replanning
-
-[navigate_w_replanning.xml](behavior_trees/navigate_w_replanning.xml) implements basic navigation by continuously computing and updating the path at a rate of 1Hz. The default controller, the nav2_dwb_controller, implements path following at a rate of 10Hz.
-
-```XML
-<root main_tree_to_execute="MainTree">
-  <BehaviorTree ID="MainTree">
-    <Sequence name="root">
-      <RateController hz="1.0">
-        <Fallback>
-          <GoalReached/>
-          <ComputePathToPose goal="${goal}" planner_id="GridBased"/>
-        </Fallback>
-      </RateController>
-      <FollowPath path="${path}" controller_id="FollowPath"/>
-    </Sequence>
-  </BehaviorTree>
-</root>
-```
+The BT Navigator package has three sample XML-based descriptions of BTs.  
+These trees are [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml), [navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml) and [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml).  
+The user may use any of these sample trees or develop a more complex tree which could better suit the user's needs.
+
+### Navigate with Replanning (time-based)
+
+[navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) implements basic navigation by continuously computing and updating the path at a rate of 1Hz. The default controller, the nav2_dwb_controller, implements path following at a rate of 10Hz.
+
+![Navigation with time based replanning](./doc/navigate_w_replanning_time.png)
 
-Navigate with replanning is composed of the following custom decorator, condition,
-contro and action nodes:
+### Navigate with Replanning (distace-based)
+
+[navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml) implements basic navigation by continuously computing and updating the path after every 1 meter distance traveled by the robot.
+
+![Navigation with distance based replanning](./doc/navigate_w_replanning_distance.png)
+
+#### Navigate with replanning is composed of the following custom decorator, condition, control and action nodes:
 
 #### Control Nodes
 * PipelineSequence: This is a variant of a Sequence Node. When this node is ticked,
@@ -69,6 +62,8 @@ returns FAILURE, all nodes are halted and this node returns FAILURE.
 #### Decorator Nodes
 * RateController: A custom control flow node, which throttles down the tick rate.  This custom node has only one child and its tick rate is defined with a pre-defined frequency that the user can set.  This node returns RUNNING when it is not ticking its child. Currently, in the navigation, the `RateController` is used to tick the  `ComputePathToPose` and `GoalReached` node at 1 Hz.
 
+* DistanceController: A custom control flow node, which controls the tick rate based on the distance traveled.  This custom node has only one child. The user can set the distance after which the planner should replan a new path.  This node returns RUNNING when it is not ticking its child. Currently, in navigation, the `DistanceController` is used to tick the  `ComputePathToPose` and `GoalReached` node after every 0.5 meters.
+
 #### Condition Nodes
 * GoalReached: Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, which in that case the `ComputePathToPose` action node will not get ticked.
 
@@ -86,7 +81,7 @@ The navigate with replanning BT first ticks the `RateController` node which spec
 
 ### Navigate with replanning and simple recovery actions
 
-With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning.xml](behavior_trees/navigate_w_replanning.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are:  `clearEntirelyCostmapServiceRequest` for both global and local costmaps, `spin` and `wait`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below.
+With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are:  `clearEntirelyCostmapServiceRequest` for both global and local costmaps, `spin` and `wait`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below.
 
 <p align="center">
 <img src="./doc/parallel_w_recovery.png" title="" width="95%">

nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml

+<!--
+  This Behavior Tree replans the global path after every 1m.
+-->
+
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <PipelineSequence name="NavigateWithReplanning">
+      <DistanceController distance="1.0">
+        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+      </DistanceController>
+      <FollowPath path="{path}"  controller_id="FollowPath"/>
+    </PipelineSequence>
+  </BehaviorTree>
+</root>

nav2_bt_navigator/src/bt_navigator.cpp

@@ -45,6 +45,7 @@ BtNavigator::BtNavigator()
     "nav2_goal_updated_condition_bt_node",
     "nav2_reinitialize_global_localization_service_bt_node",
     "nav2_rate_controller_bt_node",
+    "nav2_distance_controller_bt_node",
     "nav2_recovery_node_bt_node",
     "nav2_pipeline_sequence_bt_node",
     "nav2_round_robin_node_bt_node",

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -15,6 +15,8 @@
 #ifndef NAV2_UTIL__GEOMETRY_UTILS_HPP_
 #define NAV2_UTIL__GEOMETRY_UTILS_HPP_
 
+#include <cmath>
+
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 
@@ -30,6 +32,16 @@ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle)
   return tf2::toMsg(q);
 }
 
+inline double euclideanDistance(
+  const geometry_msgs::msg::Point pos1,
+  const geometry_msgs::msg::Point pos2)
+{
+  const double dx = pos1.x - pos2.x;
+  const double dy = pos1.y - pos2.y;
+  const double dz = pos1.z - pos2.z;
+  return std::sqrt(dx * dx + dy * dy + dz * dz);
+}
+
 }  // namespace geometry_utils
 }  // namespace nav2_util
 

tools/bt2img.py

@@ -37,6 +37,7 @@ control_nodes = [
     "Subtree",
     "Timeout",
     "RateController",
+    "DistanceController",
     "RecoveryNode",
     "PipelineSequence",
     "RoundRobin"