Add condition nodes for time and distance replanning (#1705)

* Add condition nodes and behavior tree to enable replan on new goal

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

* Fix time expired and distance traveled conditions

* Remove new_goal_received from blackboard

* Fix IDLE check condition in new condition nodes

* Fix lint errors

* Fix lint errors

* Address reviewer's comments

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

* Add tests

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

nav2_behavior_tree/CMakeLists.txt

@@ -80,6 +80,12 @@ list(APPEND plugin_libs nav2_goal_reached_condition_bt_node)
 add_library(nav2_goal_updated_condition_bt_node SHARED plugins/condition/goal_updated_condition.cpp)
 list(APPEND plugin_libs nav2_goal_updated_condition_bt_node)
 
+add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp)
+list(APPEND plugin_libs nav2_time_expired_condition_bt_node)
+
+add_library(nav2_distance_traveled_condition_bt_node SHARED plugins/condition/distance_traveled_condition.cpp)
+list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node)
+
 add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp)
 list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node)
 

nav2_behavior_tree/plugins/decorator/distance_controller.hpp → nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_controller.hpp

@@ -13,8 +13,8 @@
 // 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_
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_
 
 #include <memory>
 #include <string>
@@ -63,4 +63,4 @@ private:
 
 }  // namespace nav2_behavior_tree
 
-#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_traveled_condition.hpp

+// Copyright (c) 2020 Sarthak Mittal
+// Copyright (c) 2019 Intel Corporation
+//
+// 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__DISTANCE_TRAVELED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_
+
+#include <string>
+#include <memory>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_ros/buffer.h"
+
+namespace nav2_behavior_tree
+{
+
+class DistanceTraveledCondition : public BT::ConditionNode
+{
+public:
+  DistanceTraveledCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  DistanceTraveledCondition() = delete;
+
+  BT::NodeStatus tick() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<double>("distance", 1.0, "Distance"),
+      BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
+      BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
+    };
+  }
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+
+  geometry_msgs::msg::PoseStamped start_pose_;
+
+  double distance_;
+  double transform_tolerance_;
+  std::string global_frame_;
+  std::string robot_base_frame_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/time_expired_condition.hpp

+// Copyright (c) 2020 Sarthak Mittal
+// Copyright (c) 2019 Intel Corporation
+//
+// 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__TIME_EXPIRED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_
+
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "behaviortree_cpp_v3/condition_node.h"
+
+namespace nav2_behavior_tree
+{
+
+class TimeExpiredCondition : public BT::ConditionNode
+{
+public:
+  TimeExpiredCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  TimeExpiredCondition() = delete;
+
+  BT::NodeStatus tick() override;
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<double>("seconds", 1.0, "Seconds")
+    };
+  }
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Time start_;
+  double period_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_

nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp

+// Copyright (c) 2019 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__DISTANCE_TRAVELED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_
+
+#include <string>
+#include <memory>
+
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "nav2_behavior_tree/plugins/distance_traveled_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+DistanceTraveledCondition::DistanceTraveledCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  distance_(1.0),
+  transform_tolerance_(0.1),
+  global_frame_("map"),
+  robot_base_frame_("base_link")
+{
+  getInput("distance", distance_);
+  getInput("global_frame", global_frame_);
+  getInput("robot_base_frame", robot_base_frame_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
+}
+
+BT::NodeStatus DistanceTraveledCondition::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    if (!nav2_util::getCurrentPose(
+        start_pose_, *tf_, global_frame_, robot_base_frame_,
+        transform_tolerance_))
+    {
+      RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+    }
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Determine distance travelled since we've started this iteration
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Get euclidean distance
+  auto travelled = nav2_util::geometry_utils::euclidean_distance(
+    start_pose_.pose, current_pose.pose);
+
+  if (travelled < distance_) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Update start pose
+  start_pose_ = current_pose;
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::DistanceTraveledCondition>("DistanceTraveled");
+}
+
+#endif  // NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_

nav2_behavior_tree/plugins/condition/time_expired_condition.cpp

+// Copyright (c) 2019 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__TIME_EXPIRED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_
+
+#include <string>
+#include <memory>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+
+#include "nav2_behavior_tree/plugins/time_expired_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+TimeExpiredCondition::TimeExpiredCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  period_(1.0)
+{
+  getInput("seconds", period_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  start_ = node_->now();
+}
+
+BT::NodeStatus TimeExpiredCondition::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    start_ = node_->now();
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Determine how long its been since we've started this iteration
+  auto elapsed = node_->now() - start_;
+
+  // Now, get that in seconds
+  auto seconds = elapsed.seconds();
+
+  if (seconds < period_) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  start_ = node_->now();  // Reset the timer
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::TimeExpiredCondition>("TimeExpired");
+}
+
+#endif  // NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_

nav2_behavior_tree/plugins/decorator/distance_controller.cpp

@@ -25,7 +25,7 @@
 
 #include "behaviortree_cpp_v3/decorator_node.h"
 
-#include "distance_controller.hpp"
+#include "nav2_behavior_tree/plugins/distance_controller.hpp"
 
 namespace nav2_behavior_tree
 {

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}
 )
+
+ament_add_gtest(test_condition_distance_traveled
+  plugins/condition/test_distance_traveled.cpp
+)
+target_link_libraries(test_condition_distance_traveled
+  nav2_distance_traveled_condition_bt_node
+)
+ament_target_dependencies(test_condition_distance_traveled
+  ${dependencies}
+)
+
+ament_add_gtest(test_condition_time_expired
+  plugins/condition/test_time_expired.cpp
+)
+target_link_libraries(test_condition_time_expired
+  nav2_time_expired_condition_bt_node
+)
+ament_target_dependencies(test_condition_time_expired
+  ${dependencies}
+)

nav2_behavior_tree/test/plugins/condition/test_distance_traveled.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 "nav2_behavior_tree/plugins/distance_traveled_condition.hpp"
+
+class DistanceTraveledConditionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    transform_handler_ = new nav2_behavior_tree::TransformHandler();
+
+    config_ = new BT::NodeConfiguration();
+
+    // 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_;
+    transform_handler_ = nullptr;
+    config_ = nullptr;
+  }
+
+  void SetUp()
+  {
+    node_ = new nav2_behavior_tree::DistanceTraveledCondition("distance_traveled", *config_);
+  }
+
+  void TearDown()
+  {
+    node_ = nullptr;
+  }
+
+protected:
+  static nav2_behavior_tree::TransformHandler * transform_handler_;
+  static BT::NodeConfiguration * config_;
+  static nav2_behavior_tree::DistanceTraveledCondition * node_;
+};
+
+nav2_behavior_tree::TransformHandler * DistanceTraveledConditionTestFixture::transform_handler_ =
+  nullptr;
+BT::NodeConfiguration * DistanceTraveledConditionTestFixture::config_ = nullptr;
+nav2_behavior_tree::DistanceTraveledCondition * DistanceTraveledConditionTestFixture::node_ =
+  nullptr;
+
+TEST_F(DistanceTraveledConditionTestFixture, test_behavior)
+{
+  EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE);
+
+  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.51
+    // we wait for 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 = pose.pose.position.x;
+      }
+    }
+
+    if (i % 2) {
+      EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE);
+    } else {
+      EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
+    }
+  }
+}
+
+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/plugins/condition/test_time_expired.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 <chrono>
+#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 "nav2_behavior_tree/plugins/time_expired_condition.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class TimeExpiredConditionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    transform_handler_ = new nav2_behavior_tree::TransformHandler();
+    config_ = new BT::NodeConfiguration();
+
+    // 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_;
+    transform_handler_ = nullptr;
+    config_ = nullptr;
+  }
+
+  void SetUp()
+  {
+    node_ = new nav2_behavior_tree::TimeExpiredCondition("time_expired", *config_);
+  }
+
+  void TearDown()
+  {
+    node_ = nullptr;
+  }
+
+protected:
+  static nav2_behavior_tree::TransformHandler * transform_handler_;
+  static BT::NodeConfiguration * config_;
+  static nav2_behavior_tree::TimeExpiredCondition * node_;
+};
+
+nav2_behavior_tree::TransformHandler * TimeExpiredConditionTestFixture::transform_handler_ =
+  nullptr;
+BT::NodeConfiguration * TimeExpiredConditionTestFixture::config_ = nullptr;
+nav2_behavior_tree::TimeExpiredCondition * TimeExpiredConditionTestFixture::node_ = nullptr;
+
+TEST_F(TimeExpiredConditionTestFixture, test_behavior)
+{
+  EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE);
+
+  for (int i = 0; i < 20; ++i) {
+    rclcpp::sleep_for(500ms);
+    if (i % 2) {
+      EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
+    } else {
+      EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE);
+    }
+  }
+}
+
+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/plugins/decorator/test_distance_controller.cpp

@@ -24,7 +24,7 @@
 
 #include "../../test_transform_handler.hpp"
 #include "../../test_dummy_tree_node.hpp"
-#include "../../../plugins/decorator/distance_controller.hpp"
+#include "nav2_behavior_tree/plugins/distance_controller.hpp"
 
 class DistanceControllerTestFixture : public ::testing::Test
 {

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -69,6 +69,8 @@ bt_navigator:
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node
     - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
 
 bt_navigator_rclcpp_node:
   ros__parameters:

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -69,6 +69,8 @@ bt_navigator:
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node
     - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
 
 bt_navigator_rclcpp_node:
   ros__parameters:

nav2_bringup/bringup/params/nav2_params.yaml

@@ -70,6 +70,8 @@ bt_navigator:
     - nav2_pipeline_sequence_bt_node
     - nav2_round_robin_node_bt_node
     - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
 
 bt_navigator_rclcpp_node:
   ros__parameters:

nav2_bt_navigator/src/bt_navigator.cpp

@@ -20,6 +20,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <set>
 
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
@@ -52,7 +53,9 @@ BtNavigator::BtNavigator()
     "nav2_recovery_node_bt_node",
     "nav2_pipeline_sequence_bt_node",
     "nav2_round_robin_node_bt_node",
-    "nav2_transform_available_condition_bt_node"
+    "nav2_transform_available_condition_bt_node",
+    "nav2_time_expired_condition_bt_node",
+    "nav2_distance_traveled_condition_bt_node"
   };
 
   // Declare this node's parameters