Add DistanceController decorator node

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

@@ -0,0 +1,105 @@
+// 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

@@ -0,0 +1,59 @@
+// 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

@@ -0,0 +1,11 @@
+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

@@ -0,0 +1,145 @@
+// 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;
+}