add humble-navigation2

navigation2/nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -0,0 +1,117 @@
+find_package(test_msgs REQUIRED)
+
+ament_add_gtest(test_bt_action_node test_bt_action_node.cpp)
+ament_target_dependencies(test_bt_action_node ${dependencies} test_msgs)
+
+ament_add_gtest(test_action_spin_action test_spin_action.cpp)
+target_link_libraries(test_action_spin_action nav2_spin_action_bt_node)
+ament_target_dependencies(test_action_spin_action ${dependencies})
+
+ament_add_gtest(test_action_back_up_action test_back_up_action.cpp)
+target_link_libraries(test_action_back_up_action nav2_back_up_action_bt_node)
+ament_target_dependencies(test_action_back_up_action ${dependencies})
+
+ament_add_gtest(test_action_drive_on_heading test_drive_on_heading_action.cpp)
+target_link_libraries(test_action_drive_on_heading nav2_drive_on_heading_bt_node)
+ament_target_dependencies(test_action_drive_on_heading ${dependencies})
+
+ament_add_gtest(test_action_wait_action test_wait_action.cpp)
+target_link_libraries(test_action_wait_action nav2_wait_action_bt_node)
+ament_target_dependencies(test_action_wait_action ${dependencies})
+
+ament_add_gtest(test_action_assisted_teleop_action test_assisted_teleop_action.cpp)
+target_link_libraries(test_action_assisted_teleop_action nav2_assisted_teleop_action_bt_node)
+ament_target_dependencies(test_action_assisted_teleop_action ${dependencies})
+
+ament_add_gtest(test_action_controller_cancel_action test_controller_cancel_node.cpp)
+target_link_libraries(test_action_controller_cancel_action nav2_controller_cancel_bt_node)
+ament_target_dependencies(test_action_controller_cancel_action ${dependencies})
+
+ament_add_gtest(test_action_wait_cancel_action test_wait_cancel_node.cpp)
+target_link_libraries(test_action_wait_cancel_action nav2_wait_cancel_bt_node)
+ament_target_dependencies(test_action_wait_cancel_action ${dependencies})
+
+ament_add_gtest(test_action_spin_cancel_action test_spin_cancel_node.cpp)
+target_link_libraries(test_action_spin_cancel_action nav2_spin_cancel_bt_node)
+ament_target_dependencies(test_action_spin_cancel_action ${dependencies})
+
+
+ament_add_gtest(test_action_back_up_cancel_action test_back_up_cancel_node.cpp)
+target_link_libraries(test_action_back_up_cancel_action nav2_back_up_cancel_bt_node)
+ament_target_dependencies(test_action_back_up_cancel_action ${dependencies})
+
+ament_add_gtest(test_action_assisted_teleop_cancel_action test_assisted_teleop_cancel_node.cpp)
+target_link_libraries(test_action_assisted_teleop_cancel_action nav2_assisted_teleop_cancel_bt_node)
+ament_target_dependencies(test_action_assisted_teleop_cancel_action ${dependencies})
+
+ament_add_gtest(test_action_drive_on_heading_cancel_action test_drive_on_heading_cancel_node.cpp)
+target_link_libraries(test_action_drive_on_heading_cancel_action nav2_drive_on_heading_cancel_bt_node)
+ament_target_dependencies(test_action_drive_on_heading_cancel_action ${dependencies})
+
+ament_add_gtest(test_action_clear_costmap_service test_clear_costmap_service.cpp)
+target_link_libraries(test_action_clear_costmap_service nav2_clear_costmap_service_bt_node)
+ament_target_dependencies(test_action_clear_costmap_service ${dependencies})
+
+ament_add_gtest(test_action_reinitialize_global_localization_service test_reinitialize_global_localization_service.cpp)
+target_link_libraries(test_action_reinitialize_global_localization_service nav2_reinitialize_global_localization_service_bt_node)
+ament_target_dependencies(test_action_reinitialize_global_localization_service ${dependencies})
+
+ament_add_gtest(test_action_compute_path_to_pose_action test_compute_path_to_pose_action.cpp)
+target_link_libraries(test_action_compute_path_to_pose_action nav2_compute_path_to_pose_action_bt_node)
+ament_target_dependencies(test_action_compute_path_to_pose_action ${dependencies})
+
+ament_add_gtest(test_action_compute_path_through_poses_action test_compute_path_through_poses_action.cpp)
+target_link_libraries(test_action_compute_path_through_poses_action nav2_compute_path_through_poses_action_bt_node)
+ament_target_dependencies(test_action_compute_path_through_poses_action ${dependencies})
+
+ament_add_gtest(test_action_smooth_path_action test_smooth_path_action.cpp)
+target_link_libraries(test_action_smooth_path_action nav2_smooth_path_action_bt_node)
+ament_target_dependencies(test_action_smooth_path_action ${dependencies})
+
+ament_add_gtest(test_action_follow_path_action test_follow_path_action.cpp)
+target_link_libraries(test_action_follow_path_action nav2_follow_path_action_bt_node)
+ament_target_dependencies(test_action_follow_path_action ${dependencies})
+
+ament_add_gtest(test_action_navigate_to_pose_action test_navigate_to_pose_action.cpp)
+target_link_libraries(test_action_navigate_to_pose_action nav2_navigate_to_pose_action_bt_node)
+ament_target_dependencies(test_action_navigate_to_pose_action ${dependencies})
+
+ament_add_gtest(test_action_navigate_through_poses_action test_navigate_through_poses_action.cpp)
+target_link_libraries(test_action_navigate_through_poses_action nav2_navigate_through_poses_action_bt_node)
+ament_target_dependencies(test_action_navigate_through_poses_action ${dependencies})
+
+ament_add_gtest(test_truncate_path_action test_truncate_path_action.cpp)
+target_link_libraries(test_truncate_path_action nav2_truncate_path_action_bt_node)
+ament_target_dependencies(test_truncate_path_action ${dependencies})
+
+ament_add_gtest(test_truncate_path_local_action test_truncate_path_local_action.cpp)
+target_link_libraries(test_truncate_path_local_action nav2_truncate_path_local_action_bt_node)
+ament_target_dependencies(test_truncate_path_local_action ${dependencies})
+
+ament_add_gtest(test_remove_passed_goals_action test_remove_passed_goals_action.cpp)
+target_link_libraries(test_remove_passed_goals_action nav2_remove_passed_goals_action_bt_node)
+ament_target_dependencies(test_remove_passed_goals_action ${dependencies})
+
+ament_add_gtest(test_get_pose_from_path_action test_get_pose_from_path_action.cpp)
+target_link_libraries(test_get_pose_from_path_action nav2_get_pose_from_path_action_bt_node)
+ament_target_dependencies(test_get_pose_from_path_action ${dependencies})
+
+ament_add_gtest(test_planner_selector_node test_planner_selector_node.cpp)
+target_link_libraries(test_planner_selector_node nav2_planner_selector_bt_node)
+ament_target_dependencies(test_planner_selector_node ${dependencies})
+
+ament_add_gtest(test_controller_selector_node test_controller_selector_node.cpp)
+target_link_libraries(test_controller_selector_node nav2_controller_selector_bt_node)
+ament_target_dependencies(test_controller_selector_node ${dependencies})
+
+ament_add_gtest(test_smoother_selector_node test_smoother_selector_node.cpp)
+target_link_libraries(test_smoother_selector_node nav2_smoother_selector_bt_node)
+ament_target_dependencies(test_smoother_selector_node ${dependencies})
+
+ament_add_gtest(test_goal_checker_selector_node test_goal_checker_selector_node.cpp)
+target_link_libraries(test_goal_checker_selector_node nav2_goal_checker_selector_bt_node)
+ament_target_dependencies(test_goal_checker_selector_node ${dependencies})
+
+ament_add_gtest(test_progress_checker_selector_node test_progress_checker_selector_node.cpp)
+target_link_libraries(test_progress_checker_selector_node nav2_progress_checker_selector_bt_node)
+ament_target_dependencies(test_progress_checker_selector_node ${dependencies})

navigation2/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp

@@ -0,0 +1,218 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp"
+
+class AssistedTeleopActionServer : public TestActionServer<nav2_msgs::action::AssistedTeleop>
+{
+public:
+  AssistedTeleopActionServer()
+  : TestActionServer("assisted_teleop")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<
+      rclcpp_action::ServerGoalHandle<nav2_msgs::action::AssistedTeleop>>
+    goal_handle)
+  override
+  {
+    nav2_msgs::action::AssistedTeleop::Result::SharedPtr result =
+      std::make_shared<nav2_msgs::action::AssistedTeleop::Result>();
+    bool return_success = getReturnSuccess();
+    if (return_success) {
+      goal_handle->succeed(result);
+    } else {
+      goal_handle->abort(result);
+    }
+  }
+};
+
+class AssistedTeleopActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("backup_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+    config_->blackboard->set<int>("number_recoveries", 0);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::AssistedTeleopAction>(
+          name, "assisted_teleop", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::AssistedTeleopAction>("AssistedTeleop", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void SetUp() override
+  {
+    config_->blackboard->set("number_recoveries", 0);
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<AssistedTeleopActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr AssistedTeleopActionTestFixture::node_ = nullptr;
+std::shared_ptr<AssistedTeleopActionServer>
+AssistedTeleopActionTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * AssistedTeleopActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> AssistedTeleopActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> AssistedTeleopActionTestFixture::tree_ = nullptr;
+
+TEST_F(AssistedTeleopActionTestFixture, test_ports)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <AssistedTeleop />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("time_allowance"), 10.0);
+
+  xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <AssistedTeleop time_allowance="20"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("time_allowance"), 20.0);
+}
+
+TEST_F(AssistedTeleopActionTestFixture, test_tick)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <AssistedTeleop is_recovery="true"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+
+  auto goal = action_server_->getCurrentGoal();
+  EXPECT_EQ(goal->time_allowance.sec, 10.0);
+}
+
+TEST_F(AssistedTeleopActionTestFixture, test_failure)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <AssistedTeleop is_recovery="true"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  action_server_->setReturnSuccess(false);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+
+  auto goal = action_server_->getCurrentGoal();
+  EXPECT_EQ(goal->time_allowance.sec, 10.0);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  AssistedTeleopActionTestFixture::action_server_ = std::make_shared<AssistedTeleopActionServer>();
+  std::thread server_thread([]() {
+      rclcpp::spin(AssistedTeleopActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp

@@ -0,0 +1,176 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp"
+#include "lifecycle_msgs/srv/change_state.hpp"
+
+class CancelAssistedTeleopServer : public TestActionServer<nav2_msgs::action::AssistedTeleop>
+{
+public:
+  CancelAssistedTeleopServer()
+  : TestActionServer("assisted_teleop")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<
+      rclcpp_action::ServerGoalHandle<nav2_msgs::action::AssistedTeleop>>
+    goal_handle)
+  {
+    while (!goal_handle->is_canceling()) {
+      // Assisted Teleop here until goal cancels
+      std::this_thread::sleep_for(std::chrono::milliseconds(15));
+    }
+  }
+};
+
+class CancelAssistedTeleopActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("cancel_back_up_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    client_ = rclcpp_action::create_client<nav2_msgs::action::AssistedTeleop>(
+      node_, "assisted_teleop");
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::AssistedTeleopCancel>(
+          name, "assisted_teleop", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::AssistedTeleopCancel>(
+      "CancelAssistedTeleop", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    client_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<CancelAssistedTeleopServer> action_server_;
+  static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::AssistedTeleop>> client_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr CancelAssistedTeleopActionTestFixture::node_ = nullptr;
+std::shared_ptr<CancelAssistedTeleopServer>
+CancelAssistedTeleopActionTestFixture::action_server_ = nullptr;
+std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::AssistedTeleop>>
+CancelAssistedTeleopActionTestFixture::client_ = nullptr;
+
+BT::NodeConfiguration * CancelAssistedTeleopActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+CancelAssistedTeleopActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> CancelAssistedTeleopActionTestFixture::tree_ = nullptr;
+
+TEST_F(CancelAssistedTeleopActionTestFixture, test_ports)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+             <CancelAssistedTeleop name="AssistedTeleopCancel"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  auto send_goal_options = rclcpp_action::Client<
+    nav2_msgs::action::AssistedTeleop>::SendGoalOptions();
+
+  // Creating a dummy goal_msg
+  auto goal_msg = nav2_msgs::action::AssistedTeleop::Goal();
+
+  // BackUping for server and sending a goal
+  client_->wait_for_action_server();
+  client_->async_send_goal(goal_msg, send_goal_options);
+
+  // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
+  std::this_thread::sleep_for(std::chrono::milliseconds(15));
+
+  // Executing tick
+  tree_->rootNode()->executeTick();
+
+  // BT node should return success, once when the goal is cancelled
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  // Adding another test case to check if the goal is infact cancelling
+  EXPECT_EQ(action_server_->isGoalCancelled(), true);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and back_up on new thread
+  CancelAssistedTeleopActionTestFixture::action_server_ =
+    std::make_shared<CancelAssistedTeleopServer>();
+  std::thread server_thread([]() {
+      rclcpp::spin(CancelAssistedTeleopActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp

@@ -0,0 +1,221 @@
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/back_up_action.hpp"
+
+class BackUpActionServer : public TestActionServer<nav2_msgs::action::BackUp>
+{
+public:
+  BackUpActionServer()
+  : TestActionServer("backup")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>
+    goal_handle)
+  override
+  {
+    nav2_msgs::action::BackUp::Result::SharedPtr result =
+      std::make_shared<nav2_msgs::action::BackUp::Result>();
+    bool return_success = getReturnSuccess();
+    if (return_success) {
+      goal_handle->succeed(result);
+    } else {
+      goal_handle->abort(result);
+    }
+  }
+};
+
+class BackUpActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("backup_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+    config_->blackboard->set<int>("number_recoveries", 0);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::BackUpAction>(
+          name, "backup", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::BackUpAction>("BackUp", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void SetUp() override
+  {
+    config_->blackboard->set("number_recoveries", 0);
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<BackUpActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr BackUpActionTestFixture::node_ = nullptr;
+std::shared_ptr<BackUpActionServer> BackUpActionTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * BackUpActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> BackUpActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> BackUpActionTestFixture::tree_ = nullptr;
+
+TEST_F(BackUpActionTestFixture, test_ports)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <BackUp />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_dist"), 0.15);
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_speed"), 0.025);
+
+  xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <BackUp backup_dist="2" backup_speed="0.26" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_dist"), 2.0);
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_speed"), 0.26);
+}
+
+TEST_F(BackUpActionTestFixture, test_tick)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <BackUp backup_dist="2" backup_speed="0.26" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+
+  auto goal = action_server_->getCurrentGoal();
+  EXPECT_EQ(goal->target.x, 2.0);
+  EXPECT_EQ(goal->speed, 0.26f);
+}
+
+TEST_F(BackUpActionTestFixture, test_failure)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <BackUp backup_dist="2" backup_speed="0.26" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  action_server_->setReturnSuccess(false);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+
+  auto goal = action_server_->getCurrentGoal();
+  EXPECT_EQ(goal->target.x, 2.0);
+  EXPECT_EQ(goal->speed, 0.26f);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  BackUpActionTestFixture::action_server_ = std::make_shared<BackUpActionServer>();
+  std::thread server_thread([]() {
+      rclcpp::spin(BackUpActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp

@@ -0,0 +1,175 @@
+// Copyright (c) 2022 Neobotix GmbH
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp"
+#include "lifecycle_msgs/srv/change_state.hpp"
+
+class CancelBackUpServer : public TestActionServer<nav2_msgs::action::BackUp>
+{
+public:
+  CancelBackUpServer()
+  : TestActionServer("back_up")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>
+    goal_handle)
+  {
+    while (!goal_handle->is_canceling()) {
+      // BackUping here until goal cancels
+      std::this_thread::sleep_for(std::chrono::milliseconds(15));
+    }
+  }
+};
+
+class CancelBackUpActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("cancel_back_up_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    client_ = rclcpp_action::create_client<nav2_msgs::action::BackUp>(
+      node_, "back_up");
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::BackUpCancel>(
+          name, "back_up", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::BackUpCancel>("CancelBackUp", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    client_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<CancelBackUpServer> action_server_;
+  static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::BackUp>> client_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr CancelBackUpActionTestFixture::node_ = nullptr;
+std::shared_ptr<CancelBackUpServer>
+CancelBackUpActionTestFixture::action_server_ = nullptr;
+std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::BackUp>>
+CancelBackUpActionTestFixture::client_ = nullptr;
+
+BT::NodeConfiguration * CancelBackUpActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+CancelBackUpActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> CancelBackUpActionTestFixture::tree_ = nullptr;
+
+TEST_F(CancelBackUpActionTestFixture, test_ports)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+             <CancelBackUp name="BackUpCancel"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::BackUp>::SendGoalOptions();
+
+  // Creating a dummy goal_msg
+  auto goal_msg = nav2_msgs::action::BackUp::Goal();
+
+  // Setting target pose
+  goal_msg.target.x = 0.5;
+
+  // BackUping for server and sending a goal
+  client_->wait_for_action_server();
+  client_->async_send_goal(goal_msg, send_goal_options);
+
+  // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
+  std::this_thread::sleep_for(std::chrono::milliseconds(15));
+
+  // Executing tick
+  tree_->rootNode()->executeTick();
+
+  // BT node should return success, once when the goal is cancelled
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  // Adding another test case to check if the goal is infact cancelling
+  EXPECT_EQ(action_server_->isGoalCancelled(), true);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and back_up on new thread
+  CancelBackUpActionTestFixture::action_server_ = std::make_shared<CancelBackUpServer>();
+  std::thread server_thread([]() {
+      rclcpp::spin(CancelBackUpActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp

@@ -0,0 +1,498 @@
+// 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 <memory>
+#include <set>
+#include <vector>
+#include <string>
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "nav2_behavior_tree/bt_action_node.hpp"
+
+#include "test_msgs/action/fibonacci.hpp"
+
+using namespace std::chrono_literals; // NOLINT
+using namespace std::placeholders;  // NOLINT
+
+class FibonacciActionServer : public rclcpp::Node
+{
+public:
+  FibonacciActionServer()
+  : rclcpp::Node("fibonacci_node", rclcpp::NodeOptions()),
+    sleep_duration_(0ms)
+  {
+    this->action_server_ = rclcpp_action::create_server<test_msgs::action::Fibonacci>(
+      this->get_node_base_interface(),
+      this->get_node_clock_interface(),
+      this->get_node_logging_interface(),
+      this->get_node_waitables_interface(),
+      "fibonacci",
+      std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
+      std::bind(&FibonacciActionServer::handle_cancel, this, _1),
+      std::bind(&FibonacciActionServer::handle_accepted, this, _1));
+  }
+
+  void setHandleGoalSleepDuration(std::chrono::milliseconds sleep_duration)
+  {
+    sleep_duration_ = sleep_duration;
+  }
+
+  void setServerLoopRate(std::chrono::nanoseconds server_loop_rate)
+  {
+    server_loop_rate_ = server_loop_rate;
+  }
+
+protected:
+  rclcpp_action::GoalResponse handle_goal(
+    const rclcpp_action::GoalUUID &,
+    std::shared_ptr<const test_msgs::action::Fibonacci::Goal>)
+  {
+    RCLCPP_INFO(this->get_logger(), "Goal is received..");
+    if (sleep_duration_ > 0ms) {
+      std::this_thread::sleep_for(sleep_duration_);
+    }
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  rclcpp_action::CancelResponse handle_cancel(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>>)
+  {
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  void handle_accepted(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>> handle)
+  {
+    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+    std::thread{std::bind(&FibonacciActionServer::execute, this, _1), handle}.detach();
+  }
+
+  void execute(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>> handle)
+  {
+    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+    if (handle) {
+      const auto goal = handle->get_goal();
+      auto result = std::make_shared<test_msgs::action::Fibonacci::Result>();
+
+      if (goal->order < 0) {
+        handle->abort(result);
+        return;
+      }
+
+      auto & sequence = result->sequence;
+      sequence.push_back(0);
+      sequence.push_back(1);
+
+      rclcpp::Rate rate(server_loop_rate_);
+      for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
+        if (handle->is_canceling()) {
+          RCLCPP_INFO(this->get_logger(), "Goal is canceling.");
+          handle->canceled(result);
+          return;
+        }
+
+        RCLCPP_INFO(this->get_logger(), "Goal is feedbacking.");
+        sequence.push_back(sequence[i] + sequence[i - 1]);
+        rate.sleep();
+      }
+
+      handle->succeed(result);
+    }
+  }
+
+protected:
+  rclcpp_action::Server<test_msgs::action::Fibonacci>::SharedPtr action_server_;
+  std::chrono::milliseconds sleep_duration_;
+  std::chrono::nanoseconds server_loop_rate_;
+};
+
+class FibonacciAction : public nav2_behavior_tree::BtActionNode<test_msgs::action::Fibonacci>
+{
+public:
+  FibonacciAction(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf)
+  : nav2_behavior_tree::BtActionNode<test_msgs::action::Fibonacci>(xml_tag_name, "fibonacci", conf)
+  {}
+
+  void on_tick() override
+  {
+    getInput("order", goal_.order);
+  }
+
+  BT::NodeStatus on_success() override
+  {
+    config().blackboard->set<std::vector<int>>("sequence", result_.result->sequence);
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  BT::NodeStatus on_cancelled() override
+  {
+    config().blackboard->set<std::vector<int>>("sequence", result_.result->sequence);
+    config().blackboard->set<bool>("on_cancelled_triggered", true);
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({BT::InputPort<int>("order", "Fibonacci order")});
+  }
+};
+
+class BTActionNodeTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("bt_action_node_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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", node_);
+    config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 20ms);
+    config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+    config_->blackboard->set<std::chrono::milliseconds>("wait_for_service_timeout", 1000ms);
+    config_->blackboard->set<bool>("initial_pose_received", false);
+    config_->blackboard->set<bool>("on_cancelled_triggered", false);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<FibonacciAction>(name, config);
+      };
+
+    factory_->registerBuilder<FibonacciAction>("Fibonacci", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void SetUp() override
+  {
+    // initialize action server and spin on new thread
+    action_server_ = std::make_shared<FibonacciActionServer>();
+    server_thread_ = std::make_shared<std::thread>(
+      []() {
+        while (rclcpp::ok() && BTActionNodeTestFixture::action_server_ != nullptr) {
+          rclcpp::spin_some(BTActionNodeTestFixture::action_server_);
+          std::this_thread::sleep_for(100ns);
+        }
+      });
+  }
+
+  void TearDown() override
+  {
+    action_server_.reset();
+    tree_.reset();
+    server_thread_->join();
+    server_thread_.reset();
+  }
+
+  static std::shared_ptr<FibonacciActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+  static std::shared_ptr<std::thread> server_thread_;
+};
+
+rclcpp::Node::SharedPtr BTActionNodeTestFixture::node_ = nullptr;
+std::shared_ptr<FibonacciActionServer> BTActionNodeTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * BTActionNodeTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> BTActionNodeTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> BTActionNodeTestFixture::tree_ = nullptr;
+std::shared_ptr<std::thread> BTActionNodeTestFixture::server_thread_ = nullptr;
+
+TEST_F(BTActionNodeTestFixture, test_server_timeout_success)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Fibonacci order="5" />
+        </BehaviorTree>
+      </root>)";
+
+  // the server timeout is larger than the goal handling duration
+  config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 20ms);
+  config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // setting a small action server goal handling duration
+  action_server_->setHandleGoalSleepDuration(2ms);
+  action_server_->setServerLoopRate(10ns);
+
+  // to keep track of the number of ticks it took to reach a terminal result
+  int ticks = 0;
+
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  // BT loop execution rate
+  rclcpp::WallRate loopRate(10ms);
+
+  // main BT execution loop
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
+    result = tree_->tickRoot();
+    ticks++;
+    loopRate.sleep();
+  }
+
+  // get calculated fibonacci sequence from blackboard
+  auto sequence = config_->blackboard->get<std::vector<int>>("sequence");
+
+  // expected fibonacci sequence for order 5
+  std::vector<int> expected = {0, 1, 1, 2, 3, 5};
+
+  // since the server timeout was larger than the action server goal handling duration
+  // the BT should have succeeded
+  EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
+
+  // checking the output fibonacci sequence
+  EXPECT_EQ(sequence.size(), expected.size());
+  for (size_t i = 0; i < expected.size(); ++i) {
+    EXPECT_EQ(sequence[i], expected[i]);
+  }
+
+  // start a new execution cycle with the previous BT to ensure previous state doesn't leak into
+  // the new cycle
+
+  // halt BT for a new execution cycle,
+  // get if the on_cancelled is triggered from blackboard and assert
+  // that the on_cancelled triggers after halting node
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
+  tree_->haltTree();
+  bool on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, false);
+
+  // setting a large action server goal handling duration
+  action_server_->setHandleGoalSleepDuration(100ms);
+  action_server_->setServerLoopRate(10ns);
+
+  // reset state variables
+  ticks = 0;
+  result = BT::NodeStatus::RUNNING;
+  config_->blackboard->set<bool>("on_cancelled_triggered", false);
+
+  // main BT execution loop
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
+    result = tree_->tickRoot();
+    ticks++;
+    loopRate.sleep();
+  }
+
+  // since the server timeout was smaller than the action server goal handling duration
+  // the BT should have failed
+  EXPECT_EQ(result, BT::NodeStatus::FAILURE);
+
+  // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should
+  // be at most 2, but it can be 1 too, because the tickOnce may execute two ticks.
+  EXPECT_LE(ticks, 3);
+  EXPECT_GE(ticks, 1);
+}
+
+TEST_F(BTActionNodeTestFixture, test_server_timeout_failure)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Fibonacci order="2" />
+        </BehaviorTree>
+      </root>)";
+
+  // setting a server timeout smaller than the time the action server will take to accept the goal
+  // to simulate a server timeout scenario
+  config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 90ms);
+  config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // the action server will take 100ms before accepting the goal
+  action_server_->setHandleGoalSleepDuration(100ms);
+  action_server_->setServerLoopRate(10ns);
+
+  // to keep track of the number of ticks it took to reach a terminal result
+  int ticks = 0;
+
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  // BT loop execution rate
+  rclcpp::WallRate loopRate(10ms);
+
+  // main BT execution loop
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
+    result = tree_->tickRoot();
+    ticks++;
+    loopRate.sleep();
+  }
+
+  // since the server timeout was smaller than the action server goal handling duration
+  // the BT should have failed
+  EXPECT_EQ(result, BT::NodeStatus::FAILURE);
+
+  // since the server timeout is 90ms and bt loop duration is 10ms, number of ticks should be 9
+  EXPECT_EQ(ticks, 10);
+
+  // start a new execution cycle with the previous BT to ensure previous state doesn't leak into
+  // the new cycle
+
+  // halt BT for a new execution cycle
+  // get if the on_cancel is triggered from blackboard and assert
+  // that the on_cancelled never can trigger after halting node
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
+  tree_->haltTree();
+  bool on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, false);
+
+  // setting a small action server goal handling duration
+  action_server_->setHandleGoalSleepDuration(25ms);
+  action_server_->setServerLoopRate(10ns);
+
+  // reset state variables
+  ticks = 0;
+  result = BT::NodeStatus::RUNNING;
+  config_->blackboard->set<bool>("on_cancelled_triggered", false);
+
+  // main BT execution loop
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
+    result = tree_->tickRoot();
+    ticks++;
+    loopRate.sleep();
+  }
+
+  // since the server timeout was smaller than the action server goal handling duration
+  // the BT should have failed
+  EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
+}
+
+TEST_F(BTActionNodeTestFixture, test_server_cancel)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Fibonacci order="1000000" />
+        </BehaviorTree>
+      </root>)";
+
+  // setting a server timeout smaller than the time the action server will take to accept the goal
+  // to simulate a server timeout scenario
+  config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 100ms);
+  config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // the action server will take 2ms before accepting the goal
+  // and the feedback period of the action server will be 50ms
+  action_server_->setHandleGoalSleepDuration(2ms);
+  action_server_->setServerLoopRate(50ms);
+
+  // to keep track of the number of ticks it took to reach expected tick count
+  int ticks = 0;
+
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  // BT loop execution rate
+  rclcpp::WallRate loopRate(100ms);
+
+  // main BT execution loop
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 5) {
+    result = tree_->tickRoot();
+    ticks++;
+    loopRate.sleep();
+  }
+
+  // halt BT for testing if the action node cancels the goal correctly
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
+  tree_->haltTree();
+
+  // get if the on_cancel is triggered from blackboard and assert
+  // that the on_cancel is triggered after halting node
+  bool on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, true);
+
+  // ticks variable must be 5 because execution time of the action server
+  // is at least 1000000 x 50 ms
+  EXPECT_EQ(ticks, 5);
+
+  // send new goal to the action server for a new execution cycle
+
+  // the action server will take 2ms before accepting the goal
+  // and the feedback period of the action server will be 1000ms
+  action_server_->setHandleGoalSleepDuration(2ms);
+  action_server_->setServerLoopRate(50ms);
+
+  // reset state variable
+  ticks = 0;
+  config_->blackboard->set<bool>("on_cancelled_triggered", false);
+  result = BT::NodeStatus::RUNNING;
+
+  // main BT execution loop
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 7) {
+    result = tree_->tickRoot();
+    ticks++;
+    loopRate.sleep();
+  }
+
+  // halt BT for testing if the action node cancels the goal correctly
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
+  tree_->haltTree();
+
+  // get if the on_cancel is triggered from blackboard and assert
+  // that the on_cancel is triggered after halting node
+  on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, true);
+
+  // ticks variable must be 7 because execution time of the action server
+  // is at least 1000000 x 50 ms
+  EXPECT_EQ(ticks, 7);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp

@@ -0,0 +1,339 @@
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_service.hpp"
+#include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
+
+class ClearEntireCostmapService : public TestService<nav2_msgs::srv::ClearEntireCostmap>
+{
+public:
+  ClearEntireCostmapService()
+  : TestService("clear_entire_costmap")
+  {}
+};
+
+class ClearEntireCostmapServiceTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("clear_entire_costmap_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+    config_->blackboard->set<int>("number_recoveries", 0);
+
+    factory_->registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>("ClearEntireCostmap");
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    server_.reset();
+    factory_.reset();
+  }
+
+  void SetUp() override
+  {
+    config_->blackboard->set("number_recoveries", 0);
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<ClearEntireCostmapService> server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr ClearEntireCostmapServiceTestFixture::node_ = nullptr;
+std::shared_ptr<ClearEntireCostmapService> ClearEntireCostmapServiceTestFixture::server_ = nullptr;
+BT::NodeConfiguration * ClearEntireCostmapServiceTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> ClearEntireCostmapServiceTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> ClearEntireCostmapServiceTestFixture::tree_ = nullptr;
+
+TEST_F(ClearEntireCostmapServiceTestFixture, test_tick)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <ClearEntireCostmap service_name="clear_entire_costmap"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+}
+
+class ClearCostmapExceptRegionService : public TestService<nav2_msgs::srv::ClearCostmapExceptRegion>
+{
+public:
+  ClearCostmapExceptRegionService()
+  : TestService("clear_costmap_except_region")
+  {}
+};
+
+class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("clear_costmap_except_region_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+    config_->blackboard->set<int>("number_recoveries", 0);
+
+    factory_->registerNodeType<nav2_behavior_tree::ClearCostmapExceptRegionService>(
+      "ClearCostmapExceptRegion");
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    server_.reset();
+    factory_.reset();
+  }
+
+  void SetUp() override
+  {
+    config_->blackboard->set("number_recoveries", 0);
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<ClearCostmapExceptRegionService> server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr
+ClearCostmapExceptRegionServiceTestFixture::node_ = nullptr;
+std::shared_ptr<ClearCostmapExceptRegionService>
+ClearCostmapExceptRegionServiceTestFixture::server_ = nullptr;
+BT::NodeConfiguration
+* ClearCostmapExceptRegionServiceTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+ClearCostmapExceptRegionServiceTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree>
+ClearCostmapExceptRegionServiceTestFixture::tree_ = nullptr;
+
+TEST_F(ClearCostmapExceptRegionServiceTestFixture, test_tick)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <ClearCostmapExceptRegion service_name="clear_costmap_except_region"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+}
+//******************************************
+class ClearCostmapAroundRobotService : public TestService<nav2_msgs::srv::ClearCostmapAroundRobot>
+{
+public:
+  ClearCostmapAroundRobotService()
+  : TestService("clear_costmap_around_robot")
+  {}
+};
+
+class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("clear_costmap_around_robot_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+    config_->blackboard->set<int>("number_recoveries", 0);
+
+    factory_->registerNodeType<nav2_behavior_tree::ClearCostmapAroundRobotService>(
+      "ClearCostmapAroundRobot");
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    server_.reset();
+    factory_.reset();
+  }
+
+  void SetUp() override
+  {
+    config_->blackboard->set("number_recoveries", 0);
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<ClearCostmapAroundRobotService> server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr
+ClearCostmapAroundRobotServiceTestFixture::node_ = nullptr;
+std::shared_ptr<ClearCostmapAroundRobotService>
+ClearCostmapAroundRobotServiceTestFixture::server_ = nullptr;
+BT::NodeConfiguration
+* ClearCostmapAroundRobotServiceTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+ClearCostmapAroundRobotServiceTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree>
+ClearCostmapAroundRobotServiceTestFixture::tree_ = nullptr;
+
+TEST_F(ClearCostmapAroundRobotServiceTestFixture, test_tick)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <ClearCostmapAroundRobot service_name="clear_costmap_around_robot"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize service and spin on new thread
+  ClearEntireCostmapServiceTestFixture::server_ = std::make_shared<ClearEntireCostmapService>();
+  std::thread server_thread([]() {
+      rclcpp::spin(ClearEntireCostmapServiceTestFixture::server_);
+    });
+
+  ClearCostmapExceptRegionServiceTestFixture::server_ =
+    std::make_shared<ClearCostmapExceptRegionService>();
+  std::thread server_thread_except_region([]() {
+      rclcpp::spin(ClearCostmapExceptRegionServiceTestFixture::server_);
+    });
+
+  ClearCostmapAroundRobotServiceTestFixture::server_ =
+    std::make_shared<ClearCostmapAroundRobotService>();
+  std::thread server_thread_around_robot([]() {
+      rclcpp::spin(ClearCostmapAroundRobotServiceTestFixture::server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+  server_thread_except_region.join();
+  server_thread_around_robot.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp

@@ -0,0 +1,275 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2021 Samsung Research America
+//
+// 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 <memory>
+#include <set>
+#include <string>
+#include <vector>
+
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp"
+
+class ComputePathThroughPosesActionServer
+  : public TestActionServer<nav2_msgs::action::ComputePathThroughPoses>
+{
+public:
+  ComputePathThroughPosesActionServer()
+  : TestActionServer("compute_path_through_poses")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<
+      rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathThroughPoses>> goal_handle)
+  override
+  {
+    const auto goal = goal_handle->get_goal();
+    auto result = std::make_shared<nav2_msgs::action::ComputePathThroughPoses::Result>();
+    result->path.poses.resize(2);
+    result->path.poses[1].pose.position.x = goal->goals[0].pose.position.x;
+    if (goal->use_start) {
+      result->path.poses[0].pose.position.x = goal->start.pose.position.x;
+    } else {
+      result->path.poses[0].pose.position.x = 0.0;
+    }
+    goal_handle->succeed(result);
+  }
+};
+
+class ComputePathThroughPosesActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("compute_path_through_poses_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::ComputePathThroughPosesAction>(
+          name, "compute_path_through_poses", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::ComputePathThroughPosesAction>(
+      "ComputePathThroughPoses", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<ComputePathThroughPosesActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr ComputePathThroughPosesActionTestFixture::node_ = nullptr;
+std::shared_ptr<ComputePathThroughPosesActionServer>
+ComputePathThroughPosesActionTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * ComputePathThroughPosesActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> ComputePathThroughPosesActionTestFixture::factory_ =
+  nullptr;
+std::shared_ptr<BT::Tree> ComputePathThroughPosesActionTestFixture::tree_ = nullptr;
+
+TEST_F(ComputePathThroughPosesActionTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  std::vector<geometry_msgs::msg::PoseStamped> goals;
+  goals.resize(1);
+  goals[0].pose.position.x = 1.0;
+  config_->blackboard->set("goals", goals);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // the goal should have reached our server
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->getInput<std::string>("planner_id"), std::string("GridBased"));
+  EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, 1.0);
+  EXPECT_FALSE(action_server_->getCurrentGoal()->use_start);
+  EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased"));
+
+  // check if returned path is correct
+  nav_msgs::msg::Path path;
+  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, 0.0);
+  EXPECT_EQ(path.poses[1].pose.position.x, 1.0);
+
+  // halt node so another goal can be sent
+  tree_->rootNode()->halt();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
+
+  // set new goal
+  goals[0].pose.position.x = -2.5;
+  config_->blackboard->set("goals", goals);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5);
+
+  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, 0.0);
+  EXPECT_EQ(path.poses[1].pose.position.x, -2.5);
+}
+
+TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <ComputePathThroughPoses goals="{goals}" start="{start}" path="{path}" planner_id="GridBased"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new start and set it on blackboard
+  geometry_msgs::msg::PoseStamped start;
+  start.header.stamp = node_->now();
+  start.pose.position.x = 2.0;
+  config_->blackboard->set("start", start);
+
+  // create new goal and set it on blackboard
+  std::vector<geometry_msgs::msg::PoseStamped> goals;
+  goals.resize(1);
+  goals[0].pose.position.x = 1.0;
+  config_->blackboard->set("goals", goals);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // the goal should have reached our server
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->getInput<std::string>("planner_id"), std::string("GridBased"));
+  EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, 1.0);
+  EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0);
+  EXPECT_TRUE(action_server_->getCurrentGoal()->use_start);
+  EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased"));
+
+  // check if returned path is correct
+  nav_msgs::msg::Path path;
+  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, 2.0);
+  EXPECT_EQ(path.poses[1].pose.position.x, 1.0);
+
+  // halt node so another goal can be sent
+  tree_->rootNode()->halt();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
+
+  // set new goal and new start
+  goals[0].pose.position.x = -2.5;
+  start.pose.position.x = -1.5;
+  config_->blackboard->set("goals", goals);
+  config_->blackboard->set("start", start);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5);
+
+  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, -1.5);
+  EXPECT_EQ(path.poses[1].pose.position.x, -2.5);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  ComputePathThroughPosesActionTestFixture::action_server_ =
+    std::make_shared<ComputePathThroughPosesActionServer>();
+
+  std::thread server_thread([]() {
+      rclcpp::spin(ComputePathThroughPosesActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp

@@ -0,0 +1,272 @@
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp"
+
+class ComputePathToPoseActionServer : public TestActionServer<nav2_msgs::action::ComputePathToPose>
+{
+public:
+  ComputePathToPoseActionServer()
+  : TestActionServer("compute_path_to_pose")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<
+      rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose>> goal_handle)
+  override
+  {
+    const auto goal = goal_handle->get_goal();
+    auto result = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>();
+    result->path.poses.resize(2);
+    result->path.poses[1].pose.position.x = goal->goal.pose.position.x;
+    if (goal->use_start) {
+      result->path.poses[0].pose.position.x = goal->start.pose.position.x;
+    } else {
+      result->path.poses[0].pose.position.x = 0.0;
+    }
+    goal_handle->succeed(result);
+  }
+};
+
+class ComputePathToPoseActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("compute_path_to_pose_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::ComputePathToPoseAction>(
+          name, "compute_path_to_pose", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::ComputePathToPoseAction>(
+      "ComputePathToPose", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<ComputePathToPoseActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr ComputePathToPoseActionTestFixture::node_ = nullptr;
+std::shared_ptr<ComputePathToPoseActionServer>
+ComputePathToPoseActionTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * ComputePathToPoseActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> ComputePathToPoseActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> ComputePathToPoseActionTestFixture::tree_ = nullptr;
+
+TEST_F(ComputePathToPoseActionTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  geometry_msgs::msg::PoseStamped goal;
+  goal.header.stamp = node_->now();
+  goal.pose.position.x = 1.0;
+  config_->blackboard->set("goal", goal);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // the goal should have reached our server
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->getInput<std::string>("planner_id"), std::string("GridBased"));
+  EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0);
+  EXPECT_FALSE(action_server_->getCurrentGoal()->use_start);
+  EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased"));
+
+  // check if returned path is correct
+  nav_msgs::msg::Path path;
+  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, 0.0);
+  EXPECT_EQ(path.poses[1].pose.position.x, 1.0);
+
+  // halt node so another goal can be sent
+  tree_->rootNode()->halt();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
+
+  // set new goal
+  goal.pose.position.x = -2.5;
+  config_->blackboard->set("goal", goal);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5);
+
+  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, 0.0);
+  EXPECT_EQ(path.poses[1].pose.position.x, -2.5);
+}
+
+TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <ComputePathToPose goal="{goal}" start="{start}" path="{path}" planner_id="GridBased"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new start and set it on blackboard
+  geometry_msgs::msg::PoseStamped start;
+  start.header.stamp = node_->now();
+  start.pose.position.x = 2.0;
+  config_->blackboard->set("start", start);
+
+  // create new goal and set it on blackboard
+  geometry_msgs::msg::PoseStamped goal;
+  goal.header.stamp = node_->now();
+  goal.pose.position.x = 1.0;
+  config_->blackboard->set("goal", goal);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // the goal should have reached our server
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->getInput<std::string>("planner_id"), std::string("GridBased"));
+  EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0);
+  EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0);
+  EXPECT_TRUE(action_server_->getCurrentGoal()->use_start);
+  EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased"));
+
+  // check if returned path is correct
+  nav_msgs::msg::Path path;
+  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, 2.0);
+  EXPECT_EQ(path.poses[1].pose.position.x, 1.0);
+
+  // halt node so another goal can be sent
+  tree_->rootNode()->halt();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
+
+  // set new goal and new start
+  goal.pose.position.x = -2.5;
+  start.pose.position.x = -1.5;
+  config_->blackboard->set("goal", goal);
+  config_->blackboard->set("start", start);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5);
+
+  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, -1.5);
+  EXPECT_EQ(path.poses[1].pose.position.x, -2.5);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  ComputePathToPoseActionTestFixture::action_server_ =
+    std::make_shared<ComputePathToPoseActionServer>();
+
+  std::thread server_thread([]() {
+      rclcpp::spin(ComputePathToPoseActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp

@@ -0,0 +1,172 @@
+// Copyright (c) 2022 Neobotix GmbH
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/controller_cancel_node.hpp"
+#include "lifecycle_msgs/srv/change_state.hpp"
+
+class CancelControllerServer : public TestActionServer<nav2_msgs::action::FollowPath>
+{
+public:
+  CancelControllerServer()
+  : TestActionServer("follow_path")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath>>
+    goal_handle)
+  {
+    while (!goal_handle->is_canceling()) {
+      // waiting here until goal cancels
+      std::this_thread::sleep_for(std::chrono::milliseconds(15));
+    }
+  }
+};
+
+class CancelControllerActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("cancel_control_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    client_ = rclcpp_action::create_client<nav2_msgs::action::FollowPath>(
+      node_, "follow_path");
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::ControllerCancel>(
+          name, "follow_path", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::ControllerCancel>("CancelControl", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    client_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<CancelControllerServer> action_server_;
+  static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::FollowPath>> client_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr CancelControllerActionTestFixture::node_ = nullptr;
+std::shared_ptr<CancelControllerServer>
+CancelControllerActionTestFixture::action_server_ = nullptr;
+std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::FollowPath>>
+CancelControllerActionTestFixture::client_ = nullptr;
+
+BT::NodeConfiguration * CancelControllerActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+CancelControllerActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> CancelControllerActionTestFixture::tree_ = nullptr;
+
+TEST_F(CancelControllerActionTestFixture, test_ports)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+             <CancelControl name="ControlCancel"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::FollowPath>::SendGoalOptions();
+
+  // Creating a dummy goal_msg
+  auto goal_msg = nav2_msgs::action::FollowPath::Goal();
+
+  // Waiting for server and sending a goal
+  client_->wait_for_action_server();
+  client_->async_send_goal(goal_msg, send_goal_options);
+
+  // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
+  std::this_thread::sleep_for(std::chrono::milliseconds(15));
+
+  // Executing tick
+  tree_->rootNode()->executeTick();
+
+  // BT node should return success, once when the goal is cancelled
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  // Adding another test case to check if the goal is infact cancelling
+  EXPECT_EQ(action_server_->isGoalCancelled(), true);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  CancelControllerActionTestFixture::action_server_ = std::make_shared<CancelControllerServer>();
+  std::thread server_thread([]() {
+      rclcpp::spin(CancelControllerActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp

@@ -0,0 +1,187 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Pablo Iñigo Blasco
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "../../test_action_server.hpp"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "std_msgs/msg/string.hpp"
+
+class ControllerSelectorTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("controller_selector_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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", node_);
+
+    BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
+        return std::make_unique<nav2_behavior_tree::ControllerSelector>(name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::ControllerSelector>(
+      "ControllerSelector",
+      builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr ControllerSelectorTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * ControllerSelectorTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> ControllerSelectorTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> ControllerSelectorTestFixture::tree_ = nullptr;
+
+TEST_F(ControllerSelectorTestFixture, test_custom_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <ControllerSelector selected_controller="{selected_controller}" default_controller="DWB" topic_name="controller_selector_custom_topic_name"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_controller_result;
+  config_->blackboard->get("selected_controller", selected_controller_result);
+
+  EXPECT_EQ(selected_controller_result, "DWB");
+
+  std_msgs::msg::String selected_controller_cmd;
+
+  selected_controller_cmd.data = "DWC";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto controller_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("controller_selector_custom_topic_name", qos);
+
+  // publish a few updates of the selected_controller
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    controller_selector_pub->publish(selected_controller_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check controller updated
+  config_->blackboard->get("selected_controller", selected_controller_result);
+  EXPECT_EQ("DWC", selected_controller_result);
+}
+
+TEST_F(ControllerSelectorTestFixture, test_default_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <ControllerSelector selected_controller="{selected_controller}" default_controller="GridBased"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_controller_result;
+  config_->blackboard->get("selected_controller", selected_controller_result);
+
+  EXPECT_EQ(selected_controller_result, "GridBased");
+
+  std_msgs::msg::String selected_controller_cmd;
+
+  selected_controller_cmd.data = "RRT";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto controller_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("controller_selector", qos);
+
+  // publish a few updates of the selected_controller
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    controller_selector_pub->publish(selected_controller_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check controller updated
+  config_->blackboard->get("selected_controller", selected_controller_result);
+  EXPECT_EQ("RRT", selected_controller_result);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp

@@ -0,0 +1,214 @@
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp"
+
+class DriveOnHeadingActionServer : public TestActionServer<nav2_msgs::action::DriveOnHeading>
+{
+public:
+  DriveOnHeadingActionServer()
+  : TestActionServer("drive_on_heading")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle
+    <nav2_msgs::action::DriveOnHeading>>
+    goal_handle)
+  override
+  {
+    nav2_msgs::action::DriveOnHeading::Result::SharedPtr result =
+      std::make_shared<nav2_msgs::action::DriveOnHeading::Result>();
+    bool return_success = getReturnSuccess();
+    if (return_success) {
+      goal_handle->succeed(result);
+    } else {
+      goal_handle->abort(result);
+    }
+  }
+};
+
+class DriveOnHeadingActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("drive_on_heading_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::DriveOnHeadingAction>(
+          name, "drive_on_heading", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::DriveOnHeadingAction>("DriveOnHeading", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<DriveOnHeadingActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr DriveOnHeadingActionTestFixture::node_ = nullptr;
+std::shared_ptr<DriveOnHeadingActionServer>
+DriveOnHeadingActionTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * DriveOnHeadingActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> DriveOnHeadingActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> DriveOnHeadingActionTestFixture::tree_ = nullptr;
+
+TEST_F(DriveOnHeadingActionTestFixture, test_ports)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <DriveOnHeading />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("dist_to_travel"), 0.15);
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("speed"), 0.025);
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("time_allowance"), 10.0);
+
+  xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <DriveOnHeading dist_to_travel="2" speed="0.26" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("dist_to_travel"), 2.0);
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("speed"), 0.26);
+}
+
+TEST_F(DriveOnHeadingActionTestFixture, test_tick)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <DriveOnHeading dist_to_travel="2" speed="0.26" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  auto goal = action_server_->getCurrentGoal();
+  EXPECT_EQ(goal->target.x, 2.0);
+  EXPECT_EQ(goal->speed, 0.26f);
+}
+
+TEST_F(DriveOnHeadingActionTestFixture, test_failure)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <DriveOnHeading dist_to_travel="2" speed="0.26" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  action_server_->setReturnSuccess(false);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+
+  auto goal = action_server_->getCurrentGoal();
+  EXPECT_EQ(goal->target.x, 2.0);
+  EXPECT_EQ(goal->speed, 0.26f);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  DriveOnHeadingActionTestFixture::action_server_ = std::make_shared<DriveOnHeadingActionServer>();
+  std::thread server_thread([]() {
+      rclcpp::spin(DriveOnHeadingActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp

@@ -0,0 +1,179 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp"
+#include "lifecycle_msgs/srv/change_state.hpp"
+
+class CancelDriveOnHeadingServer : public TestActionServer<nav2_msgs::action::DriveOnHeading>
+{
+public:
+  CancelDriveOnHeadingServer()
+  : TestActionServer("drive_on_heading_cancel")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle
+    <nav2_msgs::action::DriveOnHeading>>
+    goal_handle)
+  {
+    while (!goal_handle->is_canceling()) {
+      // DriveOnHeadingCancel here until goal cancels
+      std::this_thread::sleep_for(std::chrono::milliseconds(15));
+    }
+  }
+};
+
+class CancelDriveOnHeadingTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("cancel_drive_on_heading_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    client_ = rclcpp_action::create_client<nav2_msgs::action::DriveOnHeading>(
+      node_, "drive_on_heading_cancel");
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::DriveOnHeadingCancel>(
+          name, "drive_on_heading_cancel", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::DriveOnHeadingCancel>(
+      "CancelDriveOnHeading",
+      builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    client_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<CancelDriveOnHeadingServer> action_server_;
+  static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::DriveOnHeading>> client_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr CancelDriveOnHeadingTestFixture::node_ = nullptr;
+std::shared_ptr<CancelDriveOnHeadingServer>
+CancelDriveOnHeadingTestFixture::action_server_ = nullptr;
+std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::DriveOnHeading>>
+CancelDriveOnHeadingTestFixture::client_ = nullptr;
+
+BT::NodeConfiguration * CancelDriveOnHeadingTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+CancelDriveOnHeadingTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> CancelDriveOnHeadingTestFixture::tree_ = nullptr;
+
+TEST_F(CancelDriveOnHeadingTestFixture, test_ports)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+             <CancelDriveOnHeading name="CancelDriveOnHeading"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  auto send_goal_options = rclcpp_action::Client
+    <nav2_msgs::action::DriveOnHeading>::SendGoalOptions();
+
+  // Creating a dummy goal_msg
+  auto goal_msg = nav2_msgs::action::DriveOnHeading::Goal();
+
+  // Setting target pose
+  goal_msg.target.x = 0.5;
+
+  // DriveOnHeadingCancel for server and sending a goal
+  client_->wait_for_action_server();
+  client_->async_send_goal(goal_msg, send_goal_options);
+
+  // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
+  std::this_thread::sleep_for(std::chrono::milliseconds(15));
+
+  // Executing tick
+  tree_->rootNode()->executeTick();
+
+  // BT node should return success, once when the goal is cancelled
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  // Adding another test case to check if the goal is infact cancelling
+  EXPECT_EQ(action_server_->isGoalCancelled(), true);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and drive on new thread
+  CancelDriveOnHeadingTestFixture::action_server_ = std::make_shared<CancelDriveOnHeadingServer>();
+  std::thread server_thread([]() {
+      rclcpp::spin(CancelDriveOnHeadingTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp

@@ -0,0 +1,186 @@
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/follow_path_action.hpp"
+
+class FollowPathActionServer : public TestActionServer<nav2_msgs::action::FollowPath>
+{
+public:
+  FollowPathActionServer()
+  : TestActionServer("follow_path")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<
+      rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath>> goal_handle)
+  override
+  {
+    const auto goal = goal_handle->get_goal();
+    auto result = std::make_shared<nav2_msgs::action::FollowPath::Result>();
+    goal_handle->succeed(result);
+  }
+};
+
+class FollowPathActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("follow_path_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::FollowPathAction>(
+          name, "follow_path", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::FollowPathAction>(
+      "FollowPath", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<FollowPathActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr FollowPathActionTestFixture::node_ = nullptr;
+std::shared_ptr<FollowPathActionServer>
+FollowPathActionTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * FollowPathActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> FollowPathActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> FollowPathActionTestFixture::tree_ = nullptr;
+
+TEST_F(FollowPathActionTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <FollowPath path="{path}" controller_id="FollowPath"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // set new path on blackboard
+  nav_msgs::msg::Path path;
+  path.poses.resize(1);
+  path.poses[0].pose.position.x = 1.0;
+  config_->blackboard->set<nav_msgs::msg::Path>("path", path);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // the goal should have reached our server
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->getInput<std::string>("controller_id"), std::string("FollowPath"));
+  EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u);
+  EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, 1.0);
+  EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath"));
+
+  // halt node so another goal can be sent
+  tree_->rootNode()->halt();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
+
+  // set new goal
+  path.poses[0].pose.position.x = -2.5;
+  config_->blackboard->set<nav_msgs::msg::Path>("path", path);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u);
+  EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, -2.5);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  FollowPathActionTestFixture::action_server_ =
+    std::make_shared<FollowPathActionServer>();
+
+  std::thread server_thread([]() {
+      rclcpp::spin(FollowPathActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp

@@ -0,0 +1,156 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2021 Samsung Research America
+// Copyright (c) 2024 Marc Morcos
+//
+// 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 <memory>
+#include <set>
+#include <string>
+#include <vector>
+
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp"
+#include "../../test_behavior_tree_fixture.hpp"
+
+class GetPoseFromPathTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("get_pose_from_path_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::GetPoseFromPath>(
+          name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::GetPoseFromPath>(
+      "GetPoseFromPath", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr GetPoseFromPathTestFixture::node_ = nullptr;
+BT::NodeConfiguration * GetPoseFromPathTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> GetPoseFromPathTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> GetPoseFromPathTestFixture::tree_ = nullptr;
+
+TEST_F(GetPoseFromPathTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <GetPoseFromPath path="{path}" pose="{pose}" index="{index}" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new path and set it on blackboard
+  nav_msgs::msg::Path path;
+  std::vector<geometry_msgs::msg::PoseStamped> goals;
+  goals.resize(2);
+  goals[0].pose.position.x = 1.0;
+  goals[1].pose.position.x = 2.0;
+  path.poses = goals;
+  path.header.frame_id = "test_frame_1";
+  config_->blackboard->set("path", path);
+
+  config_->blackboard->set("index", 0);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // the goal should have reached our server
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  // check if returned pose is correct
+  geometry_msgs::msg::PoseStamped pose;
+  EXPECT_TRUE(config_->blackboard->get<geometry_msgs::msg::PoseStamped>("pose", pose));
+  EXPECT_EQ(pose.header.frame_id, "test_frame_1");
+  EXPECT_EQ(pose.pose.position.x, 1.0);
+
+  // halt node so another goal can be sent
+  tree_->haltTree();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
+
+  // try last element
+  config_->blackboard->set("index", -1);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  // check if returned pose is correct
+  EXPECT_TRUE(config_->blackboard->get<geometry_msgs::msg::PoseStamped>("pose", pose));
+  EXPECT_EQ(pose.header.frame_id, "test_frame_1");
+  EXPECT_EQ(pose.pose.position.x, 2.0);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp

@@ -0,0 +1,187 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Pablo Iñigo Blasco
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "../../test_action_server.hpp"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "std_msgs/msg/string.hpp"
+
+class GoalCheckerSelectorTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("goal_checker_selector_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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", node_);
+
+    BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
+        return std::make_unique<nav2_behavior_tree::GoalCheckerSelector>(name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::GoalCheckerSelector>(
+      "GoalCheckerSelector",
+      builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr GoalCheckerSelectorTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * GoalCheckerSelectorTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> GoalCheckerSelectorTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> GoalCheckerSelectorTestFixture::tree_ = nullptr;
+
+TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="SimpleGoalCheck" topic_name="goal_checker_selector_custom_topic_name"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_goal_checker_result;
+  config_->blackboard->get("selected_goal_checker", selected_goal_checker_result);
+
+  EXPECT_EQ(selected_goal_checker_result, "SimpleGoalCheck");
+
+  std_msgs::msg::String selected_goal_checker_cmd;
+
+  selected_goal_checker_cmd.data = "AngularGoalChecker";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto goal_checker_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("goal_checker_selector_custom_topic_name", qos);
+
+  // publish a few updates of the selected_goal_checker
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    goal_checker_selector_pub->publish(selected_goal_checker_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check goal_checker updated
+  config_->blackboard->get("selected_goal_checker", selected_goal_checker_result);
+  EXPECT_EQ("AngularGoalChecker", selected_goal_checker_result);
+}
+
+TEST_F(GoalCheckerSelectorTestFixture, test_default_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="GridBased"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_goal_checker_result;
+  config_->blackboard->get("selected_goal_checker", selected_goal_checker_result);
+
+  EXPECT_EQ(selected_goal_checker_result, "GridBased");
+
+  std_msgs::msg::String selected_goal_checker_cmd;
+
+  selected_goal_checker_cmd.data = "RRT";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto goal_checker_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("goal_checker_selector", qos);
+
+  // publish a few updates of the selected_goal_checker
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    goal_checker_selector_pub->publish(selected_goal_checker_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check goal_checker updated
+  config_->blackboard->get("selected_goal_checker", selected_goal_checker_result);
+  EXPECT_EQ("RRT", selected_goal_checker_result);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp

@@ -0,0 +1,177 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2021 Samsung Research America
+//
+// 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 <memory>
+#include <set>
+#include <string>
+#include <vector>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp"
+
+class NavigateThroughPosesActionServer
+  : public TestActionServer<nav2_msgs::action::NavigateThroughPoses>
+{
+public:
+  NavigateThroughPosesActionServer()
+  : TestActionServer("navigate_through_poses")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<
+      rclcpp_action::ServerGoalHandle<nav2_msgs::action::NavigateThroughPoses>> goal_handle)
+  override
+  {
+    const auto goal = goal_handle->get_goal();
+    auto result = std::make_shared<nav2_msgs::action::NavigateThroughPoses::Result>();
+    goal_handle->succeed(result);
+  }
+};
+
+class NavigateThroughPosesActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("navigate_through_poses_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+    std::vector<geometry_msgs::msg::PoseStamped> poses;
+    config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>(
+      "goals", poses);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::NavigateThroughPosesAction>(
+          name, "navigate_through_poses", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::NavigateThroughPosesAction>(
+      "NavigateThroughPoses", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<NavigateThroughPosesActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr NavigateThroughPosesActionTestFixture::node_ = nullptr;
+std::shared_ptr<NavigateThroughPosesActionServer>
+NavigateThroughPosesActionTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * NavigateThroughPosesActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> NavigateThroughPosesActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> NavigateThroughPosesActionTestFixture::tree_ = nullptr;
+
+TEST_F(NavigateThroughPosesActionTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <NavigateThroughPoses goals="{goals}" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  std::vector<geometry_msgs::msg::PoseStamped> poses;
+  poses.resize(1);
+  poses[0].pose.position.x = -2.5;
+  poses[0].pose.orientation.x = 1.0;
+  config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>("goals", poses);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // goal should have reached our server
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(action_server_->getCurrentGoal()->poses, poses);
+
+  // halt node so another goal can be sent
+  tree_->rootNode()->halt();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  NavigateThroughPosesActionTestFixture::action_server_ =
+    std::make_shared<NavigateThroughPosesActionServer>();
+
+  std::thread server_thread([]() {
+      rclcpp::spin(NavigateThroughPosesActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp

@@ -0,0 +1,180 @@
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp"
+
+class NavigateToPoseActionServer : public TestActionServer<nav2_msgs::action::NavigateToPose>
+{
+public:
+  NavigateToPoseActionServer()
+  : TestActionServer("navigate_to_pose")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<
+      rclcpp_action::ServerGoalHandle<nav2_msgs::action::NavigateToPose>> goal_handle)
+  override
+  {
+    const auto goal = goal_handle->get_goal();
+    auto result = std::make_shared<nav2_msgs::action::NavigateToPose::Result>();
+    goal_handle->succeed(result);
+  }
+};
+
+class NavigateToPoseActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("navigate_to_pose_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::NavigateToPoseAction>(
+          name, "navigate_to_pose", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::NavigateToPoseAction>(
+      "NavigateToPose", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<NavigateToPoseActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr NavigateToPoseActionTestFixture::node_ = nullptr;
+std::shared_ptr<NavigateToPoseActionServer>
+NavigateToPoseActionTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * NavigateToPoseActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> NavigateToPoseActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> NavigateToPoseActionTestFixture::tree_ = nullptr;
+
+TEST_F(NavigateToPoseActionTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <NavigateToPose goal="{goal}" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  geometry_msgs::msg::PoseStamped pose;
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // goal should have reached our server
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose);
+
+  // halt node so another goal can be sent
+  tree_->rootNode()->halt();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
+
+  // set new goal
+  pose.pose.position.x = -2.5;
+  pose.pose.orientation.x = 1.0;
+  config_->blackboard->set<geometry_msgs::msg::PoseStamped>("goal", pose);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose);
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  NavigateToPoseActionTestFixture::action_server_ =
+    std::make_shared<NavigateToPoseActionServer>();
+
+  std::thread server_thread([]() {
+      rclcpp::spin(NavigateToPoseActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp

@@ -0,0 +1,185 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Pablo Iñigo Blasco
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "../../test_action_server.hpp"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "std_msgs/msg/string.hpp"
+
+class PlannerSelectorTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("planner_selector_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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", node_);
+
+    BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
+        return std::make_unique<nav2_behavior_tree::PlannerSelector>(name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::PlannerSelector>("PlannerSelector", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr PlannerSelectorTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * PlannerSelectorTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> PlannerSelectorTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> PlannerSelectorTestFixture::tree_ = nullptr;
+
+TEST_F(PlannerSelectorTestFixture, test_custom_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector_custom_topic_name"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_planner_result;
+  config_->blackboard->get("selected_planner", selected_planner_result);
+
+  EXPECT_EQ(selected_planner_result, "GridBased");
+
+  std_msgs::msg::String selected_planner_cmd;
+
+  selected_planner_cmd.data = "RRT";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto planner_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("planner_selector_custom_topic_name", qos);
+
+  // publish a few updates of the selected_planner
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    planner_selector_pub->publish(selected_planner_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check planner updated
+  config_->blackboard->get("selected_planner", selected_planner_result);
+  EXPECT_EQ("RRT", selected_planner_result);
+}
+
+TEST_F(PlannerSelectorTestFixture, test_default_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_planner_result;
+  config_->blackboard->get("selected_planner", selected_planner_result);
+
+  EXPECT_EQ(selected_planner_result, "GridBased");
+
+  std_msgs::msg::String selected_planner_cmd;
+
+  selected_planner_cmd.data = "RRT";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto planner_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("planner_selector", qos);
+
+  // publish a few updates of the selected_planner
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    planner_selector_pub->publish(selected_planner_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check planner updated
+  config_->blackboard->get("selected_planner", selected_planner_result);
+  EXPECT_EQ("RRT", selected_planner_result);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp

@@ -0,0 +1,187 @@
+// Copyright (c) 2024 Open Navigation LLC
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "../../test_action_server.hpp"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "std_msgs/msg/string.hpp"
+
+class ProgressCheckerSelectorTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("progress_checker_selector_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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("node", node_);
+
+    BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
+        return std::make_unique<nav2_behavior_tree::ProgressCheckerSelector>(name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::ProgressCheckerSelector>(
+      "ProgressCheckerSelector",
+      builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr ProgressCheckerSelectorTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * ProgressCheckerSelectorTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> ProgressCheckerSelectorTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> ProgressCheckerSelectorTestFixture::tree_ = nullptr;
+
+TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="SimpleProgressCheck" topic_name="progress_checker_selector_custom_topic_name"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_progress_checker_result;
+  config_->blackboard->get("selected_progress_checker", selected_progress_checker_result);
+
+  EXPECT_EQ(selected_progress_checker_result, "SimpleProgressCheck");
+
+  std_msgs::msg::String selected_progress_checker_cmd;
+
+  selected_progress_checker_cmd.data = "AngularProgressChecker";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto progress_checker_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>(
+    "progress_checker_selector_custom_topic_name", qos);
+
+  // publish a few updates of the selected_progress_checker
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    progress_checker_selector_pub->publish(selected_progress_checker_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check progress_checker updated
+  config_->blackboard->get("selected_progress_checker", selected_progress_checker_result);
+  EXPECT_EQ("AngularProgressChecker", selected_progress_checker_result);
+}
+
+TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="GridBased"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_progress_checker_result;
+  config_->blackboard->get("selected_progress_checker", selected_progress_checker_result);
+
+  EXPECT_EQ(selected_progress_checker_result, "GridBased");
+
+  std_msgs::msg::String selected_progress_checker_cmd;
+
+  selected_progress_checker_cmd.data = "RRT";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto progress_checker_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("progress_checker_selector", qos);
+
+  // publish a few updates of the selected_progress_checker
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    progress_checker_selector_pub->publish(selected_progress_checker_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check goal_checker updated
+  config_->blackboard->get("selected_progress_checker", selected_progress_checker_result);
+  EXPECT_EQ("RRT", selected_progress_checker_result);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp

@@ -0,0 +1,131 @@
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_service.hpp"
+#include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp"
+
+class ReinitializeGlobalLocalizationService : public TestService<std_srvs::srv::Empty>
+{
+public:
+  ReinitializeGlobalLocalizationService()
+  : TestService("reinitialize_global_localization")
+  {}
+};
+
+class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("reinitialize_global_localization_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+
+    factory_->registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(
+      "ReinitializeGlobalLocalization");
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    server_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<ReinitializeGlobalLocalizationService> server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr ReinitializeGlobalLocalizationServiceTestFixture::node_ = nullptr;
+std::shared_ptr<ReinitializeGlobalLocalizationService>
+ReinitializeGlobalLocalizationServiceTestFixture::server_ = nullptr;
+BT::NodeConfiguration * ReinitializeGlobalLocalizationServiceTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+ReinitializeGlobalLocalizationServiceTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> ReinitializeGlobalLocalizationServiceTestFixture::tree_ = nullptr;
+
+TEST_F(ReinitializeGlobalLocalizationServiceTestFixture, test_tick)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <ReinitializeGlobalLocalization service_name="reinitialize_global_localization"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize service and spin on new thread
+  ReinitializeGlobalLocalizationServiceTestFixture::server_ =
+    std::make_shared<ReinitializeGlobalLocalizationService>();
+  std::thread server_thread([]() {
+      rclcpp::spin(ReinitializeGlobalLocalizationServiceTestFixture::server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp

@@ -0,0 +1,160 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2021 Samsung Research America
+//
+// 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 <memory>
+#include <set>
+#include <string>
+#include <vector>
+
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp"
+#include "../../test_behavior_tree_fixture.hpp"
+
+class RemovePassedGoalsTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("passed_goals_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    config_ = new BT::NodeConfiguration();
+    transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>(node_);
+
+    // 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",
+      node_);
+    config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(
+      "tf_buffer",
+      transform_handler_->getBuffer());
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::RemovePassedGoals>(
+          name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::RemovePassedGoals>(
+      "RemovePassedGoals", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    transform_handler_->deactivate();
+    delete config_;
+    config_ = nullptr;
+    transform_handler_.reset();
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+  static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
+};
+
+rclcpp::Node::SharedPtr RemovePassedGoalsTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * RemovePassedGoalsTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> RemovePassedGoalsTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> RemovePassedGoalsTestFixture::tree_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::TransformHandler>
+RemovePassedGoalsTestFixture::transform_handler_ = nullptr;
+
+TEST_F(RemovePassedGoalsTestFixture, test_tick)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 0.25;
+  pose.position.y = 0.0;
+
+  transform_handler_->activate();
+  transform_handler_->waitForTransform();
+  transform_handler_->updateRobotPose(pose);
+
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <RemovePassedGoals radius="0.5" input_goals="{goals}" output_goals="{goals}"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  std::vector<geometry_msgs::msg::PoseStamped> poses;
+  poses.resize(4);
+  poses[0].pose.position.x = 0.0;
+  poses[0].pose.position.y = 0.0;
+
+  poses[1].pose.position.x = 0.5;
+  poses[1].pose.position.y = 0.0;
+
+  poses[2].pose.position.x = 1.0;
+  poses[2].pose.position.y = 0.0;
+
+  poses[3].pose.position.x = 2.0;
+  poses[3].pose.position.y = 0.0;
+
+  config_->blackboard->set("goals", poses);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check that it removed the point in range
+  std::vector<geometry_msgs::msg::PoseStamped> output_poses;
+  config_->blackboard->get("goals", output_poses);
+
+  EXPECT_EQ(output_poses.size(), 2u);
+  EXPECT_EQ(output_poses[0], poses[2]);
+  EXPECT_EQ(output_poses[1], poses[3]);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp

@@ -0,0 +1,183 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+// Copyright (c) 2021 RoboTech Vision
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "nav_msgs/msg/path.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp"
+
+class SmoothPathActionServer : public TestActionServer<nav2_msgs::action::SmoothPath>
+{
+public:
+  SmoothPathActionServer()
+  : TestActionServer("smooth_path")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<
+      rclcpp_action::ServerGoalHandle<nav2_msgs::action::SmoothPath>> goal_handle)
+  override
+  {
+    const auto goal = goal_handle->get_goal();
+    auto result = std::make_shared<nav2_msgs::action::SmoothPath::Result>();
+    goal_handle->succeed(result);
+  }
+};
+
+class SmoothPathActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("smooth_path_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::SmoothPathAction>(
+          name, "smooth_path", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::SmoothPathAction>(
+      "SmoothPath", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<SmoothPathActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr SmoothPathActionTestFixture::node_ = nullptr;
+std::shared_ptr<SmoothPathActionServer>
+SmoothPathActionTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * SmoothPathActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> SmoothPathActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> SmoothPathActionTestFixture::tree_ = nullptr;
+
+TEST_F(SmoothPathActionTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <SmoothPath unsmoothed_path="{unsmoothed_path}" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  nav_msgs::msg::Path path;
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // goal should have reached our server
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(action_server_->getCurrentGoal()->path, path);
+
+  // halt node so another goal can be sent
+  tree_->rootNode()->halt();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
+
+  // set new goal
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = -2.5;
+  pose.pose.orientation.x = 1.0;
+  path.poses.push_back(pose);
+  config_->blackboard->set<nav_msgs::msg::Path>("unsmoothed_path", path);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  nav_msgs::msg::Path path_empty;
+  EXPECT_NE(path_empty, path);
+  EXPECT_EQ(action_server_->getCurrentGoal()->path, path);
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  SmoothPathActionTestFixture::action_server_ =
+    std::make_shared<SmoothPathActionServer>();
+
+  std::thread server_thread([]() {
+      rclcpp::spin(SmoothPathActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp

@@ -0,0 +1,187 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Pablo Iñigo Blasco
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "../../test_action_server.hpp"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "std_msgs/msg/string.hpp"
+
+class SmootherSelectorTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("smoother_selector_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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", node_);
+
+    BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
+        return std::make_unique<nav2_behavior_tree::SmootherSelector>(name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::SmootherSelector>(
+      "SmootherSelector",
+      builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr SmootherSelectorTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * SmootherSelectorTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> SmootherSelectorTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> SmootherSelectorTestFixture::tree_ = nullptr;
+
+TEST_F(SmootherSelectorTestFixture, test_custom_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <SmootherSelector selected_smoother="{selected_smoother}" default_smoother="DWB" topic_name="smoother_selector_custom_topic_name"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_smoother_result;
+  config_->blackboard->get("selected_smoother", selected_smoother_result);
+
+  EXPECT_EQ(selected_smoother_result, "DWB");
+
+  std_msgs::msg::String selected_smoother_cmd;
+
+  selected_smoother_cmd.data = "DWC";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto smoother_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("smoother_selector_custom_topic_name", qos);
+
+  // publish a few updates of the selected_smoother
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    smoother_selector_pub->publish(selected_smoother_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check smoother updated
+  config_->blackboard->get("selected_smoother", selected_smoother_result);
+  EXPECT_EQ("DWC", selected_smoother_result);
+}
+
+TEST_F(SmootherSelectorTestFixture, test_default_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <SmootherSelector selected_smoother="{selected_smoother}" default_smoother="GridBased"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_smoother_result;
+  config_->blackboard->get("selected_smoother", selected_smoother_result);
+
+  EXPECT_EQ(selected_smoother_result, "GridBased");
+
+  std_msgs::msg::String selected_smoother_cmd;
+
+  selected_smoother_cmd.data = "RRT";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto smoother_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("smoother_selector", qos);
+
+  // publish a few updates of the selected_smoother
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    smoother_selector_pub->publish(selected_smoother_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check smoother updated
+  config_->blackboard->get("selected_smoother", selected_smoother_result);
+  EXPECT_EQ("RRT", selected_smoother_result);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp

@@ -0,0 +1,215 @@
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/spin_action.hpp"
+
+class SpinActionServer : public TestActionServer<nav2_msgs::action::Spin>
+{
+public:
+  SpinActionServer()
+  : TestActionServer("spin")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>
+    goal_handle)
+  override
+  {
+    nav2_msgs::action::Spin::Result::SharedPtr result =
+      std::make_shared<nav2_msgs::action::Spin::Result>();
+    bool return_success = getReturnSuccess();
+    if (return_success) {
+      goal_handle->succeed(result);
+    } else {
+      goal_handle->abort(result);
+    }
+  }
+};
+
+class SpinActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("spin_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+    config_->blackboard->set<int>("number_recoveries", 0);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::SpinAction>(
+          name, "spin", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::SpinAction>("Spin", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void SetUp() override
+  {
+    config_->blackboard->set("number_recoveries", 0);
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<SpinActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr SpinActionTestFixture::node_ = nullptr;
+std::shared_ptr<SpinActionServer> SpinActionTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * SpinActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> SpinActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> SpinActionTestFixture::tree_ = nullptr;
+
+TEST_F(SpinActionTestFixture, test_ports)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Spin server_name="spin"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("spin_dist"), 1.57);
+
+  xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Spin spin_dist="3.14" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("spin_dist"), 3.14);
+}
+
+TEST_F(SpinActionTestFixture, test_tick)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Spin spin_dist="3.14" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+  EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f);
+}
+
+TEST_F(SpinActionTestFixture, test_failure)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Spin spin_dist="3.14" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  action_server_->setReturnSuccess(false);
+
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+
+  std::cout << tree_->rootNode()->status();
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+  EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  SpinActionTestFixture::action_server_ = std::make_shared<SpinActionServer>();
+  std::thread server_thread([]() {
+      rclcpp::spin(SpinActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp

@@ -0,0 +1,175 @@
+// Copyright (c) 2022 Neobotix GmbH
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp"
+#include "lifecycle_msgs/srv/change_state.hpp"
+
+class CancelSpinServer : public TestActionServer<nav2_msgs::action::Spin>
+{
+public:
+  CancelSpinServer()
+  : TestActionServer("spin")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>
+    goal_handle)
+  {
+    while (!goal_handle->is_canceling()) {
+      // Spining here until goal cancels
+      std::this_thread::sleep_for(std::chrono::milliseconds(15));
+    }
+  }
+};
+
+class CancelSpinActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("cancel_spin_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    client_ = rclcpp_action::create_client<nav2_msgs::action::Spin>(
+      node_, "spin");
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::SpinCancel>(
+          name, "spin", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::SpinCancel>("CancelSpin", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    client_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<CancelSpinServer> action_server_;
+  static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Spin>> client_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr CancelSpinActionTestFixture::node_ = nullptr;
+std::shared_ptr<CancelSpinServer>
+CancelSpinActionTestFixture::action_server_ = nullptr;
+std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Spin>>
+CancelSpinActionTestFixture::client_ = nullptr;
+
+BT::NodeConfiguration * CancelSpinActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+CancelSpinActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> CancelSpinActionTestFixture::tree_ = nullptr;
+
+TEST_F(CancelSpinActionTestFixture, test_ports)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+             <CancelSpin name="SpinCancel"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::Spin>::SendGoalOptions();
+
+  // Creating a dummy goal_msg
+  auto goal_msg = nav2_msgs::action::Spin::Goal();
+
+  // Setting target yaw
+  goal_msg.target_yaw = 1.57;
+
+  // Spining for server and sending a goal
+  client_->wait_for_action_server();
+  client_->async_send_goal(goal_msg, send_goal_options);
+
+  // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
+  std::this_thread::sleep_for(std::chrono::milliseconds(15));
+
+  // Executing tick
+  tree_->rootNode()->executeTick();
+
+  // BT node should return success, once when the goal is cancelled
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  // Adding another test case to check if the goal is infact cancelling
+  EXPECT_EQ(action_server_->isGoalCancelled(), true);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  CancelSpinActionTestFixture::action_server_ = std::make_shared<CancelSpinServer>();
+  std::thread server_thread([]() {
+      rclcpp::spin(CancelSpinActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp

@@ -0,0 +1,156 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Francisco Martin Rico
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp"
+
+
+class TruncatePathTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("change_goal_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::TruncatePath>(
+          name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::TruncatePath>(
+      "TruncatePath", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr TruncatePathTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * TruncatePathTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> TruncatePathTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> TruncatePathTestFixture::tree_ = nullptr;
+
+TEST_F(TruncatePathTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  nav_msgs::msg::Path path;
+  path.header.stamp = node_->now();
+
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = 0;
+  pose.pose.position.y = 0;
+  pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0);
+  path.poses.push_back(pose);
+
+  pose.pose.position.x = 0.5;
+  pose.pose.position.y = 0.0;
+  path.poses.push_back(pose);
+
+  pose.pose.position.x = 0.9;
+  pose.pose.position.y = 0.0;
+  path.poses.push_back(pose);
+
+  pose.pose.position.x = 1.5;
+  pose.pose.position.y = 0.5;
+  path.poses.push_back(pose);
+
+  EXPECT_EQ(path.poses.size(), 4u);
+
+  config_->blackboard->set("path", path);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  nav_msgs::msg::Path truncated_path;
+  config_->blackboard->get("truncated_path", truncated_path);
+
+  EXPECT_NE(path, truncated_path);
+  EXPECT_EQ(truncated_path.poses.size(), 2u);
+
+  double r, p, y;
+  tf2::Quaternion q;
+  tf2::fromMsg(truncated_path.poses.back().pose.orientation, q);
+  tf2::Matrix3x3(q).getRPY(r, p, y);
+
+  EXPECT_NEAR(y, 0.463, 0.001);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp

@@ -0,0 +1,458 @@
+// Copyright (c) 2021 RoboTech Vision
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp"
+
+
+class TruncatePathLocalTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp() override
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::TruncatePathLocal>(
+      "truncate_path_local", *config_);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::TruncatePathLocal>(
+          name, config);
+      };
+    try {
+      factory_->registerBuilder<nav2_behavior_tree::TruncatePathLocal>(
+        "TruncatePathLocal", builder);
+    } catch (BT::BehaviorTreeException const &) {
+      // ignoring multiple registrations of TruncatePathLocal
+    }
+  }
+
+  void TearDown() override
+  {
+    bt_node_.reset();
+    tree_.reset();
+  }
+
+  static geometry_msgs::msg::PoseStamped poseMsg(double x, double y, double orientation)
+  {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = x;
+    pose.pose.position.y = y;
+    pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(orientation);
+    return pose;
+  }
+
+  nav_msgs::msg::Path createLoopCrossingTestPath()
+  {
+    nav_msgs::msg::Path path;
+    path.header.stamp = node_->now();
+    path.header.frame_id = "map";
+
+    // this is a loop to make it harder for robot to find the proper closest pose
+    path.poses.push_back(poseMsg(-0.3, -1.2, -M_PI * 3 / 2));
+    // the position is closest to robot but orientation is different
+    path.poses.push_back(poseMsg(-0.3, 0.0, -M_PI * 3 / 2));
+    path.poses.push_back(poseMsg(-0.5, 1.0, -M_PI));
+    path.poses.push_back(poseMsg(-1.5, 1.0, -M_PI / 2));
+    path.poses.push_back(poseMsg(-1.5, 0.0, 0.0));
+
+    // this is the correct path section for the first match
+    path.poses.push_back(poseMsg(-0.5, 0.0, 0.0));
+    path.poses.push_back(poseMsg(0.4, 0.0, 0.0));
+    path.poses.push_back(poseMsg(1.5, 0.0, 0.0));
+
+    // this is a loop to make it harder for robot to find the proper closest pose
+    path.poses.push_back(poseMsg(1.5, 1.0, M_PI / 2));
+    path.poses.push_back(poseMsg(0.5, 1.0, M_PI));
+    // the position is closest to robot but orientation is different
+    path.poses.push_back(poseMsg(0.3, 0.0, M_PI * 3 / 2));
+    path.poses.push_back(poseMsg(0.3, -1.0, M_PI * 3 / 2));
+
+    return path;
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::TruncatePathLocal> bt_node_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+std::shared_ptr<nav2_behavior_tree::TruncatePathLocal> TruncatePathLocalTestFixture::bt_node_ =
+  nullptr;
+std::shared_ptr<BT::Tree> TruncatePathLocalTestFixture::tree_ = nullptr;
+
+TEST_F(TruncatePathLocalTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <TruncatePathLocal
+            distance_forward="2.0"
+            distance_backward="1.0"
+            robot_frame="base_link"
+            transform_tolerance="0.2"
+            angular_distance_weight="0.2"
+            pose="{pose}"
+            input_path="{path}"
+            output_path="{truncated_path}"
+          />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create path and set it on blackboard
+  nav_msgs::msg::Path path = createLoopCrossingTestPath();
+  EXPECT_EQ(path.poses.size(), 12u);
+
+  config_->blackboard->set("path", path);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+
+  nav_msgs::msg::Path truncated_path;
+  config_->blackboard->get("truncated_path", truncated_path);
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_NE(path, truncated_path);
+  ASSERT_GE(truncated_path.poses.size(), 1u);
+  EXPECT_EQ(truncated_path.poses.size(), 3u);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.x, -0.5);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.x, 1.5);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.y, 0.0);
+
+  /////////////////////////////////////////
+  // should match the first loop crossing
+  config_->blackboard->set("pose", poseMsg(0.0, 0.0, M_PI / 2));
+
+  tree_->haltTree();
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+  config_->blackboard->get("truncated_path", truncated_path);
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_NE(path, truncated_path);
+  ASSERT_GE(truncated_path.poses.size(), 1u);
+  EXPECT_EQ(truncated_path.poses.size(), 2u);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.x, -0.3);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.x, -0.5);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.y, 1.0);
+
+  /////////////////////////////////////////
+  // should match the last loop crossing
+  config_->blackboard->set("pose", poseMsg(0.0, 0.0, -M_PI / 2));
+
+  tree_->haltTree();
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+  config_->blackboard->get("truncated_path", truncated_path);
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_NE(path, truncated_path);
+  ASSERT_GE(truncated_path.poses.size(), 1u);
+  EXPECT_EQ(truncated_path.poses.size(), 2u);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.x, 0.3);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.x, 0.3);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.y, -1.0);
+
+  SUCCEED();
+}
+
+TEST_F(TruncatePathLocalTestFixture, test_success_on_empty_path)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <TruncatePathLocal
+            distance_forward="2.0"
+            distance_backward="1.0"
+            robot_frame="base_link"
+            transform_tolerance="0.2"
+            angular_distance_weight="0.2"
+            pose="{pose}"
+            input_path="{path}"
+            output_path="{truncated_path}"
+            max_robot_pose_search_dist="infinity"
+          />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create path and set it on blackboard
+  nav_msgs::msg::Path path;
+  path.header.stamp = node_->now();
+  path.header.frame_id = "map";
+
+  config_->blackboard->set("path", path);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+  nav_msgs::msg::Path truncated_path;
+  config_->blackboard->get("truncated_path", truncated_path);
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(path, truncated_path);
+  SUCCEED();
+}
+
+TEST_F(TruncatePathLocalTestFixture, test_failure_on_no_pose)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <TruncatePathLocal
+            distance_forward="2.0"
+            distance_backward="1.0"
+            transform_tolerance="0.2"
+            angular_distance_weight="0.2"
+            robot_frame="{robot_frame}"
+            input_path="{path}"
+            output_path="{truncated_path}"
+            max_robot_pose_search_dist="infinity"
+          />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create path and set it on blackboard
+  nav_msgs::msg::Path path;
+  path.header.stamp = node_->now();
+  path.header.frame_id = "map";
+
+  config_->blackboard->set("path", path);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+  nav_msgs::msg::Path truncated_path;
+  config_->blackboard->get("truncated_path", truncated_path);
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+  SUCCEED();
+}
+
+TEST_F(TruncatePathLocalTestFixture, test_failure_on_invalid_robot_frame)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <TruncatePathLocal
+            distance_forward="2.0"
+            distance_backward="1.0"
+            transform_tolerance="0.2"
+            robot_frame="invalid_frame"
+            angular_distance_weight="0.2"
+            input_path="{path}"
+            output_path="{truncated_path}"
+            max_robot_pose_search_dist="infinity"
+          />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  nav_msgs::msg::Path path = createLoopCrossingTestPath();
+  EXPECT_EQ(path.poses.size(), 12u);
+
+  config_->blackboard->set("path", path);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+  nav_msgs::msg::Path truncated_path;
+  config_->blackboard->get("truncated_path", truncated_path);
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+  SUCCEED();
+}
+
+TEST_F(TruncatePathLocalTestFixture, test_path_pruning)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <TruncatePathLocal
+            distance_forward="2.0"
+            distance_backward="1.0"
+            robot_frame="base_link"
+            transform_tolerance="0.2"
+            angular_distance_weight="0.0"
+            pose="{pose}"
+            input_path="{path}"
+            output_path="{truncated_path}"
+            max_robot_pose_search_dist="3.0"
+          />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create path and set it on blackboard
+  nav_msgs::msg::Path path = createLoopCrossingTestPath();
+  nav_msgs::msg::Path truncated_path;
+
+  config_->blackboard->set("path", path);
+
+  /////////////////////////////////////////
+  // should match the first loop crossing
+  config_->blackboard->set("pose", poseMsg(0.0, 0.0, 0.0));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+  config_->blackboard->get("truncated_path", truncated_path);
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_NE(path, truncated_path);
+  ASSERT_GE(truncated_path.poses.size(), 1u);
+  EXPECT_EQ(truncated_path.poses.size(), 2u);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.x, -0.3);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.x, -0.5);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.y, 1.0);
+
+  /////////////////////////////////////////
+  // move along the path to leave the first loop crossing behind
+  config_->blackboard->set("pose", poseMsg(-1.5, 1.0, 0.0));
+  // tick until node succeeds
+  tree_->haltTree();
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+  // this truncated_path is not interesting, let's proceed to the second loop crossing
+
+  /////////////////////////////////////////
+  // should match the second loop crossing
+  config_->blackboard->set("pose", poseMsg(0.0, 0.0, 0.0));
+  // tick until node succeeds
+  tree_->haltTree();
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+  config_->blackboard->get("truncated_path", truncated_path);
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_NE(path, truncated_path);
+  ASSERT_GE(truncated_path.poses.size(), 1u);
+  EXPECT_EQ(truncated_path.poses.size(), 3u);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.x, -0.5);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.x, 1.5);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.y, 0.0);
+
+  /////////////////////////////////////////
+  // move along the path to leave the second loop crossing behind
+  config_->blackboard->set("pose", poseMsg(1.5, 1.0, 0.0));
+  // tick until node succeeds
+  tree_->haltTree();
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+  // this truncated_path is not interesting, let's proceed to the last loop crossing
+
+  /////////////////////////////////////////
+  // should match the last loop crossing
+  config_->blackboard->set("pose", poseMsg(0.0, 0.0, 0.0));
+  // tick until node succeeds
+  tree_->haltTree();
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+  config_->blackboard->get("truncated_path", truncated_path);
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_NE(path, truncated_path);
+  ASSERT_GE(truncated_path.poses.size(), 1u);
+  EXPECT_EQ(truncated_path.poses.size(), 2u);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.x, 0.3);
+  EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.x, 0.3);
+  EXPECT_EQ(truncated_path.poses.back().pose.position.y, -1.0);
+
+  SUCCEED();
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp

@@ -0,0 +1,179 @@
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/wait_action.hpp"
+
+class WaitActionServer : public TestActionServer<nav2_msgs::action::Wait>
+{
+public:
+  WaitActionServer()
+  : TestActionServer("wait")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Wait>>)
+  override
+  {}
+};
+
+class WaitActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("wait_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+    config_->blackboard->set<int>("number_recoveries", 0);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::WaitAction>(
+          name, "wait", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::WaitAction>("Wait", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    factory_.reset();
+  }
+
+  void SetUp() override
+  {
+    config_->blackboard->set("number_recoveries", 0);
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<WaitActionServer> action_server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr WaitActionTestFixture::node_ = nullptr;
+std::shared_ptr<WaitActionServer> WaitActionTestFixture::action_server_ = nullptr;
+BT::NodeConfiguration * WaitActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> WaitActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> WaitActionTestFixture::tree_ = nullptr;
+
+TEST_F(WaitActionTestFixture, test_ports)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Wait />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(tree_->rootNode()->getInput<int>("wait_duration"), 1);
+
+  xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Wait wait_duration="10" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(tree_->rootNode()->getInput<int>("wait_duration"), 10);
+}
+
+TEST_F(WaitActionTestFixture, test_tick)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Wait wait_duration="-5"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+  EXPECT_EQ(rclcpp::Duration(action_server_->getCurrentGoal()->time).seconds(), 5.0);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  WaitActionTestFixture::action_server_ = std::make_shared<WaitActionServer>();
+  std::thread server_thread([]() {
+      rclcpp::spin(WaitActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp

@@ -0,0 +1,175 @@
+// Copyright (c) 2022 Neobotix GmbH
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp"
+#include "lifecycle_msgs/srv/change_state.hpp"
+
+class CancelWaitServer : public TestActionServer<nav2_msgs::action::Wait>
+{
+public:
+  CancelWaitServer()
+  : TestActionServer("wait")
+  {}
+
+protected:
+  void execute(
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Wait>>
+    goal_handle)
+  {
+    while (!goal_handle->is_canceling()) {
+      // waiting here until goal cancels
+      std::this_thread::sleep_for(std::chrono::milliseconds(15));
+    }
+  }
+};
+
+class CancelWaitActionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("cancel_wait_action_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    client_ = rclcpp_action::create_client<nav2_msgs::action::Wait>(
+      node_, "wait");
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::WaitCancel>(
+          name, "wait", config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::WaitCancel>("CancelWait", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    action_server_.reset();
+    client_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<CancelWaitServer> action_server_;
+  static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Wait>> client_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr CancelWaitActionTestFixture::node_ = nullptr;
+std::shared_ptr<CancelWaitServer>
+CancelWaitActionTestFixture::action_server_ = nullptr;
+std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Wait>>
+CancelWaitActionTestFixture::client_ = nullptr;
+
+BT::NodeConfiguration * CancelWaitActionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+CancelWaitActionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> CancelWaitActionTestFixture::tree_ = nullptr;
+
+TEST_F(CancelWaitActionTestFixture, test_ports)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+             <CancelWait name="WaitCancel"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::Wait>::SendGoalOptions();
+
+  // Creating a dummy goal_msg
+  auto goal_msg = nav2_msgs::action::Wait::Goal();
+
+  // Setting a waiting time for 5 Seconds.
+  goal_msg.time.sec = 5;
+
+  // Waiting for server and sending a goal
+  client_->wait_for_action_server();
+  client_->async_send_goal(goal_msg, send_goal_options);
+
+  // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
+  std::this_thread::sleep_for(std::chrono::milliseconds(15));
+
+  // Executing tick
+  tree_->rootNode()->executeTick();
+
+  // BT node should return success, once when the goal is cancelled
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  // Adding another test case to check if the goal is infact cancelling
+  EXPECT_EQ(action_server_->isGoalCancelled(), true);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize action server and spin on new thread
+  CancelWaitActionTestFixture::action_server_ = std::make_shared<CancelWaitServer>();
+  std::thread server_thread([]() {
+      rclcpp::spin(CancelWaitActionTestFixture::action_server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt

@@ -0,0 +1,47 @@
+ament_add_gtest(test_condition_distance_traveled 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 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})
+
+ament_add_gtest(test_condition_path_expiring_timer test_path_expiring_timer.cpp)
+target_link_libraries(test_condition_path_expiring_timer nav2_path_expiring_timer_condition)
+ament_target_dependencies(test_condition_time_expired ${dependencies})
+
+ament_add_gtest(test_condition_goal_reached test_goal_reached.cpp)
+target_link_libraries(test_condition_goal_reached nav2_goal_reached_condition_bt_node)
+ament_target_dependencies(test_condition_goal_reached ${dependencies})
+
+ament_add_gtest(test_condition_goal_updated test_goal_updated.cpp)
+target_link_libraries(test_condition_goal_updated nav2_goal_updated_condition_bt_node)
+ament_target_dependencies(test_condition_goal_updated ${dependencies})
+
+ament_add_gtest(test_condition_globally_updated_goal test_globally_updated_goal.cpp)
+target_link_libraries(test_condition_globally_updated_goal nav2_globally_updated_goal_condition_bt_node)
+ament_target_dependencies(test_condition_globally_updated_goal ${dependencies})
+
+ament_add_gtest(test_condition_initial_pose_received test_initial_pose_received.cpp)
+target_link_libraries(test_condition_initial_pose_received nav2_initial_pose_received_condition_bt_node)
+ament_target_dependencies(test_condition_initial_pose_received ${dependencies})
+
+ament_add_gtest(test_condition_transform_available test_transform_available.cpp)
+target_link_libraries(test_condition_transform_available nav2_transform_available_condition_bt_node)
+ament_target_dependencies(test_condition_transform_available ${dependencies})
+
+ament_add_gtest(test_condition_is_stuck test_is_stuck.cpp)
+target_link_libraries(test_condition_is_stuck nav2_is_stuck_condition_bt_node)
+ament_target_dependencies(test_condition_is_stuck ${dependencies})
+
+ament_add_gtest(test_condition_is_battery_charging test_is_battery_charging.cpp)
+target_link_libraries(test_condition_is_battery_charging nav2_is_battery_charging_condition_bt_node)
+ament_target_dependencies(test_condition_is_battery_charging ${dependencies})
+
+ament_add_gtest(test_condition_is_battery_low test_is_battery_low.cpp)
+target_link_libraries(test_condition_is_battery_low nav2_is_battery_low_condition_bt_node)
+ament_target_dependencies(test_condition_is_battery_low ${dependencies})
+
+ament_add_gtest(test_condition_is_path_valid test_is_path_valid.cpp)
+target_link_libraries(test_condition_is_path_valid nav2_is_path_valid_condition_bt_node)
+ament_target_dependencies(test_condition_is_path_valid ${dependencies})

navigation2/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp

@@ -0,0 +1,92 @@
+// 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 <memory>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
+
+class DistanceTraveledConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::DistanceTraveledCondition>(
+      "distance_traveled", *config_);
+  }
+
+  void TearDown()
+  {
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::DistanceTraveledCondition> bt_node_;
+};
+
+std::shared_ptr<nav2_behavior_tree::DistanceTraveledCondition>
+DistanceTraveledConditionTestFixture::bt_node_ = nullptr;
+
+TEST_F(DistanceTraveledConditionTestFixture, test_behavior)
+{
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_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(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+    } else {
+      EXPECT_EQ(bt_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;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp

@@ -0,0 +1,73 @@
+// Copyright (c) 2021 Joshua Wallace
+//
+// 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 <memory>
+#include <set>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
+
+class GloballyUpdatedGoalConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::GloballyUpdatedGoalCondition>(
+      "globally_updated_goal", *config_);
+  }
+
+  void TearDown()
+  {
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::GloballyUpdatedGoalCondition> bt_node_;
+};
+
+std::shared_ptr<nav2_behavior_tree::GloballyUpdatedGoalCondition>
+GloballyUpdatedGoalConditionTestFixture::bt_node_ = nullptr;
+
+TEST_F(GloballyUpdatedGoalConditionTestFixture, test_behavior)
+{
+  geometry_msgs::msg::PoseStamped goal;
+  config_->blackboard->set("goal", goal);
+
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+
+  goal.pose.position.x = 1.0;
+  config_->blackboard->set("goal", goal);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_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;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp

@@ -0,0 +1,110 @@
+// 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 <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue{0.1});
+
+    geometry_msgs::msg::PoseStamped goal;
+    goal.header.stamp = node_->now();
+    goal.header.frame_id = "map";
+    goal.pose.position.x = 1.0;
+    goal.pose.position.y = 1.0;
+    config_->blackboard->set("goal", goal);
+
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <GoalReached goal="{goal}" />
+        </BehaviorTree>
+      </root>)";
+
+    factory_->registerNodeType<nav2_behavior_tree::GoalReachedCondition>("GoalReached");
+    tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  }
+
+  void TearDown()
+  {
+    tree_.reset();
+  }
+
+protected:
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+std::shared_ptr<BT::Tree> GoalReachedConditionTestFixture::tree_ = nullptr;
+
+TEST_F(GoalReachedConditionTestFixture, test_behavior)
+{
+  EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);
+
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 0.0;
+  pose.position.y = 0.0;
+  transform_handler_->updateRobotPose(pose);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);
+
+  pose.position.x = 0.5;
+  pose.position.y = 0.5;
+  transform_handler_->updateRobotPose(pose);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);
+
+  pose.position.x = 0.9;
+  pose.position.y = 0.9;
+  transform_handler_->updateRobotPose(pose);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS);
+
+  pose.position.x = 1.0;
+  pose.position.y = 1.0;
+  transform_handler_->updateRobotPose(pose);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(tree_->tickRoot(), 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;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/test_goal_updated.cpp

@@ -0,0 +1,74 @@
+// 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 <memory>
+#include <set>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
+
+class GoalUpdatedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedCondition>(
+      "goal_updated", *config_);
+  }
+
+  void TearDown()
+  {
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::GoalUpdatedCondition> bt_node_;
+};
+
+std::shared_ptr<nav2_behavior_tree::GoalUpdatedCondition>
+GoalUpdatedConditionTestFixture::bt_node_ = nullptr;
+
+TEST_F(GoalUpdatedConditionTestFixture, test_behavior)
+{
+  geometry_msgs::msg::PoseStamped goal;
+  config_->blackboard->set("goal", goal);
+
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+
+  goal.pose.position.x = 1.0;
+  config_->blackboard->set("goal", goal);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_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;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp

@@ -0,0 +1,81 @@
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
+
+class TestNode : public BT::SyncActionNode
+{
+public:
+  TestNode(const std::string & name, const BT::NodeConfiguration & config)
+  : SyncActionNode(name, config)
+  {}
+
+  BT::NodeStatus tick()
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return {};
+  }
+};
+
+class InitialPoseReceivedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    test_node_ = std::make_shared<TestNode>("TestNode", *config_);
+  }
+
+  void TearDown()
+  {
+    test_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<TestNode> test_node_;
+};
+
+std::shared_ptr<TestNode> InitialPoseReceivedConditionTestFixture::test_node_ = nullptr;
+
+TEST_F(InitialPoseReceivedConditionTestFixture, test_behavior)
+{
+  EXPECT_EQ(nav2_behavior_tree::initialPoseReceived(*test_node_), BT::NodeStatus::FAILURE);
+  config_->blackboard->set("initial_pose_received", true);
+  EXPECT_EQ(nav2_behavior_tree::initialPoseReceived(*test_node_), 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;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp

@@ -0,0 +1,130 @@
+// Copyright (c) 2023 Alberto J. Tudela Roldán
+//
+// 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 <memory>
+#include <set>
+#include <string>
+#include <chrono>
+
+#include "sensor_msgs/msg/battery_state.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp"
+
+class IsBatteryChargingConditionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("test_is_battery_charging");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+
+    factory_->registerNodeType<nav2_behavior_tree::IsBatteryChargingCondition>("IsBatteryCharging");
+
+    battery_pub_ = node_->create_publisher<sensor_msgs::msg::BatteryState>(
+      "/battery_status",
+      rclcpp::SystemDefaultsQoS());
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    battery_pub_.reset();
+    node_.reset();
+    factory_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_pub_;
+};
+
+rclcpp::Node::SharedPtr IsBatteryChargingConditionTestFixture::node_ = nullptr;
+BT::NodeConfiguration * IsBatteryChargingConditionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> IsBatteryChargingConditionTestFixture::factory_ = nullptr;
+rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr
+IsBatteryChargingConditionTestFixture::battery_pub_ = nullptr;
+
+TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <IsBatteryCharging battery_topic="/battery_status"/>
+        </BehaviorTree>
+      </root>)";
+
+  auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
+
+  sensor_msgs::msg::BatteryState battery_msg;
+  battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+  battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS);
+
+  battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+  battery_msg.power_supply_status =
+    sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+  battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), 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;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp

@@ -0,0 +1,162 @@
+// 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.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <set>
+#include <string>
+#include <chrono>
+
+#include "sensor_msgs/msg/battery_state.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp"
+
+class IsBatteryLowConditionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("test_is_battery_low");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+
+    factory_->registerNodeType<nav2_behavior_tree::IsBatteryLowCondition>("IsBatteryLow");
+
+    battery_pub_ = node_->create_publisher<sensor_msgs::msg::BatteryState>(
+      "/battery_status",
+      rclcpp::SystemDefaultsQoS());
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    battery_pub_.reset();
+    node_.reset();
+    factory_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_pub_;
+};
+
+rclcpp::Node::SharedPtr IsBatteryLowConditionTestFixture::node_ = nullptr;
+BT::NodeConfiguration * IsBatteryLowConditionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> IsBatteryLowConditionTestFixture::factory_ = nullptr;
+rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr
+IsBatteryLowConditionTestFixture::battery_pub_ = nullptr;
+
+TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <IsBatteryLow min_battery="0.5" battery_topic="/battery_status"/>
+        </BehaviorTree>
+      </root>)";
+
+  auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
+
+  sensor_msgs::msg::BatteryState battery_msg;
+  battery_msg.percentage = 1.0;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+  battery_msg.percentage = 0.49;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS);
+
+  battery_msg.percentage = 0.51;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+  battery_msg.percentage = 0.0;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS);
+}
+
+TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <IsBatteryLow min_battery="5.0" battery_topic="/battery_status" is_voltage="true"/>
+        </BehaviorTree>
+      </root>)";
+
+  auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
+
+  sensor_msgs::msg::BatteryState battery_msg;
+  battery_msg.voltage = 10.0;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+  battery_msg.voltage = 4.9;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS);
+
+  battery_msg.voltage = 5.1;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+  battery_msg.voltage = 0.0;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), 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;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp

@@ -0,0 +1,127 @@
+// Copyright (c) 2021 Joshua Wallace
+//
+// 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 <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "../../test_service.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class IsPathValidService : public TestService<nav2_msgs::srv::IsPathValid>
+{
+public:
+  IsPathValidService()
+  : TestService("is_path_valid")
+  {}
+
+  virtual void handle_service(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::IsPathValid::Request> request,
+    const std::shared_ptr<nav2_msgs::srv::IsPathValid::Response> response)
+  {
+    (void)request_header;
+    (void)request;
+    response->is_valid = true;
+  }
+};
+
+class IsPathValidTestFixture : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    node_ = std::make_shared<rclcpp::Node>("test_is_path_valid_condition");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+    config_ = new BT::NodeConfiguration();
+    config_->blackboard = BT::Blackboard::create();
+    config_->blackboard->set<rclcpp::Node::SharedPtr>("node", node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(10));
+    factory_->registerNodeType<nav2_behavior_tree::IsPathValidCondition>("IsPathValid");
+  }
+
+  void TearDown()
+  {
+    delete config_;
+    config_ = nullptr;
+    server_.reset();
+    node_.reset();
+    factory_.reset();
+    tree_.reset();
+  }
+
+  static std::shared_ptr<IsPathValidService> server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+std::shared_ptr<IsPathValidService> IsPathValidTestFixture::server_ = nullptr;
+rclcpp::Node::SharedPtr IsPathValidTestFixture::node_ = nullptr;
+BT::NodeConfiguration * IsPathValidTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> IsPathValidTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> IsPathValidTestFixture::tree_ = nullptr;
+
+TEST_F(IsPathValidTestFixture, test_behavior)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <IsPathValid/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  std::this_thread::sleep_for(500ms);
+
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize service and spin on new thread
+  IsPathValidTestFixture::server_ = std::make_shared<IsPathValidService>();
+  std::thread server_thread([]() {
+      rclcpp::spin(IsPathValidTestFixture::server_);
+    });
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/test_is_stuck.cpp

@@ -0,0 +1,101 @@
+// 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_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class IsStuckTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::IsStuckCondition>("is_stuck", *config_);
+  }
+
+  void TearDown()
+  {
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::IsStuckCondition> bt_node_;
+};
+
+std::shared_ptr<nav2_behavior_tree::IsStuckCondition>
+IsStuckTestFixture::bt_node_ = nullptr;
+
+TEST_F(IsStuckTestFixture, test_behavior)
+{
+  auto odom_pub = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 1);
+  nav_msgs::msg::Odometry odom_msg;
+
+  // fill up odometry history with zero velocity
+  auto time = node_->now();
+  odom_msg.header.stamp = time;
+  odom_msg.twist.twist.linear.x = 0.0;
+  odom_pub->publish(odom_msg);
+  std::this_thread::sleep_for(500ms);
+  odom_pub->publish(odom_msg);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+
+  // huge negative velocity to simulate sudden brake
+  odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.1);
+  odom_msg.twist.twist.linear.x = -1.5;
+  odom_pub->publish(odom_msg);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+
+  // huge positive velocity means robot is not stuck anymore
+  odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.2);
+  odom_msg.twist.twist.linear.x = 1.0;
+  odom_pub->publish(odom_msg);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+
+  // stuck again due to negative velocity change is smaller time period
+  odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.25);
+  odom_msg.twist.twist.linear.x = 0.0;
+  odom_pub->publish(odom_msg);
+  std::this_thread::sleep_for(500ms);
+  EXPECT_EQ(bt_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;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp

@@ -0,0 +1,100 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// 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 "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class PathExpiringTimerConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    node_ = std::make_shared<rclcpp::Node>("test_path_expiring_condition");
+    config_ = new BT::NodeConfiguration();
+    config_->blackboard = BT::Blackboard::create();
+    config_->blackboard->set<rclcpp::Node::SharedPtr>("node", node_);
+    bt_node_ = std::make_shared<nav2_behavior_tree::PathExpiringTimerCondition>(
+      "time_expired", *config_);
+  }
+
+  void TearDown()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    bt_node_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static std::shared_ptr<nav2_behavior_tree::PathExpiringTimerCondition> bt_node_;
+  static BT::NodeConfiguration * config_;
+};
+
+rclcpp::Node::SharedPtr PathExpiringTimerConditionTestFixture::node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::PathExpiringTimerCondition>
+PathExpiringTimerConditionTestFixture::bt_node_ = nullptr;
+BT::NodeConfiguration * PathExpiringTimerConditionTestFixture::config_ = nullptr;
+
+TEST_F(PathExpiringTimerConditionTestFixture, test_behavior)
+{
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+
+  for (int i = 0; i < 20; ++i) {
+    rclcpp::sleep_for(500ms);
+    if (i % 2) {
+      EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+    } else {
+      EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+    }
+  }
+
+  // place a new path on the blackboard to reset the timer
+  nav_msgs::msg::Path path;
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = 1.0;
+  path.poses.push_back(pose);
+
+  config_->blackboard->set<nav_msgs::msg::Path>("path", path);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  rclcpp::sleep_for(1500ms);
+  EXPECT_EQ(bt_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;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp

@@ -0,0 +1,79 @@
+// 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 "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class TimeExpiredConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::TimeExpiredCondition>(
+      "time_expired", *config_);
+  }
+
+  void TearDown()
+  {
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::TimeExpiredCondition> bt_node_;
+};
+
+std::shared_ptr<nav2_behavior_tree::TimeExpiredCondition>
+TimeExpiredConditionTestFixture::bt_node_ = nullptr;
+
+TEST_F(TimeExpiredConditionTestFixture, test_behavior)
+{
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+
+  for (int i = 0; i < 20; ++i) {
+    rclcpp::sleep_for(500ms);
+    if (i % 2) {
+      EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+    } else {
+      EXPECT_EQ(bt_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;
+}

navigation2/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp

@@ -0,0 +1,121 @@
+// 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 <memory>
+#include <set>
+#include <chrono>
+#include <string>
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/transform_available_condition.hpp"
+
+class TransformAvailableConditionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("test_behavior_tree_fixture");
+    transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>(node_);
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+    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(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<bool>("initial_pose_received", false);
+  }
+
+  static void TearDownTestCase()
+  {
+    transform_handler_->deactivate();
+    delete config_;
+    config_ = nullptr;
+    transform_handler_.reset();
+    node_.reset();
+    factory_.reset();
+  }
+
+  void SetUp()
+  {
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <TransformAvailable child="base_link" parent="map" />
+        </BehaviorTree>
+      </root>)";
+
+    factory_->registerNodeType<nav2_behavior_tree::TransformAvailableCondition>(
+      "TransformAvailable");
+    tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  }
+
+  void TearDown()
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr TransformAvailableConditionTestFixture::node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::TransformHandler>
+TransformAvailableConditionTestFixture::transform_handler_ = nullptr;
+BT::NodeConfiguration * TransformAvailableConditionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+TransformAvailableConditionTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> TransformAvailableConditionTestFixture::tree_ = nullptr;
+
+TEST_F(TransformAvailableConditionTestFixture, test_behavior)
+{
+  EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);
+  transform_handler_->activate();
+  transform_handler_->waitForTransform();
+  EXPECT_EQ(tree_->tickRoot(), 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;
+}

navigation2/nav2_behavior_tree/test/plugins/control/CMakeLists.txt

@@ -0,0 +1,11 @@
+ament_add_gtest(test_control_recovery_node test_recovery_node.cpp)
+target_link_libraries(test_control_recovery_node nav2_recovery_node_bt_node)
+ament_target_dependencies(test_control_recovery_node ${dependencies})
+
+ament_add_gtest(test_control_pipeline_sequence test_pipeline_sequence.cpp)
+target_link_libraries(test_control_pipeline_sequence nav2_pipeline_sequence_bt_node)
+ament_target_dependencies(test_control_pipeline_sequence ${dependencies})
+
+ament_add_gtest(test_control_round_robin_node test_round_robin_node.cpp)
+target_link_libraries(test_control_round_robin_node nav2_round_robin_node_bt_node)
+ament_target_dependencies(test_control_round_robin_node ${dependencies})

navigation2/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp

@@ -0,0 +1,141 @@
+// 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 <memory>
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "../../test_dummy_tree_node.hpp"
+#include "nav2_behavior_tree/plugins/control/pipeline_sequence.hpp"
+
+class PipelineSequenceTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp() override
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::PipelineSequence>(
+      "pipeline_sequence", *config_);
+    first_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    second_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    third_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    bt_node_->addChild(first_child_.get());
+    bt_node_->addChild(second_child_.get());
+    bt_node_->addChild(third_child_.get());
+  }
+
+  void TearDown() override
+  {
+    first_child_.reset();
+    second_child_.reset();
+    third_child_.reset();
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::PipelineSequence> bt_node_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> first_child_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> second_child_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> third_child_;
+};
+
+std::shared_ptr<nav2_behavior_tree::PipelineSequence>
+PipelineSequenceTestFixture::bt_node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+PipelineSequenceTestFixture::first_child_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+PipelineSequenceTestFixture::second_child_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+PipelineSequenceTestFixture::third_child_ = nullptr;
+
+TEST_F(PipelineSequenceTestFixture, test_failure_on_idle_child)
+{
+  first_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_THROW(bt_node_->executeTick(), std::runtime_error);
+}
+
+TEST_F(PipelineSequenceTestFixture, test_failure)
+{
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+}
+
+TEST_F(PipelineSequenceTestFixture, test_behavior)
+{
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  second_child_->changeStatus(BT::NodeStatus::IDLE);
+  third_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  second_child_->changeStatus(BT::NodeStatus::RUNNING);
+  third_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp

@@ -0,0 +1,160 @@
+// 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 <memory>
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "../../test_dummy_tree_node.hpp"
+#include "nav2_behavior_tree/plugins/control/recovery_node.hpp"
+
+// Changes status to SUCCESS after a specified number of failures
+class RecoveryDummy : public nav2_behavior_tree::DummyNode
+{
+public:
+  BT::NodeStatus tick() override
+  {
+    if (ticks_ == num_success_) {
+      setStatus(BT::NodeStatus::SUCCESS);
+    } else if (ticks_ == num_failure_) {
+      setStatus(BT::NodeStatus::FAILURE);
+    }
+    ticks_++;
+    return status();
+  }
+
+  void returnSuccessOn(int tick)
+  {
+    num_success_ = tick;
+    ticks_ = 0;
+  }
+
+  void returnFailureOn(int tick)
+  {
+    num_failure_ = tick;
+    ticks_ = 0;
+  }
+
+private:
+  int ticks_{0};
+  int num_success_{-1};
+  int num_failure_{-1};
+};
+
+class RecoveryNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp() override
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::RecoveryNode>(
+      "recovery_node", *config_);
+    first_child_ = std::make_shared<RecoveryDummy>();
+    second_child_ = std::make_shared<RecoveryDummy>();
+    bt_node_->addChild(first_child_.get());
+    bt_node_->addChild(second_child_.get());
+  }
+
+  void TearDown() override
+  {
+    first_child_.reset();
+    second_child_.reset();
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::RecoveryNode> bt_node_;
+  static std::shared_ptr<RecoveryDummy> first_child_;
+  static std::shared_ptr<RecoveryDummy> second_child_;
+};
+
+std::shared_ptr<nav2_behavior_tree::RecoveryNode> RecoveryNodeTestFixture::bt_node_ = nullptr;
+std::shared_ptr<RecoveryDummy> RecoveryNodeTestFixture::first_child_ = nullptr;
+std::shared_ptr<RecoveryDummy> RecoveryNodeTestFixture::second_child_ = nullptr;
+
+TEST_F(RecoveryNodeTestFixture, test_only_two_children)
+{
+  nav2_behavior_tree::DummyNode dummy;
+  bt_node_->addChild(&dummy);
+  EXPECT_THROW(bt_node_->executeTick(), BT::BehaviorTreeException);
+}
+
+TEST_F(RecoveryNodeTestFixture, test_running)
+{
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+}
+
+TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child)
+{
+  first_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
+}
+
+TEST_F(RecoveryNodeTestFixture, test_success_one_retry)
+{
+  // first child returns success right away
+  first_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+
+  // first child fails, second child succeeds, then first child succeeds (one retry)
+  first_child_->returnSuccessOn(1);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+}
+
+TEST_F(RecoveryNodeTestFixture, test_failure_one_retry)
+{
+  // first child fails, second child fails
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+
+  // first child fails, second child succeeds, then first child fails (one retry)
+  first_child_->returnFailureOn(1);
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp

@@ -0,0 +1,130 @@
+// 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 <memory>
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "../../test_dummy_tree_node.hpp"
+#include "nav2_behavior_tree/plugins/control/round_robin_node.hpp"
+
+class RoundRobinNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp() override
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::RoundRobinNode>(
+      "round_robin", *config_);
+    first_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    second_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    third_child_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    bt_node_->addChild(first_child_.get());
+    bt_node_->addChild(second_child_.get());
+    bt_node_->addChild(third_child_.get());
+  }
+
+  void TearDown() override
+  {
+    first_child_.reset();
+    second_child_.reset();
+    third_child_.reset();
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::RoundRobinNode> bt_node_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> first_child_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> second_child_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> third_child_;
+};
+
+std::shared_ptr<nav2_behavior_tree::RoundRobinNode> RoundRobinNodeTestFixture::bt_node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode> RoundRobinNodeTestFixture::first_child_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode> RoundRobinNodeTestFixture::second_child_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode> RoundRobinNodeTestFixture::third_child_ = nullptr;
+
+TEST_F(RoundRobinNodeTestFixture, test_failure_on_idle_child)
+{
+  first_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
+}
+
+TEST_F(RoundRobinNodeTestFixture, test_failure)
+{
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  second_child_->changeStatus(BT::NodeStatus::FAILURE);
+  third_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  third_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+}
+
+TEST_F(RoundRobinNodeTestFixture, test_behavior)
+{
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  second_child_->changeStatus(BT::NodeStatus::IDLE);
+  third_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::RUNNING);
+  third_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+
+  first_child_->changeStatus(BT::NodeStatus::IDLE);
+  second_child_->changeStatus(BT::NodeStatus::IDLE);
+  third_child_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::RUNNING);
+  second_child_->changeStatus(BT::NodeStatus::IDLE);
+  third_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  third_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
+  EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt

@@ -0,0 +1,27 @@
+ament_add_gtest(test_decorator_distance_controller 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_decorator_speed_controller test_speed_controller.cpp)
+target_link_libraries(test_decorator_speed_controller nav2_speed_controller_bt_node)
+ament_target_dependencies(test_decorator_speed_controller ${dependencies})
+
+ament_add_gtest(test_decorator_rate_controller test_rate_controller.cpp)
+target_link_libraries(test_decorator_rate_controller nav2_rate_controller_bt_node)
+ament_target_dependencies(test_decorator_rate_controller ${dependencies})
+
+ament_add_gtest(test_goal_updater_node test_goal_updater_node.cpp)
+target_link_libraries(test_goal_updater_node nav2_goal_updater_node_bt_node)
+ament_target_dependencies(test_goal_updater_node ${dependencies})
+
+ament_add_gtest(test_single_trigger_node test_single_trigger_node.cpp)
+target_link_libraries(test_single_trigger_node nav2_single_trigger_bt_node)
+ament_target_dependencies(test_single_trigger_node ${dependencies})
+
+ament_add_gtest(test_goal_updated_controller test_goal_updated_controller.cpp)
+target_link_libraries(test_goal_updated_controller nav2_goal_updated_controller_bt_node)
+ament_target_dependencies(test_goal_updated_controller ${dependencies})
+
+ament_add_gtest(test_decorator_path_longer_on_approach test_path_longer_on_approach.cpp)
+target_link_libraries(test_decorator_path_longer_on_approach nav2_path_longer_on_approach_bt_node)
+ament_target_dependencies(test_decorator_path_longer_on_approach ${dependencies})

navigation2/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp

@@ -0,0 +1,109 @@
+// 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 "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "../../test_dummy_tree_node.hpp"
+#include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp"
+
+class DistanceControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::DistanceController>(
+      "distance_controller", *config_);
+    dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    bt_node_->setChild(dummy_node_.get());
+  }
+
+  void TearDown()
+  {
+    dummy_node_.reset();
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::DistanceController> bt_node_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
+};
+
+std::shared_ptr<nav2_behavior_tree::DistanceController>
+DistanceControllerTestFixture::bt_node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+DistanceControllerTestFixture::dummy_node_ = nullptr;
+
+TEST_F(DistanceControllerTestFixture, test_behavior)
+{
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
+
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_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(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+    } else {
+      EXPECT_EQ(bt_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;
+}

navigation2/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp

@@ -0,0 +1,108 @@
+// 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 "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    // Setting fake goals on blackboard
+    geometry_msgs::msg::PoseStamped goal1;
+    goal1.header.stamp = node_->now();
+    std::vector<geometry_msgs::msg::PoseStamped> poses1;
+    poses1.push_back(goal1);
+    config_->blackboard->set("goal", goal1);
+    config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>("goals", poses1);
+
+    bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedController>(
+      "goal_updated_controller", *config_);
+    dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    bt_node_->setChild(dummy_node_.get());
+  }
+
+  void TearDown()
+  {
+    dummy_node_.reset();
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::GoalUpdatedController> bt_node_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
+};
+
+std::shared_ptr<nav2_behavior_tree::GoalUpdatedController>
+GoalUpdatedControllerTestFixture::bt_node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+GoalUpdatedControllerTestFixture::dummy_node_ = nullptr;
+
+TEST_F(GoalUpdatedControllerTestFixture, test_behavior)
+{
+  // Creating updated fake-goals
+  geometry_msgs::msg::PoseStamped goal2;
+  goal2.header.stamp = node_->now();
+  std::vector<geometry_msgs::msg::PoseStamped> poses2;
+  poses2.push_back(goal2);
+
+  // starting in idle
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
+
+  // tick for the first time, dummy node should be ticked
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
+
+  // tick again with updated goal, dummy node should be ticked
+  config_->blackboard->set("goal", goal2);
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
+
+  // tick again without update, dummy node should not be ticked
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
+
+  // tick again with updated goals, dummy node should be ticked
+  config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>("goals", poses2);
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_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;
+}

navigation2/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp

@@ -0,0 +1,208 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Francisco Martin Rico
+//
+// 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 <memory>
+#include <set>
+#include <string>
+
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_action_server.hpp"
+#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
+
+
+class GoalUpdaterTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("goal_updater_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::GoalUpdater>(
+          name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::GoalUpdater>(
+      "GoalUpdater", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr GoalUpdaterTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * GoalUpdaterTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> GoalUpdaterTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> GoalUpdaterTestFixture::tree_ = nullptr;
+
+TEST_F(GoalUpdaterTestFixture, test_tick)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
+            <AlwaysSuccess/>
+          </GoalUpdater>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  auto goal_updater_pub =
+    node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
+
+  // create new goal and set it on blackboard
+  geometry_msgs::msg::PoseStamped goal;
+  goal.header.stamp = node_->now();
+  goal.pose.position.x = 1.0;
+  config_->blackboard->set("goal", goal);
+
+  // tick tree without publishing updated goal and get updated_goal
+  tree_->rootNode()->executeTick();
+  geometry_msgs::msg::PoseStamped updated_goal;
+  config_->blackboard->get("updated_goal", updated_goal);
+}
+
+TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
+            <AlwaysSuccess/>
+          </GoalUpdater>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  auto goal_updater_pub =
+    node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
+
+  // create new goal and set it on blackboard
+  geometry_msgs::msg::PoseStamped goal;
+  goal.header.stamp = node_->now();
+  goal.pose.position.x = 1.0;
+  config_->blackboard->set("goal", goal);
+
+  // publish updated_goal older than goal
+  geometry_msgs::msg::PoseStamped goal_to_update;
+  goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0);
+  goal_to_update.pose.position.x = 2.0;
+
+  goal_updater_pub->publish(goal_to_update);
+  tree_->rootNode()->executeTick();
+  geometry_msgs::msg::PoseStamped updated_goal;
+  config_->blackboard->get("updated_goal", updated_goal);
+
+  // expect to succeed and not update goal
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(updated_goal, goal);
+}
+
+TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
+            <AlwaysSuccess/>
+          </GoalUpdater>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  auto goal_updater_pub =
+    node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
+
+  // create new goal and set it on blackboard
+  geometry_msgs::msg::PoseStamped goal;
+  goal.header.stamp = node_->now();
+  goal.pose.position.x = 1.0;
+  config_->blackboard->set("goal", goal);
+
+  // publish updated_goal older than goal
+  geometry_msgs::msg::PoseStamped goal_to_update_1;
+  goal_to_update_1.header.stamp = node_->now();
+  goal_to_update_1.pose.position.x = 2.0;
+
+  geometry_msgs::msg::PoseStamped goal_to_update_2;
+  goal_to_update_2.header.stamp = node_->now();
+  goal_to_update_2.pose.position.x = 3.0;
+
+  goal_updater_pub->publish(goal_to_update_1);
+  goal_updater_pub->publish(goal_to_update_2);
+  tree_->rootNode()->executeTick();
+  geometry_msgs::msg::PoseStamped updated_goal;
+  config_->blackboard->get("updated_goal", updated_goal);
+
+  // expect to succeed
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  // expect to update goal with latest goal update
+  EXPECT_EQ(updated_goal, goal_to_update_2);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp

@@ -0,0 +1,163 @@
+// Copyright (c) 2022 Neobotix GmbH
+//
+// 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 <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "../../test_dummy_tree_node.hpp"
+#include "nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class PathLongerOnApproachTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("path_longer_on_approach_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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",
+      node_);
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::PathLongerOnApproach>(
+          name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::PathLongerOnApproach>(
+      "PathLongerOnApproach", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr PathLongerOnApproachTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * PathLongerOnApproachTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> PathLongerOnApproachTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> PathLongerOnApproachTestFixture::tree_ = nullptr;
+
+TEST_F(PathLongerOnApproachTestFixture, test_tick)
+{
+  // Success test
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <PathLongerOnApproach path="{path}" prox_len="5.0" length_factor="2.0">
+            <AlwaysSuccess/>
+          </PathLongerOnApproach>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // set new path on blackboard
+  nav_msgs::msg::Path new_path;
+  new_path.poses.resize(10);
+  for (unsigned int i = 0; i < new_path.poses.size(); i++) {
+    // Assuming distance between waypoints to be 1.5m
+    new_path.poses[i].pose.position.x = 1.5 * i;
+  }
+  config_->blackboard->set<nav_msgs::msg::Path>("path", new_path);
+
+  tree_->rootNode()->executeTick();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+
+  // Failure test
+  // create tree
+  xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <PathLongerOnApproach path="{path}" prox_len="20.0" length_factor="1.0">
+            <AlwaysFailure/>
+          </PathLongerOnApproach>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // set old path on blackboard
+  nav_msgs::msg::Path old_path;
+  old_path.poses.resize(5);
+  for (unsigned int i = 1; i <= old_path.poses.size(); i++) {
+    // Assuming distance between waypoints to be 3.0m
+    old_path.poses[i - 1].pose.position.x = 3.0 * i;
+  }
+  config_->blackboard->set<nav_msgs::msg::Path>("path", old_path);
+  tree_->rootNode()->executeTick();
+
+  // set new path on blackboard
+  new_path.poses.resize(11);
+  for (unsigned int i = 0; i <= new_path.poses.size(); i++) {
+    // Assuming distance between waypoints to be 1.5m
+    new_path.poses[i].pose.position.x = 1.5 * i;
+  }
+  config_->blackboard->set<nav_msgs::msg::Path>("path", new_path);
+  tree_->rootNode()->executeTick();
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_behavior_tree/test/plugins/decorator/test_rate_controller.cpp

@@ -0,0 +1,87 @@
+// 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 "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/decorator/rate_controller.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class RateControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::RateController>(
+      "rate_controller", *config_);
+    dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    bt_node_->setChild(dummy_node_.get());
+  }
+
+  void TearDown()
+  {
+    dummy_node_.reset();
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::RateController> bt_node_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
+};
+
+std::shared_ptr<nav2_behavior_tree::RateController>
+RateControllerTestFixture::bt_node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+RateControllerTestFixture::dummy_node_ = nullptr;
+
+TEST_F(RateControllerTestFixture, test_behavior)
+{
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
+
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
+
+  for (int i = 0; i < 10; ++i) {
+    dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+    std::this_thread::sleep_for(500ms);
+    if (i % 2) {
+      EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+      EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
+    } else {
+      EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+    }
+  }
+}
+
+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;
+}

navigation2/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp

@@ -0,0 +1,94 @@
+// 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 "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class SingleTriggerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    bt_node_ = std::make_shared<nav2_behavior_tree::SingleTrigger>(
+      "single_trigger", *config_);
+    dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    bt_node_->setChild(dummy_node_.get());
+  }
+
+  void TearDown()
+  {
+    dummy_node_.reset();
+    bt_node_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::SingleTrigger> bt_node_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
+};
+
+std::shared_ptr<nav2_behavior_tree::SingleTrigger>
+SingleTriggerTestFixture::bt_node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+SingleTriggerTestFixture::dummy_node_ = nullptr;
+
+TEST_F(SingleTriggerTestFixture, test_behavior)
+{
+  // starting in idle
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
+
+  // tick once, should work
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
+
+  // tick again with dummy node success, should fail
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+
+  // tick again with dummy node idle, should still fail
+  dummy_node_->changeStatus(BT::NodeStatus::IDLE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
+
+  // halt BT for a new execution run, should work when dummy node is running
+  // and once when dummy node returns success and then fail
+  bt_node_->halt();
+  dummy_node_->changeStatus(BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_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;
+}

navigation2/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp

@@ -0,0 +1,149 @@
+// 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_behavior_tree_fixture.hpp"
+#include "../../test_dummy_tree_node.hpp"
+#include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp"
+
+using namespace std::chrono;  // NOLINT
+using namespace std::chrono_literals;  // NOLINT
+
+class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  void SetUp()
+  {
+    odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(node_);
+    config_->blackboard->set<std::shared_ptr<nav2_util::OdomSmoother>>(
+      "odom_smoother", odom_smoother_);  // NOLINT
+
+    geometry_msgs::msg::PoseStamped goal;
+    goal.header.stamp = node_->now();
+    config_->blackboard->set("goal", goal);
+
+    std::vector<geometry_msgs::msg::PoseStamped> fake_poses;
+    config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>("goals", fake_poses);  // NOLINT
+
+    bt_node_ = std::make_shared<nav2_behavior_tree::SpeedController>("speed_controller", *config_);
+    dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();
+    bt_node_->setChild(dummy_node_.get());
+  }
+
+  void TearDown()
+  {
+    dummy_node_.reset();
+    bt_node_.reset();
+    odom_smoother_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
+  static std::shared_ptr<nav2_behavior_tree::SpeedController> bt_node_;
+  static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
+};
+
+std::shared_ptr<nav2_util::OdomSmoother>
+SpeedControllerTestFixture::odom_smoother_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::SpeedController>
+SpeedControllerTestFixture::bt_node_ = nullptr;
+std::shared_ptr<nav2_behavior_tree::DummyNode>
+SpeedControllerTestFixture::dummy_node_ = nullptr;
+
+/*
+ * Test for speed controller behavior
+ * Speed controller calculates the period after which it should succeed
+ * based on the current velocity which is scaled to a pre-defined rate range
+ * Current velocity is set using odom messages
+ * The period is reset on the basis of current velocity after the last period is exceeded
+ */
+TEST_F(SpeedControllerTestFixture, test_behavior)
+{
+  auto odom_pub = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 1);
+  nav_msgs::msg::Odometry odom_msg;
+
+  auto time = node_->now();
+  odom_msg.header.stamp = time;
+  odom_msg.twist.twist.linear.x = 0.223;
+  odom_pub->publish(odom_msg);
+
+  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
+
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
+
+  // after the first tick, period should be a default value of 1s
+  // first tick should return running since period has not exceeded
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  // set the child node to success so node can return success
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+
+  // should return success since period has exceeded and new period should be set to ~2s
+  rclcpp::sleep_for(1s);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+
+  // send new velocity for update after the next period
+  odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.5);
+  odom_msg.twist.twist.linear.x = 0;
+  odom_msg.twist.twist.linear.y = 0;
+  odom_pub->publish(odom_msg);
+
+  // Period should be set to ~2s based on the last speed of 0.223 m/s
+  rclcpp::sleep_for(1s);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+  rclcpp::sleep_for(1s);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
+
+  // period should be set to ~10s based on the last speed of 0 m/s
+  // should return running for the first 9 seconds
+  for (int i = 0; i < 9; ++i) {
+    rclcpp::sleep_for(1s);
+    EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  }
+
+  // set the child node to success so node can return success
+  dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
+
+  // should return success since period has exceeded
+  rclcpp::sleep_for(1s);
+  EXPECT_EQ(bt_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;
+}