add humble-navigation2

2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
@@ -0,0 +1,62 @@
+// 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 <string>
+#include <memory>
+
+#include "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+AssistedTeleopAction::AssistedTeleopAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf)
+{
+  double time_allowance;
+  getInput("time_allowance", time_allowance);
+  getInput("is_recovery", is_recovery_);
+
+  // Populate the input message
+  goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+}
+
+void AssistedTeleopAction::on_tick()
+{
+  if (is_recovery_) {
+    increment_recovery_count();
+  }
+}
+
+BT::NodeStatus AssistedTeleopAction::on_aborted()
+{
+  return is_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,47 @@
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+AssistedTeleopCancel::AssistedTeleopCancel(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtCancelActionNode<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf)
+{
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,62 @@
+// Copyright (c) 2018 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 <string>
+#include <memory>
+
+#include "nav2_behavior_tree/plugins/action/back_up_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+BackUpAction::BackUpAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::BackUp>(xml_tag_name, action_name, conf)
+{
+  double dist;
+  getInput("backup_dist", dist);
+  double speed;
+  getInput("backup_speed", speed);
+  double time_allowance;
+  getInput("time_allowance", time_allowance);
+
+  // Populate the input message
+  goal_.target.x = dist;
+  goal_.target.y = 0.0;
+  goal_.target.z = 0.0;
+  goal_.speed = speed;
+  goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+}
+
+void BackUpAction::on_tick()
+{
+  increment_recovery_count();
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,47 @@
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+BackUpCancel::BackUpCancel(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtCancelActionNode<nav2_msgs::action::BackUp>(xml_tag_name, action_name, conf)
+{
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder =
+    [](const std::string & name, const BT::NodeConfiguration & config)
+    {
+      return std::make_unique<nav2_behavior_tree::BackUpCancel>(
+        name, "backup", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::BackUpCancel>(
+    "CancelBackUp", builder);
+}
@@ -0,0 +1,71 @@
+// Copyright (c) 2019 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 <string>
+#include <memory>
+
+#include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
+
+namespace nav2_behavior_tree
+{
+
+ClearEntireCostmapService::ClearEntireCostmapService(
+  const std::string & service_node_name,
+  const BT::NodeConfiguration & conf)
+: BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, conf)
+{
+}
+
+void ClearEntireCostmapService::on_tick()
+{
+  increment_recovery_count();
+}
+
+ClearCostmapExceptRegionService::ClearCostmapExceptRegionService(
+  const std::string & service_node_name,
+  const BT::NodeConfiguration & conf)
+: BtServiceNode<nav2_msgs::srv::ClearCostmapExceptRegion>(service_node_name, conf)
+{
+}
+
+void ClearCostmapExceptRegionService::on_tick()
+{
+  getInput("reset_distance", request_->reset_distance);
+  increment_recovery_count();
+}
+
+ClearCostmapAroundRobotService::ClearCostmapAroundRobotService(
+  const std::string & service_node_name,
+  const BT::NodeConfiguration & conf)
+: BtServiceNode<nav2_msgs::srv::ClearCostmapAroundRobot>(service_node_name, conf)
+{
+}
+
+void ClearCostmapAroundRobotService::on_tick()
+{
+  getInput("reset_distance", request_->reset_distance);
+  increment_recovery_count();
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>("ClearEntireCostmap");
+  factory.registerNodeType<nav2_behavior_tree::ClearCostmapExceptRegionService>(
+    "ClearCostmapExceptRegion");
+  factory.registerNodeType<nav2_behavior_tree::ClearCostmapAroundRobotService>(
+    "ClearCostmapAroundRobot");
+}
@@ -0,0 +1,75 @@
+// 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 <memory>
+#include <string>
+#include <vector>
+
+#include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+ComputePathThroughPosesAction::ComputePathThroughPosesAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::ComputePathThroughPoses>(xml_tag_name, action_name, conf)
+{
+}
+
+void ComputePathThroughPosesAction::on_tick()
+{
+  getInput("goals", goal_.goals);
+  getInput("planner_id", goal_.planner_id);
+  if (getInput("start", goal_.start)) {
+    goal_.use_start = true;
+  }
+}
+
+BT::NodeStatus ComputePathThroughPosesAction::on_success()
+{
+  setOutput("path", result_.result->path);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus ComputePathThroughPosesAction::on_aborted()
+{
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus ComputePathThroughPosesAction::on_cancelled()
+{
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,81 @@
+// Copyright (c) 2018 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 <memory>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+ComputePathToPoseAction::ComputePathToPoseAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::ComputePathToPose>(xml_tag_name, action_name, conf)
+{
+}
+
+void ComputePathToPoseAction::on_tick()
+{
+  getInput("goal", goal_.goal);
+  getInput("planner_id", goal_.planner_id);
+  if (getInput("start", goal_.start)) {
+    goal_.use_start = true;
+  }
+}
+
+BT::NodeStatus ComputePathToPoseAction::on_success()
+{
+  setOutput("path", result_.result->path);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus ComputePathToPoseAction::on_aborted()
+{
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus ComputePathToPoseAction::on_cancelled()
+{
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::SUCCESS;
+}
+
+void ComputePathToPoseAction::halt()
+{
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  BtActionNode::halt();
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,47 @@
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/controller_cancel_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+ControllerCancel::ControllerCancel(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtCancelActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
+{
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,91 @@
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+using std::placeholders::_1;
+
+ControllerSelector::ControllerSelector(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::SyncActionNode(name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+
+  getInput("topic_name", topic_name_);
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  controller_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+    topic_name_,
+    qos,
+    std::bind(&ControllerSelector::callbackControllerSelect, this, _1),
+    sub_option);
+}
+
+BT::NodeStatus ControllerSelector::tick()
+{
+  callback_group_executor_.spin_some();
+
+  // This behavior always use the last selected controller received from the topic input.
+  // When no input is specified it uses the default controller.
+  // If the default controller is not specified then we work in "required controller mode":
+  // In this mode, the behavior returns failure if the controller selection is not received from
+  // the topic input.
+  if (last_selected_controller_.empty()) {
+    std::string default_controller;
+    getInput("default_controller", default_controller);
+    if (default_controller.empty()) {
+      return BT::NodeStatus::FAILURE;
+    } else {
+      last_selected_controller_ = default_controller;
+    }
+  }
+
+  setOutput("selected_controller", last_selected_controller_);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+void
+ControllerSelector::callbackControllerSelect(const std_msgs::msg::String::SharedPtr msg)
+{
+  last_selected_controller_ = msg->data;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::ControllerSelector>("ControllerSelector");
+}
@@ -0,0 +1,57 @@
+// Copyright (c) 2018 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 <string>
+#include <memory>
+
+#include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+DriveOnHeadingAction::DriveOnHeadingAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::DriveOnHeading>(xml_tag_name, action_name, conf)
+{
+  double dist;
+  getInput("dist_to_travel", dist);
+  double speed;
+  getInput("speed", speed);
+  double time_allowance;
+  getInput("time_allowance", time_allowance);
+
+  // Populate the input message
+  goal_.target.x = dist;
+  goal_.target.y = 0.0;
+  goal_.target.z = 0.0;
+  goal_.speed = speed;
+  goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,47 @@
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+DriveOnHeadingCancel::DriveOnHeadingCancel(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtCancelActionNode<nav2_msgs::action::DriveOnHeading>(xml_tag_name, action_name, conf)
+{
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder =
+    [](const std::string & name, const BT::NodeConfiguration & config)
+    {
+      return std::make_unique<nav2_behavior_tree::DriveOnHeadingCancel>(
+        name, "drive_on_heading", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::DriveOnHeadingCancel>(
+    "CancelDriveOnHeading", builder);
+}
@@ -0,0 +1,83 @@
+// Copyright (c) 2018 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 <memory>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/action/follow_path_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+FollowPathAction::FollowPathAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
+{
+}
+
+void FollowPathAction::on_tick()
+{
+  getInput("path", goal_.path);
+  getInput("controller_id", goal_.controller_id);
+  getInput("goal_checker_id", goal_.goal_checker_id);
+}
+
+void FollowPathAction::on_wait_for_result(
+  std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback>/*feedback*/)
+{
+  // Grab the new path
+  nav_msgs::msg::Path new_path;
+  getInput("path", new_path);
+
+  // Check if it is not same with the current one
+  if (goal_.path != new_path && new_path != nav_msgs::msg::Path()) {
+    // the action server on the next loop iteration
+    goal_.path = new_path;
+    goal_updated_ = true;
+  }
+
+  std::string new_controller_id;
+  getInput("controller_id", new_controller_id);
+
+  if (goal_.controller_id != new_controller_id) {
+    goal_.controller_id = new_controller_id;
+    goal_updated_ = true;
+  }
+
+  std::string new_goal_checker_id;
+  getInput("goal_checker_id", new_goal_checker_id);
+
+  if (goal_.goal_checker_id != new_goal_checker_id) {
+    goal_.goal_checker_id = new_goal_checker_id;
+    goal_updated_ = true;
+  }
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,79 @@
+// 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 <string>
+#include <memory>
+#include <limits>
+
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+GetPoseFromPath::GetPoseFromPath(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(name, conf)
+{
+}
+
+inline BT::NodeStatus GetPoseFromPath::tick()
+{
+  setStatus(BT::NodeStatus::RUNNING);
+
+  nav_msgs::msg::Path input_path;
+  getInput("path", input_path);
+
+  int pose_index;
+  getInput("index", pose_index);
+
+  if (input_path.poses.empty()) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Account for negative indices
+  if(pose_index < 0) {
+    pose_index = input_path.poses.size() + pose_index;
+  }
+
+  // out of bounds index
+  if(pose_index < 0 || static_cast<unsigned>(pose_index) >= input_path.poses.size()) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // extract pose
+  geometry_msgs::msg::PoseStamped output_pose;
+  output_pose = input_path.poses[pose_index];
+
+  // populate pose frame from path if necessary
+  if(output_pose.header.frame_id.empty()) {
+    output_pose.header.frame_id = input_path.header.frame_id;
+  }
+
+
+  setOutput("pose", output_pose);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GetPoseFromPath>("GetPoseFromPath");
+}
@@ -0,0 +1,82 @@
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+using std::placeholders::_1;
+
+GoalCheckerSelector::GoalCheckerSelector(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::SyncActionNode(name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+
+  getInput("topic_name", topic_name_);
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  goal_checker_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+    topic_name_, qos, std::bind(&GoalCheckerSelector::callbackGoalCheckerSelect, this, _1));
+}
+
+BT::NodeStatus GoalCheckerSelector::tick()
+{
+  rclcpp::spin_some(node_);
+
+  // This behavior always use the last selected goal checker received from the topic input.
+  // When no input is specified it uses the default goal checker.
+  // If the default goal checker is not specified then we work in "required goal checker mode":
+  // In this mode, the behavior returns failure if the goal checker selection is not received from
+  // the topic input.
+  if (last_selected_goal_checker_.empty()) {
+    std::string default_goal_checker;
+    getInput("default_goal_checker", default_goal_checker);
+    if (default_goal_checker.empty()) {
+      return BT::NodeStatus::FAILURE;
+    } else {
+      last_selected_goal_checker_ = default_goal_checker;
+    }
+  }
+
+  setOutput("selected_goal_checker", last_selected_goal_checker_);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+void
+GoalCheckerSelector::callbackGoalCheckerSelect(const std_msgs::msg::String::SharedPtr msg)
+{
+  last_selected_goal_checker_ = msg->data;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GoalCheckerSelector>("GoalCheckerSelector");
+}
@@ -0,0 +1,56 @@
+// 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 <memory>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+NavigateThroughPosesAction::NavigateThroughPosesAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::NavigateThroughPoses>(xml_tag_name, action_name, conf)
+{
+}
+
+void NavigateThroughPosesAction::on_tick()
+{
+  if (!getInput("goals", goal_.poses)) {
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "NavigateThroughPosesAction: goal not provided");
+    return;
+  }
+  getInput("behavior_tree", goal_.behavior_tree);
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,56 @@
+// Copyright (c) 2018 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 <memory>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+NavigateToPoseAction::NavigateToPoseAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::NavigateToPose>(xml_tag_name, action_name, conf)
+{
+}
+
+void NavigateToPoseAction::on_tick()
+{
+  if (!getInput("goal", goal_.pose)) {
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "NavigateToPoseAction: goal not provided");
+    return;
+  }
+  getInput("behavior_tree", goal_.behavior_tree);
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,91 @@
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+using std::placeholders::_1;
+
+PlannerSelector::PlannerSelector(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::SyncActionNode(name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+
+  getInput("topic_name", topic_name_);
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  planner_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+    topic_name_,
+    qos,
+    std::bind(&PlannerSelector::callbackPlannerSelect, this, _1),
+    sub_option);
+}
+
+BT::NodeStatus PlannerSelector::tick()
+{
+  callback_group_executor_.spin_some();
+
+  // This behavior always use the last selected planner received from the topic input.
+  // When no input is specified it uses the default planner.
+  // If the default planner is not specified then we work in "required planner mode":
+  // In this mode, the behavior returns failure if the planner selection is not received from
+  // the topic input.
+  if (last_selected_planner_.empty()) {
+    std::string default_planner;
+    getInput("default_planner", default_planner);
+    if (default_planner.empty()) {
+      return BT::NodeStatus::FAILURE;
+    } else {
+      last_selected_planner_ = default_planner;
+    }
+  }
+
+  setOutput("selected_planner", last_selected_planner_);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+void
+PlannerSelector::callbackPlannerSelect(const std_msgs::msg::String::SharedPtr msg)
+{
+  last_selected_planner_ = msg->data;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::PlannerSelector>("PlannerSelector");
+}
@@ -0,0 +1,81 @@
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+using std::placeholders::_1;
+
+ProgressCheckerSelector::ProgressCheckerSelector(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::SyncActionNode(name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+
+  getInput("topic_name", topic_name_);
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  progress_checker_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+    topic_name_, qos, std::bind(&ProgressCheckerSelector::callbackProgressCheckerSelect, this, _1));
+}
+
+BT::NodeStatus ProgressCheckerSelector::tick()
+{
+  rclcpp::spin_some(node_);
+
+  // This behavior always use the last selected progress checker received from the topic input.
+  // When no input is specified it uses the default goaprogressl checker.
+  // If the default progress checker is not specified then we work in
+  // "required progress checker mode": In this mode, the behavior returns failure if the progress
+  // checker selection is not received from the topic input.
+  if (last_selected_progress_checker_.empty()) {
+    std::string default_progress_checker;
+    getInput("default_progress_checker", default_progress_checker);
+    if (default_progress_checker.empty()) {
+      return BT::NodeStatus::FAILURE;
+    } else {
+      last_selected_progress_checker_ = default_progress_checker;
+    }
+  }
+
+  setOutput("selected_progress_checker", last_selected_progress_checker_);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+void
+ProgressCheckerSelector::callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg)
+{
+  last_selected_progress_checker_ = msg->data;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::ProgressCheckerSelector>("ProgressCheckerSelector");
+}
@@ -0,0 +1,34 @@
+// Copyright (c) 2019 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 <string>
+#include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp"
+
+namespace nav2_behavior_tree
+{
+
+ReinitializeGlobalLocalizationService::ReinitializeGlobalLocalizationService(
+  const std::string & service_node_name,
+  const BT::NodeConfiguration & conf)
+: BtServiceNode<std_srvs::srv::Empty>(service_node_name, conf)
+{}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(
+    "ReinitializeGlobalLocalization");
+}
@@ -0,0 +1,86 @@
+// 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 <string>
+#include <memory>
+#include <limits>
+
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+RemovePassedGoals::RemovePassedGoals(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(name, conf),
+  viapoint_achieved_radius_(0.5)
+{
+  getInput("radius", viapoint_achieved_radius_);
+
+  getInput("global_frame", global_frame_);
+  getInput("robot_base_frame", robot_base_frame_);
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+  auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  node->get_parameter("transform_tolerance", transform_tolerance_);
+}
+
+inline BT::NodeStatus RemovePassedGoals::tick()
+{
+  setStatus(BT::NodeStatus::RUNNING);
+
+  Goals goal_poses;
+  getInput("input_goals", goal_poses);
+
+  if (goal_poses.empty()) {
+    setOutput("output_goals", goal_poses);
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  using namespace nav2_util::geometry_utils;  // NOLINT
+
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  double dist_to_goal;
+  while (goal_poses.size() > 1) {
+    dist_to_goal = euclidean_distance(goal_poses[0].pose, current_pose.pose);
+
+    if (dist_to_goal > viapoint_achieved_radius_) {
+      break;
+    }
+
+    goal_poses.erase(goal_poses.begin());
+  }
+
+  setOutput("output_goals", goal_poses);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::RemovePassedGoals>("RemovePassedGoals");
+}
@@ -0,0 +1,64 @@
+// Copyright (c) 2021 RoboTech Vision
+// Copyright (c) 2018 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 <memory>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+SmoothPathAction::SmoothPathAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::SmoothPath>(xml_tag_name, action_name, conf)
+{
+}
+
+void SmoothPathAction::on_tick()
+{
+  getInput("unsmoothed_path", goal_.path);
+  getInput("smoother_id", goal_.smoother_id);
+  double max_smoothing_duration;
+  getInput("max_smoothing_duration", max_smoothing_duration);
+  goal_.max_smoothing_duration = rclcpp::Duration::from_seconds(max_smoothing_duration);
+  getInput("check_for_collisions", goal_.check_for_collisions);
+}
+
+BT::NodeStatus SmoothPathAction::on_success()
+{
+  setOutput("smoothed_path", result_.result->path);
+  setOutput("smoothing_duration", rclcpp::Duration(result_.result->smoothing_duration).seconds());
+  setOutput("was_completed", result_.result->was_completed);
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,92 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Pablo Iñigo Blasco
+// Copyright (c) 2022 Owen Hooper
+//
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+using std::placeholders::_1;
+
+SmootherSelector::SmootherSelector(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::SyncActionNode(name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+
+  getInput("topic_name", topic_name_);
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  smoother_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+    topic_name_,
+    qos,
+    std::bind(&SmootherSelector::callbackSmootherSelect, this, _1),
+    sub_option);
+}
+
+BT::NodeStatus SmootherSelector::tick()
+{
+  callback_group_executor_.spin_some();
+
+  // This behavior always use the last selected smoother received from the topic input.
+  // When no input is specified it uses the default smoother.
+  // If the default smoother is not specified then we work in "required smoother mode":
+  // In this mode, the behavior returns failure if the smoother selection is not received from
+  // the topic input.
+  if (last_selected_smoother_.empty()) {
+    std::string default_smoother;
+    getInput("default_smoother", default_smoother);
+    if (default_smoother.empty()) {
+      return BT::NodeStatus::FAILURE;
+    } else {
+      last_selected_smoother_ = default_smoother;
+    }
+  }
+
+  setOutput("selected_smoother", last_selected_smoother_);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+void
+SmootherSelector::callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg)
+{
+  last_selected_smoother_ = msg->data;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::SmootherSelector>("SmootherSelector");
+}
@@ -0,0 +1,57 @@
+// Copyright (c) 2018 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 <string>
+#include <memory>
+
+#include "nav2_behavior_tree/plugins/action/spin_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+SpinAction::SpinAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::Spin>(xml_tag_name, action_name, conf)
+{
+  double dist;
+  getInput("spin_dist", dist);
+  double time_allowance;
+  getInput("time_allowance", time_allowance);
+  goal_.target_yaw = dist;
+  goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+  getInput("is_recovery", is_recovery_);
+}
+
+void SpinAction::on_tick()
+{
+  if (is_recovery_) {
+    increment_recovery_count();
+  }
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,47 @@
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+SpinCancel::SpinCancel(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtCancelActionNode<nav2_msgs::action::Spin>(xml_tag_name, action_name, conf)
+{
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,89 @@
+// 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 <string>
+#include <memory>
+#include <limits>
+
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+#include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+TruncatePath::TruncatePath(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(name, conf),
+  distance_(1.0)
+{
+  getInput("distance", distance_);
+}
+
+inline BT::NodeStatus TruncatePath::tick()
+{
+  setStatus(BT::NodeStatus::RUNNING);
+
+  nav_msgs::msg::Path input_path;
+
+  getInput("input_path", input_path);
+
+  if (input_path.poses.empty()) {
+    setOutput("output_path", input_path);
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  geometry_msgs::msg::PoseStamped final_pose = input_path.poses.back();
+
+  double distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
+    input_path.poses.back(), final_pose);
+
+  while (distance_to_goal < distance_ && input_path.poses.size() > 2) {
+    input_path.poses.pop_back();
+    distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
+      input_path.poses.back(), final_pose);
+  }
+
+  double dx = final_pose.pose.position.x - input_path.poses.back().pose.position.x;
+  double dy = final_pose.pose.position.y - input_path.poses.back().pose.position.y;
+
+  double final_angle = atan2(dy, dx);
+
+  if (std::isnan(final_angle) || std::isinf(final_angle)) {
+    RCLCPP_WARN(
+      config().blackboard->get<rclcpp::Node::SharedPtr>("node")->get_logger(),
+      "Final angle is not valid while truncating path. Setting to 0.0");
+    final_angle = 0.0;
+  }
+
+  input_path.poses.back().pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+    final_angle);
+
+  setOutput("output_path", input_path);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::TruncatePath>("TruncatePath");
+}
@@ -0,0 +1,157 @@
+// 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 <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "tf2_ros/create_timer_ros.h"
+
+#include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+TruncatePathLocal::TruncatePathLocal(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(name, conf)
+{
+  tf_buffer_ =
+    config().blackboard->template get<std::shared_ptr<tf2_ros::Buffer>>(
+    "tf_buffer");
+}
+
+inline BT::NodeStatus TruncatePathLocal::tick()
+{
+  setStatus(BT::NodeStatus::RUNNING);
+
+  double distance_forward, distance_backward;
+  geometry_msgs::msg::PoseStamped pose;
+  double angular_distance_weight;
+  double max_robot_pose_search_dist;
+
+  getInput("distance_forward", distance_forward);
+  getInput("distance_backward", distance_backward);
+  getInput("angular_distance_weight", angular_distance_weight);
+  getInput("max_robot_pose_search_dist", max_robot_pose_search_dist);
+
+  bool path_pruning = std::isfinite(max_robot_pose_search_dist);
+  nav_msgs::msg::Path new_path;
+  getInput("input_path", new_path);
+  if (!path_pruning || new_path != path_) {
+    path_ = new_path;
+    closest_pose_detection_begin_ = path_.poses.begin();
+  }
+
+  if (!getRobotPose(path_.header.frame_id, pose)) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  if (path_.poses.empty()) {
+    setOutput("output_path", path_);
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  auto closest_pose_detection_end = path_.poses.end();
+  if (path_pruning) {
+    closest_pose_detection_end = nav2_util::geometry_utils::first_after_integrated_distance(
+      closest_pose_detection_begin_, path_.poses.end(), max_robot_pose_search_dist);
+  }
+
+  // find the closest pose on the path
+  auto current_pose = nav2_util::geometry_utils::min_by(
+    closest_pose_detection_begin_, closest_pose_detection_end,
+    [&pose, angular_distance_weight](const geometry_msgs::msg::PoseStamped & ps) {
+      return poseDistance(pose, ps, angular_distance_weight);
+    });
+
+  if (path_pruning) {
+    closest_pose_detection_begin_ = current_pose;
+  }
+
+  // expand forwards to extract desired length
+  auto forward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance(
+    current_pose, path_.poses.end(), distance_forward);
+
+  // expand backwards to extract desired length
+  // Note: current_pose + 1 is used because reverse iterator points to a cell before it
+  auto backward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance(
+    std::reverse_iterator(current_pose + 1), path_.poses.rend(), distance_backward);
+
+  nav_msgs::msg::Path output_path;
+  output_path.header = path_.header;
+  output_path.poses = std::vector<geometry_msgs::msg::PoseStamped>(
+    backward_pose_it.base(), forward_pose_it);
+  setOutput("output_path", output_path);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+inline bool TruncatePathLocal::getRobotPose(
+  std::string path_frame_id, geometry_msgs::msg::PoseStamped & pose)
+{
+  if (!getInput("pose", pose)) {
+    std::string robot_frame;
+    if (!getInput("robot_frame", robot_frame)) {
+      RCLCPP_ERROR(
+        config().blackboard->get<rclcpp::Node::SharedPtr>("node")->get_logger(),
+        "Neither pose nor robot_frame specified for %s", name().c_str());
+      return false;
+    }
+    double transform_tolerance;
+    getInput("transform_tolerance", transform_tolerance);
+    if (!nav2_util::getCurrentPose(
+        pose, *tf_buffer_, path_frame_id, robot_frame, transform_tolerance))
+    {
+      RCLCPP_WARN(
+        config().blackboard->get<rclcpp::Node::SharedPtr>("node")->get_logger(),
+        "Failed to lookup current robot pose for %s", name().c_str());
+      return false;
+    }
+  }
+  return true;
+}
+
+double
+TruncatePathLocal::poseDistance(
+  const geometry_msgs::msg::PoseStamped & pose1,
+  const geometry_msgs::msg::PoseStamped & pose2,
+  const double angular_distance_weight)
+{
+  double dx = pose1.pose.position.x - pose2.pose.position.x;
+  double dy = pose1.pose.position.y - pose2.pose.position.y;
+  // taking angular distance into account in addition to spatial distance
+  // (to improve picking a correct pose near cusps and loops)
+  tf2::Quaternion q1;
+  tf2::convert(pose1.pose.orientation, q1);
+  tf2::Quaternion q2;
+  tf2::convert(pose2.pose.orientation, q2);
+  double da = angular_distance_weight * std::abs(q1.angleShortestPath(q2));
+  return std::sqrt(dx * dx + dy * dy + da * da);
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory) {
+  factory.registerNodeType<nav2_behavior_tree::TruncatePathLocal>(
+    "TruncatePathLocal");
+}
@@ -0,0 +1,58 @@
+// Copyright (c) 2018 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 <string>
+#include <memory>
+
+#include "nav2_behavior_tree/plugins/action/wait_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+WaitAction::WaitAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)
+{
+  int duration;
+  getInput("wait_duration", duration);
+  if (duration <= 0) {
+    RCLCPP_WARN(
+      node_->get_logger(), "Wait duration is negative or zero "
+      "(%i). Setting to positive.", duration);
+    duration *= -1;
+  }
+
+  goal_.time.sec = duration;
+}
+
+void WaitAction::on_tick()
+{
+  increment_recovery_count();
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,47 @@
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+WaitCancel::WaitCancel(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtCancelActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)
+{
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  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);
+}
@@ -0,0 +1,86 @@
+// Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <memory>
+
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+DistanceTraveledCondition::DistanceTraveledCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  distance_(1.0),
+  transform_tolerance_(0.1),
+  global_frame_("map"),
+  robot_base_frame_("base_link")
+{
+  getInput("distance", distance_);
+  getInput("global_frame", global_frame_);
+  getInput("robot_base_frame", robot_base_frame_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
+}
+
+BT::NodeStatus DistanceTraveledCondition::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    if (!nav2_util::getCurrentPose(
+        start_pose_, *tf_, global_frame_, robot_base_frame_,
+        transform_tolerance_))
+    {
+      RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+    }
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Determine distance travelled since we've started this iteration
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Get euclidean distance
+  auto travelled = nav2_util::geometry_utils::euclidean_distance(
+    start_pose_.pose, current_pose.pose);
+
+  if (travelled < distance_) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Update start pose
+  start_pose_ = current_pose;
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::DistanceTraveledCondition>("DistanceTraveled");
+}
@@ -0,0 +1,61 @@
+// 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 <vector>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+GloballyUpdatedGoalCondition::GloballyUpdatedGoalCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  first_time(true)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+}
+
+BT::NodeStatus GloballyUpdatedGoalCondition::tick()
+{
+  if (first_time) {
+    first_time = false;
+    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
+    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  geometry_msgs::msg::PoseStamped current_goal;
+  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
+
+  if (goal_ != current_goal || goals_ != current_goals) {
+    goal_ = current_goal;
+    goals_ = current_goals;
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GloballyUpdatedGoalCondition>("GlobalUpdatedGoal");
+}
@@ -0,0 +1,96 @@
+// 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 <string>
+#include <memory>
+
+#include "nav2_util/robot_utils.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/node_utils.hpp"
+
+#include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+GoalReachedCondition::GoalReachedCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  initialized_(false),
+  global_frame_("map"),
+  robot_base_frame_("base_link")
+{
+  getInput("global_frame", global_frame_);
+  getInput("robot_base_frame", robot_base_frame_);
+}
+
+GoalReachedCondition::~GoalReachedCondition()
+{
+  cleanup();
+}
+
+BT::NodeStatus GoalReachedCondition::tick()
+{
+  if (!initialized_) {
+    initialize();
+  }
+
+  if (isGoalReached()) {
+    return BT::NodeStatus::SUCCESS;
+  }
+  return BT::NodeStatus::FAILURE;
+}
+
+void GoalReachedCondition::initialize()
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+
+  nav2_util::declare_parameter_if_not_declared(
+    node_, "goal_reached_tol",
+    rclcpp::ParameterValue(0.25));
+  node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
+
+  initialized_ = true;
+}
+
+bool GoalReachedCondition::isGoalReached()
+{
+  geometry_msgs::msg::PoseStamped current_pose;
+
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_))
+  {
+    RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+    return false;
+  }
+
+  geometry_msgs::msg::PoseStamped goal;
+  getInput("goal", goal);
+  double dx = goal.pose.position.x - current_pose.pose.position.x;
+  double dy = goal.pose.position.y - current_pose.pose.position.y;
+
+  return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_);
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GoalReachedCondition>("GoalReached");
+}
@@ -0,0 +1,56 @@
+// Copyright (c) 2020 Aitor Miguel Blanco
+//
+// 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 <string>
+#include <vector>
+#include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+GoalUpdatedCondition::GoalUpdatedCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf)
+{}
+
+BT::NodeStatus GoalUpdatedCondition::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
+    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    return BT::NodeStatus::FAILURE;
+  }
+
+  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  geometry_msgs::msg::PoseStamped current_goal;
+  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
+
+  if (goal_ != current_goal || goals_ != current_goals) {
+    goal_ = current_goal;
+    goals_ = current_goals;
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GoalUpdatedCondition>("GoalUpdated");
+}
@@ -0,0 +1,34 @@
+// 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 "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node)
+{
+  auto initPoseReceived = tree_node.config().blackboard->get<bool>("initial_pose_received");
+  return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerSimpleCondition(
+    "InitialPoseReceived",
+    std::bind(&nav2_behavior_tree::initialPoseReceived, std::placeholders::_1));
+}
@@ -0,0 +1,66 @@
+// 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 <string>
+
+#include "nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+IsBatteryChargingCondition::IsBatteryChargingCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  battery_topic_("/battery_status"),
+  is_battery_charging_(false)
+{
+  getInput("battery_topic", battery_topic_);
+  auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node->get_node_base_interface());
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  battery_sub_ = node->create_subscription<sensor_msgs::msg::BatteryState>(
+    battery_topic_,
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&IsBatteryChargingCondition::batteryCallback, this, std::placeholders::_1),
+    sub_option);
+}
+
+BT::NodeStatus IsBatteryChargingCondition::tick()
+{
+  callback_group_executor_.spin_some();
+  if (is_battery_charging_) {
+    return BT::NodeStatus::SUCCESS;
+  }
+  return BT::NodeStatus::FAILURE;
+}
+
+void IsBatteryChargingCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg)
+{
+  is_battery_charging_ =
+    (msg->power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING);
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsBatteryChargingCondition>("IsBatteryCharging");
+}
@@ -0,0 +1,74 @@
+// 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 <string>
+
+#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+IsBatteryLowCondition::IsBatteryLowCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  battery_topic_("/battery_status"),
+  min_battery_(0.0),
+  is_voltage_(false),
+  is_battery_low_(false)
+{
+  getInput("min_battery", min_battery_);
+  getInput("battery_topic", battery_topic_);
+  getInput("is_voltage", is_voltage_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
+    battery_topic_,
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1),
+    sub_option);
+}
+
+BT::NodeStatus IsBatteryLowCondition::tick()
+{
+  callback_group_executor_.spin_some();
+  if (is_battery_low_) {
+    return BT::NodeStatus::SUCCESS;
+  }
+  return BT::NodeStatus::FAILURE;
+}
+
+void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg)
+{
+  if (is_voltage_) {
+    is_battery_low_ = msg->voltage <= min_battery_;
+  } else {
+    is_battery_low_ = msg->percentage <= min_battery_;
+  }
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsBatteryLowCondition>("IsBatteryLow");
+}
@@ -0,0 +1,61 @@
+// 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 "nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp"
+#include <chrono>
+#include <memory>
+#include <string>
+
+namespace nav2_behavior_tree
+{
+
+IsPathValidCondition::IsPathValidCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
+
+  server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
+  getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+}
+
+BT::NodeStatus IsPathValidCondition::tick()
+{
+  nav_msgs::msg::Path path;
+  getInput("path", path);
+
+  auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>();
+
+  request->path = path;
+  auto result = client_->async_send_request(request);
+
+  if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) ==
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    if (result.get()->is_valid) {
+      return BT::NodeStatus::SUCCESS;
+    }
+  }
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsPathValidCondition>("IsPathValid");
+}
@@ -0,0 +1,150 @@
+// Copyright (c) 2018 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 <string>
+#include <chrono>
+
+#include "nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp"
+
+using namespace std::chrono_literals; // NOLINT
+
+namespace nav2_behavior_tree
+{
+
+IsStuckCondition::IsStuckCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  is_stuck_(false),
+  odom_history_size_(10),
+  current_accel_(0.0),
+  brake_accel_limit_(-10.0)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+  callback_group_executor_thread = std::thread([this]() {callback_group_executor_.spin();});
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
+    "odom",
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&IsStuckCondition::onOdomReceived, this, std::placeholders::_1),
+    sub_option);
+
+  RCLCPP_DEBUG(node_->get_logger(), "Initialized an IsStuckCondition BT node");
+
+  RCLCPP_INFO_ONCE(node_->get_logger(), "Waiting on odometry");
+}
+
+IsStuckCondition::~IsStuckCondition()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStuckCondition BT node");
+  callback_group_executor_.cancel();
+  callback_group_executor_thread.join();
+}
+
+void IsStuckCondition::onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg)
+{
+  RCLCPP_INFO_ONCE(node_->get_logger(), "Got odometry");
+
+  while (odom_history_.size() >= odom_history_size_) {
+    odom_history_.pop_front();
+  }
+
+  odom_history_.push_back(*msg);
+
+  // TODO(orduno) #383 Move the state calculation and is stuck to robot class
+  updateStates();
+}
+
+BT::NodeStatus IsStuckCondition::tick()
+{
+  // TODO(orduno) #383 Once check for is stuck and state calculations are moved to robot class
+  //              this becomes
+  // if (robot_state_.isStuck()) {
+
+  if (is_stuck_) {
+    logStuck("Robot got stuck!");
+    return BT::NodeStatus::SUCCESS;  // Successfully detected a stuck condition
+  }
+
+  logStuck("Robot is free");
+  return BT::NodeStatus::FAILURE;  // Failed to detected a stuck condition
+}
+
+void IsStuckCondition::logStuck(const std::string & msg) const
+{
+  static std::string prev_msg;
+
+  if (msg == prev_msg) {
+    return;
+  }
+
+  RCLCPP_INFO(node_->get_logger(), "%s", msg.c_str());
+  prev_msg = msg;
+}
+
+void IsStuckCondition::updateStates()
+{
+  // Approximate acceleration
+  // TODO(orduno) #400 Smooth out velocity history for better accel approx.
+  if (odom_history_.size() > 2) {
+    auto curr_odom = odom_history_.end()[-1];
+    double curr_time = static_cast<double>(curr_odom.header.stamp.sec);
+    curr_time += (static_cast<double>(curr_odom.header.stamp.nanosec)) * 1e-9;
+
+    auto prev_odom = odom_history_.end()[-2];
+    double prev_time = static_cast<double>(prev_odom.header.stamp.sec);
+    prev_time += (static_cast<double>(prev_odom.header.stamp.nanosec)) * 1e-9;
+
+    double dt = curr_time - prev_time;
+    double vel_diff = static_cast<double>(
+      curr_odom.twist.twist.linear.x - prev_odom.twist.twist.linear.x);
+    current_accel_ = vel_diff / dt;
+  }
+
+  is_stuck_ = isStuck();
+}
+
+bool IsStuckCondition::isStuck()
+{
+  // TODO(orduno) #400 The robot getting stuck can result on different types of motion
+  // depending on the state prior to getting stuck (sudden change in accel, not moving at all,
+  // random oscillations, etc). For now, we only address the case where there is a sudden
+  // harsh deceleration. A better approach to capture all situations would be to do a forward
+  // simulation of the robot motion and compare it with the actual one.
+
+  // Detect if robot bumped into something by checking for abnormal deceleration
+  if (current_accel_ < brake_accel_limit_) {
+    RCLCPP_DEBUG(
+      node_->get_logger(), "Current deceleration is beyond brake limit."
+      " brake limit: %.2f, current accel: %.2f", brake_accel_limit_, current_accel_);
+
+    return true;
+  }
+
+  return false;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsStuckCondition>("IsStuck");
+}
@@ -0,0 +1,75 @@
+// 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 <string>
+#include <memory>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+
+#include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+PathExpiringTimerCondition::PathExpiringTimerCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  period_(1.0),
+  first_time_(true)
+{
+  getInput("seconds", period_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+}
+
+BT::NodeStatus PathExpiringTimerCondition::tick()
+{
+  if (first_time_) {
+    getInput("path", prev_path_);
+    first_time_ = false;
+    start_ = node_->now();
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Grab the new path
+  nav_msgs::msg::Path path;
+  getInput("path", path);
+
+  // Reset timer if the path has been updated
+  if (prev_path_ != path) {
+    prev_path_ = path;
+    start_ = node_->now();
+  }
+
+  // Determine how long its been since we've started this iteration
+  auto elapsed = node_->now() - start_;
+
+  // Now, get that in seconds
+  auto seconds = elapsed.seconds();
+
+  if (seconds < period_) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  start_ = node_->now();  // Reset the timer
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::PathExpiringTimerCondition>("PathExpiringTimer");
+}
@@ -0,0 +1,64 @@
+// Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <memory>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+
+#include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+TimeExpiredCondition::TimeExpiredCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  period_(1.0)
+{
+  getInput("seconds", period_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  start_ = node_->now();
+}
+
+BT::NodeStatus TimeExpiredCondition::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    start_ = node_->now();
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Determine how long its been since we've started this iteration
+  auto elapsed = node_->now() - start_;
+
+  // Now, get that in seconds
+  auto seconds = elapsed.seconds();
+
+  if (seconds < period_) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  start_ = node_->now();  // Reset the timer
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::TimeExpiredCondition>("TimeExpired");
+}
@@ -0,0 +1,81 @@
+// Copyright (c) 2020 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 <string>
+#include <chrono>
+#include <memory>
+
+#include "nav2_behavior_tree/plugins/condition/transform_available_condition.hpp"
+
+using namespace std::chrono_literals; // NOLINT
+
+namespace nav2_behavior_tree
+{
+
+TransformAvailableCondition::TransformAvailableCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  was_found_(false)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+
+  getInput("child", child_frame_);
+  getInput("parent", parent_frame_);
+
+  if (child_frame_.empty() || parent_frame_.empty()) {
+    RCLCPP_FATAL(
+      node_->get_logger(), "Child frame (%s) or parent frame (%s) were empty.",
+      child_frame_.c_str(), parent_frame_.c_str());
+    exit(-1);
+  }
+
+  RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node");
+}
+
+TransformAvailableCondition::~TransformAvailableCondition()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "Shutting down TransformAvailableCondition BT node");
+}
+
+BT::NodeStatus TransformAvailableCondition::tick()
+{
+  if (was_found_) {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  std::string tf_error;
+  bool found = tf_->canTransform(
+    child_frame_, parent_frame_, tf2::TimePointZero, &tf_error);
+
+  if (found) {
+    was_found_ = true;
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  RCLCPP_INFO(
+    node_->get_logger(), "Transform from %s to %s was not found, tf error: %s",
+    child_frame_.c_str(), parent_frame_.c_str(), tf_error.c_str());
+
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::TransformAvailableCondition>("TransformAvailable");
+}
@@ -0,0 +1,80 @@
+// 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 <stdexcept>
+#include <sstream>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/control/pipeline_sequence.hpp"
+
+namespace nav2_behavior_tree
+{
+
+PipelineSequence::PipelineSequence(const std::string & name)
+: BT::ControlNode(name, {})
+{
+}
+
+PipelineSequence::PipelineSequence(
+  const std::string & name,
+  const BT::NodeConfiguration & config)
+: BT::ControlNode(name, config)
+{
+}
+
+BT::NodeStatus PipelineSequence::tick()
+{
+  for (std::size_t i = 0; i < children_nodes_.size(); ++i) {
+    auto status = children_nodes_[i]->executeTick();
+    switch (status) {
+      case BT::NodeStatus::FAILURE:
+        ControlNode::haltChildren();
+        last_child_ticked_ = 0;  // reset
+        return status;
+      case BT::NodeStatus::SUCCESS:
+        // do nothing and continue on to the next child. If it is the last child
+        // we'll exit the loop and hit the wrap-up code at the end of the method.
+        break;
+      case BT::NodeStatus::RUNNING:
+        if (i >= last_child_ticked_) {
+          last_child_ticked_ = i;
+          return status;
+        }
+        // else do nothing and continue on to the next child
+        break;
+      default:
+        std::stringstream error_msg;
+        error_msg << "Invalid node status. Received status " << status <<
+          "from child " << children_nodes_[i]->name();
+        throw std::runtime_error(error_msg.str());
+    }
+  }
+  // Wrap up.
+  ControlNode::haltChildren();
+  last_child_ticked_ = 0;  // reset
+  return BT::NodeStatus::SUCCESS;
+}
+
+void PipelineSequence::halt()
+{
+  BT::ControlNode::halt();
+  last_child_ticked_ = 0;
+}
+
+}  // namespace nav2_behavior_tree
+
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::PipelineSequence>("PipelineSequence");
+}
@@ -0,0 +1,129 @@
+// 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 <string>
+#include "nav2_behavior_tree/plugins/control/recovery_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+RecoveryNode::RecoveryNode(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ControlNode::ControlNode(name, conf),
+  current_child_idx_(0),
+  number_of_retries_(1),
+  retry_count_(0)
+{
+  getInput("number_of_retries", number_of_retries_);
+}
+
+BT::NodeStatus RecoveryNode::tick()
+{
+  const unsigned children_count = children_nodes_.size();
+
+  if (children_count != 2) {
+    throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children.");
+  }
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  while (current_child_idx_ < children_count && retry_count_ <= number_of_retries_) {
+    TreeNode * child_node = children_nodes_[current_child_idx_];
+    const BT::NodeStatus child_status = child_node->executeTick();
+
+    if (current_child_idx_ == 0) {
+      switch (child_status) {
+        case BT::NodeStatus::SUCCESS:
+          {
+            // reset node and return success when first child returns success
+            halt();
+            return BT::NodeStatus::SUCCESS;
+          }
+
+        case BT::NodeStatus::FAILURE:
+          {
+            if (retry_count_ < number_of_retries_) {
+              // halt first child and tick second child in next iteration
+              ControlNode::haltChild(0);
+              current_child_idx_++;
+              break;
+            } else {
+              // reset node and return failure when max retries has been exceeded
+              halt();
+              return BT::NodeStatus::FAILURE;
+            }
+          }
+
+        case BT::NodeStatus::RUNNING:
+          {
+            return BT::NodeStatus::RUNNING;
+          }
+
+        default:
+          {
+            throw BT::LogicError("A child node must never return IDLE");
+          }
+      }  // end switch
+
+    } else if (current_child_idx_ == 1) {
+      switch (child_status) {
+        case BT::NodeStatus::SUCCESS:
+          {
+            // halt second child, increment recovery count, and tick first child in next iteration
+            ControlNode::haltChild(1);
+            retry_count_++;
+            current_child_idx_--;
+          }
+          break;
+
+        case BT::NodeStatus::FAILURE:
+          {
+            // reset node and return failure if second child fails
+            halt();
+            return BT::NodeStatus::FAILURE;
+          }
+
+        case BT::NodeStatus::RUNNING:
+          {
+            return BT::NodeStatus::RUNNING;
+          }
+
+        default:
+          {
+            throw BT::LogicError("A child node must never return IDLE");
+          }
+      }  // end switch
+    }
+  }  // end while loop
+
+  // reset node and return failure
+  halt();
+  return BT::NodeStatus::FAILURE;
+}
+
+void RecoveryNode::halt()
+{
+  ControlNode::halt();
+  retry_count_ = 0;
+  current_child_idx_ = 0;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::RecoveryNode>("RecoveryNode");
+}
@@ -0,0 +1,93 @@
+// 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 <string>
+
+#include "nav2_behavior_tree/plugins/control/round_robin_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+RoundRobinNode::RoundRobinNode(const std::string & name)
+: BT::ControlNode::ControlNode(name, {})
+{
+}
+
+RoundRobinNode::RoundRobinNode(
+  const std::string & name,
+  const BT::NodeConfiguration & config)
+: BT::ControlNode(name, config)
+{
+}
+
+BT::NodeStatus RoundRobinNode::tick()
+{
+  const auto num_children = children_nodes_.size();
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  while (num_failed_children_ < num_children) {
+    TreeNode * child_node = children_nodes_[current_child_idx_];
+    const BT::NodeStatus child_status = child_node->executeTick();
+
+    switch (child_status) {
+      case BT::NodeStatus::SUCCESS:
+        {
+          // Wrap around to the first child
+          if (++current_child_idx_ >= num_children) {
+            current_child_idx_ = 0;
+          }
+          num_failed_children_ = 0;
+          ControlNode::haltChildren();
+          return BT::NodeStatus::SUCCESS;
+        }
+
+      case BT::NodeStatus::FAILURE:
+        {
+          if (++current_child_idx_ >= num_children) {
+            current_child_idx_ = 0;
+          }
+          num_failed_children_++;
+          break;
+        }
+
+      case BT::NodeStatus::RUNNING:
+        {
+          return BT::NodeStatus::RUNNING;
+        }
+
+      default:
+        {
+          throw BT::LogicError("Invalid status return from BT node");
+        }
+    }
+  }
+
+  halt();
+  return BT::NodeStatus::FAILURE;
+}
+
+void RoundRobinNode::halt()
+{
+  ControlNode::halt();
+  current_child_idx_ = 0;
+  num_failed_children_ = 0;
+}
+
+}  // namespace nav2_behavior_tree
+
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::RoundRobinNode>("RoundRobin");
+}
@@ -0,0 +1,120 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <chrono>
+#include <string>
+#include <memory>
+#include <cmath>
+
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_ros/buffer.h"
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+#include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp"
+
+namespace nav2_behavior_tree
+{
+
+DistanceController::DistanceController(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::DecoratorNode(name, conf),
+  distance_(1.0),
+  global_frame_("map"),
+  robot_base_frame_("base_link"),
+  first_time_(false)
+{
+  getInput("distance", distance_);
+  getInput("global_frame", global_frame_);
+  getInput("robot_base_frame", robot_base_frame_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
+}
+
+inline BT::NodeStatus DistanceController::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    // Reset the starting position since we're starting a new iteration of
+    // the distance controller (moving from IDLE to RUNNING)
+    if (!nav2_util::getCurrentPose(
+        start_pose_, *tf_, global_frame_, robot_base_frame_,
+        transform_tolerance_))
+    {
+      RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+      return BT::NodeStatus::FAILURE;
+    }
+    first_time_ = true;
+  }
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  // Determine distance travelled since we've started this iteration
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Get euclidean distance
+  auto travelled = nav2_util::geometry_utils::euclidean_distance(
+    start_pose_.pose, current_pose.pose);
+
+  // The child gets ticked the first time through and every time the threshold
+  // distance is crossed. In addition, once the child begins to run, it is
+  // ticked each time 'til completion
+  if (first_time_ || (child_node_->status() == BT::NodeStatus::RUNNING) ||
+    travelled >= distance_)
+  {
+    first_time_ = false;
+    const BT::NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state) {
+      case BT::NodeStatus::RUNNING:
+        return BT::NodeStatus::RUNNING;
+
+      case BT::NodeStatus::SUCCESS:
+        if (!nav2_util::getCurrentPose(
+            start_pose_, *tf_, global_frame_, robot_base_frame_,
+            transform_tolerance_))
+        {
+          RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+          return BT::NodeStatus::FAILURE;
+        }
+        return BT::NodeStatus::SUCCESS;
+
+      case BT::NodeStatus::FAILURE:
+      default:
+        return BT::NodeStatus::FAILURE;
+    }
+  }
+
+  return status();
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::DistanceController>("DistanceController");
+}
@@ -0,0 +1,88 @@
+// Copyright (c) 2018 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 <chrono>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "behaviortree_cpp_v3/decorator_node.h"
+#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"
+
+
+namespace nav2_behavior_tree
+{
+
+GoalUpdatedController::GoalUpdatedController(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::DecoratorNode(name, conf)
+{
+}
+
+BT::NodeStatus GoalUpdatedController::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    // Reset since we're starting a new iteration of
+    // the goal updated controller (moving from IDLE to RUNNING)
+
+    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
+    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+
+    goal_was_updated_ = true;
+  }
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  geometry_msgs::msg::PoseStamped current_goal;
+  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
+
+  if (goal_ != current_goal || goals_ != current_goals) {
+    goal_ = current_goal;
+    goals_ = current_goals;
+    goal_was_updated_ = true;
+  }
+
+  // The child gets ticked the first time through and any time the goal has
+  // changed or preempted. In addition, once the child begins to run, it is ticked each time
+  // 'til completion
+  if ((child_node_->status() == BT::NodeStatus::RUNNING) || goal_was_updated_) {
+    goal_was_updated_ = false;
+    const BT::NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state) {
+      case BT::NodeStatus::RUNNING:
+        return BT::NodeStatus::RUNNING;
+
+      case BT::NodeStatus::SUCCESS:
+        return BT::NodeStatus::SUCCESS;
+
+      case BT::NodeStatus::FAILURE:
+      default:
+        return BT::NodeStatus::FAILURE;
+    }
+  }
+
+  return status();
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GoalUpdatedController>("GoalUpdatedController");
+}
@@ -0,0 +1,91 @@
+// 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 <string>
+#include <memory>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+using std::placeholders::_1;
+
+GoalUpdater::GoalUpdater(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::DecoratorNode(name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+
+  std::string goal_updater_topic;
+  node_->get_parameter_or<std::string>("goal_updater_topic", goal_updater_topic, "goal_update");
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  goal_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
+    goal_updater_topic,
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&GoalUpdater::callback_updated_goal, this, _1),
+    sub_option);
+}
+
+inline BT::NodeStatus GoalUpdater::tick()
+{
+  geometry_msgs::msg::PoseStamped goal;
+
+  getInput("input_goal", goal);
+
+  callback_group_executor_.spin_some();
+
+  if (last_goal_received_.header.stamp != rclcpp::Time(0)) {
+    auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp);
+    auto goal_time = rclcpp::Time(goal.header.stamp);
+    if (last_goal_received_time > goal_time) {
+      goal = last_goal_received_;
+    } else {
+      RCLCPP_WARN(
+        node_->get_logger(), "The timestamp of the received goal (%f) is older than the "
+        "current goal (%f). Ignoring the received goal.",
+        last_goal_received_time.seconds(), goal_time.seconds());
+    }
+  }
+
+  setOutput("output_goal", goal);
+  return child_node_->executeTick();
+}
+
+void
+GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
+{
+  last_goal_received_ = *msg;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GoalUpdater>("GoalUpdater");
+}
@@ -0,0 +1,106 @@
+// 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 <string>
+#include <memory>
+#include <vector>
+#include "nav2_util/geometry_utils.hpp"
+
+#include "nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp"
+
+namespace nav2_behavior_tree
+{
+
+PathLongerOnApproach::PathLongerOnApproach(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::DecoratorNode(name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+}
+
+bool PathLongerOnApproach::isPathUpdated(
+  nav_msgs::msg::Path & new_path,
+  nav_msgs::msg::Path & old_path)
+{
+  return old_path.poses.size() != 0 &&
+         new_path.poses.size() != 0 &&
+         new_path.poses.size() != old_path.poses.size() &&
+         old_path.poses.back().pose.position == new_path.poses.back().pose.position;
+}
+
+bool PathLongerOnApproach::isRobotInGoalProximity(
+  nav_msgs::msg::Path & old_path,
+  double & prox_leng)
+{
+  return nav2_util::geometry_utils::calculate_path_length(old_path, 0) < prox_leng;
+}
+
+bool PathLongerOnApproach::isNewPathLonger(
+  nav_msgs::msg::Path & new_path,
+  nav_msgs::msg::Path & old_path,
+  double & length_factor)
+{
+  return nav2_util::geometry_utils::calculate_path_length(new_path, 0) >
+         length_factor * nav2_util::geometry_utils::calculate_path_length(
+    old_path, 0);
+}
+
+inline BT::NodeStatus PathLongerOnApproach::tick()
+{
+  getInput("path", new_path_);
+  getInput("prox_len", prox_len_);
+  getInput("length_factor", length_factor_);
+
+  if (first_time_ == false) {
+    if (old_path_.poses.empty() || new_path_.poses.empty() ||
+      old_path_.poses.back().pose != new_path_.poses.back().pose)
+    {
+      first_time_ = true;
+    }
+  }
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  // Check if the path is updated and valid, compare the old and the new path length,
+  // given the goal proximity and check if the new path is longer
+  if (isPathUpdated(new_path_, old_path_) && isRobotInGoalProximity(old_path_, prox_len_) &&
+    isNewPathLonger(new_path_, old_path_, length_factor_) && !first_time_)
+  {
+    const BT::NodeStatus child_state = child_node_->executeTick();
+    switch (child_state) {
+      case BT::NodeStatus::RUNNING:
+        return child_state;
+      case BT::NodeStatus::SUCCESS:
+      case BT::NodeStatus::FAILURE:
+        old_path_ = new_path_;
+        resetChild();
+        return child_state;
+      default:
+        old_path_ = new_path_;
+        return BT::NodeStatus::FAILURE;
+    }
+  }
+  old_path_ = new_path_;
+  first_time_ = false;
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::PathLongerOnApproach>("PathLongerOnApproach");
+}
@@ -0,0 +1,85 @@
+// Copyright (c) 2018 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 <chrono>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/decorator/rate_controller.hpp"
+
+namespace nav2_behavior_tree
+{
+
+RateController::RateController(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::DecoratorNode(name, conf),
+  first_time_(false)
+{
+  double hz = 1.0;
+  getInput("hz", hz);
+  period_ = 1.0 / hz;
+}
+
+BT::NodeStatus RateController::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    // Reset the starting point since we're starting a new iteration of
+    // the rate controller (moving from IDLE to RUNNING)
+    start_ = std::chrono::high_resolution_clock::now();
+    first_time_ = true;
+  }
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  // Determine how long its been since we've started this iteration
+  auto now = std::chrono::high_resolution_clock::now();
+  auto elapsed = now - start_;
+
+  // Now, get that in seconds
+  typedef std::chrono::duration<float> float_seconds;
+  auto seconds = std::chrono::duration_cast<float_seconds>(elapsed);
+
+  // The child gets ticked the first time through and any time the period has
+  // expired. In addition, once the child begins to run, it is ticked each time
+  // 'til completion
+  if (first_time_ || (child_node_->status() == BT::NodeStatus::RUNNING) ||
+    seconds.count() >= period_)
+  {
+    first_time_ = false;
+    const BT::NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state) {
+      case BT::NodeStatus::RUNNING:
+        return BT::NodeStatus::RUNNING;
+
+      case BT::NodeStatus::SUCCESS:
+        start_ = std::chrono::high_resolution_clock::now();  // Reset the timer
+        return BT::NodeStatus::SUCCESS;
+
+      case BT::NodeStatus::FAILURE:
+      default:
+        return BT::NodeStatus::FAILURE;
+    }
+  }
+
+  return status();
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::RateController>("RateController");
+}
@@ -0,0 +1,69 @@
+// 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 <chrono>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+SingleTrigger::SingleTrigger(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::DecoratorNode(name, conf),
+  first_time_(true)
+{
+}
+
+BT::NodeStatus SingleTrigger::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    first_time_ = true;
+  }
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  if (first_time_) {
+    const BT::NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state) {
+      case BT::NodeStatus::RUNNING:
+        return BT::NodeStatus::RUNNING;
+
+      case BT::NodeStatus::SUCCESS:
+        first_time_ = false;
+        return BT::NodeStatus::SUCCESS;
+
+      case BT::NodeStatus::FAILURE:
+        first_time_ = false;
+        return BT::NodeStatus::FAILURE;
+
+      default:
+        first_time_ = false;
+        return BT::NodeStatus::FAILURE;
+    }
+  }
+
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::SingleTrigger>("SingleTrigger");
+}
@@ -0,0 +1,127 @@
+// 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 <string>
+#include <memory>
+#include <vector>
+#include "nav2_util/geometry_utils.hpp"
+
+#include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp"
+
+namespace nav2_behavior_tree
+{
+
+SpeedController::SpeedController(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::DecoratorNode(name, conf),
+  first_tick_(false),
+  period_(1.0),
+  min_rate_(0.1),
+  max_rate_(1.0),
+  min_speed_(0.0),
+  max_speed_(0.5)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+
+  getInput("min_rate", min_rate_);
+  getInput("max_rate", max_rate_);
+  getInput("min_speed", min_speed_);
+  getInput("max_speed", max_speed_);
+
+  if (min_rate_ <= 0.0 || max_rate_ <= 0.0) {
+    std::string err_msg = "SpeedController node cannot have rate <= 0.0";
+    RCLCPP_FATAL(node_->get_logger(), "%s" ,err_msg.c_str());
+    throw BT::BehaviorTreeException(err_msg);
+  }
+
+  d_rate_ = max_rate_ - min_rate_;
+  d_speed_ = max_speed_ - min_speed_;
+
+  std::string odom_topic;
+  node_->get_parameter_or("odom_topic", odom_topic, std::string("odom"));
+  odom_smoother_ = config().blackboard->get<std::shared_ptr<nav2_util::OdomSmoother>>(
+    "odom_smoother");
+}
+
+inline BT::NodeStatus SpeedController::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    // Reset since we're starting a new iteration of
+    // the speed controller (moving from IDLE to RUNNING)
+    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
+    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    period_ = 1.0 / max_rate_;
+    start_ = node_->now();
+    first_tick_ = true;
+  }
+
+  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  geometry_msgs::msg::PoseStamped current_goal;
+  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
+
+  if (goal_ != current_goal || goals_ != current_goals) {
+    // Reset state and set period to max since we have a new goal
+    period_ = 1.0 / max_rate_;
+    start_ = node_->now();
+    first_tick_ = true;
+    goal_ = current_goal;
+    goals_ = current_goals;
+  }
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  auto elapsed = node_->now() - start_;
+
+  // The child gets ticked the first time through and any time the period has
+  // expired. In addition, once the child begins to run, it is ticked each time
+  // 'til completion
+  if (first_tick_ || (child_node_->status() == BT::NodeStatus::RUNNING) ||
+    elapsed.seconds() >= period_)
+  {
+    first_tick_ = false;
+
+    // update period if the last period is exceeded
+    if (elapsed.seconds() >= period_) {
+      updatePeriod();
+      start_ = node_->now();
+    }
+
+    const BT::NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state) {
+      case BT::NodeStatus::RUNNING:
+        return BT::NodeStatus::RUNNING;
+
+      case BT::NodeStatus::SUCCESS:
+        return BT::NodeStatus::SUCCESS;
+
+      case BT::NodeStatus::FAILURE:
+      default:
+        return BT::NodeStatus::FAILURE;
+    }
+  }
+
+  return status();
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::SpeedController>("SpeedController");
+}