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);
+}