add humble-navigation2

navigation2/nav2_behaviors/CMakeLists.txt

@@ -0,0 +1,157 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_behaviors)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav2_behavior_tree REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(pluginlib REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+set(library_name behavior_server_core)
+set(executable_name behavior_server)
+
+set(dependencies
+  rclcpp
+  rclcpp_action
+  rclcpp_lifecycle
+  rclcpp_components
+  std_msgs
+  nav2_util
+  nav2_behavior_tree
+  nav2_msgs
+  nav_msgs
+  tf2
+  tf2_geometry_msgs
+  geometry_msgs
+  nav2_costmap_2d
+  nav2_core
+  pluginlib
+)
+
+# plugins
+add_library(nav2_spin_behavior SHARED
+  plugins/spin.cpp
+)
+
+ament_target_dependencies(nav2_spin_behavior
+  ${dependencies}
+)
+
+add_library(nav2_wait_behavior SHARED
+plugins/wait.cpp
+)
+
+ament_target_dependencies(nav2_wait_behavior
+${dependencies}
+)
+
+add_library(nav2_drive_on_heading_behavior SHARED
+  plugins/drive_on_heading.cpp
+)
+
+ament_target_dependencies(nav2_drive_on_heading_behavior
+  ${dependencies}
+)
+
+add_library(nav2_back_up_behavior SHARED
+  plugins/back_up.cpp
+)
+
+ament_target_dependencies(nav2_back_up_behavior
+  ${dependencies}
+)
+
+add_library(nav2_assisted_teleop_behavior SHARED
+  plugins/assisted_teleop.cpp
+)
+
+ament_target_dependencies(nav2_assisted_teleop_behavior
+  ${dependencies}
+)
+
+pluginlib_export_plugin_description_file(nav2_core behavior_plugin.xml)
+
+# Library
+add_library(${library_name} SHARED
+  src/behavior_server.cpp
+)
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+# Executable
+add_executable(${executable_name}
+  src/main.cpp
+)
+
+target_link_libraries(${executable_name} ${library_name})
+
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+
+rclcpp_components_register_nodes(${library_name} "behavior_server::BehaviorServer")
+
+install(TARGETS ${library_name}
+                nav2_spin_behavior
+                nav2_wait_behavior
+                nav2_assisted_teleop_behavior
+                nav2_drive_on_heading_behavior
+                nav2_back_up_behavior
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS ${executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(FILES behavior_plugin.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(DIRECTORY plugins/
+  DESTINATION share/${PROJECT_NAME}/plugins/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name}
+  nav2_spin_behavior
+  nav2_wait_behavior
+  nav2_assisted_teleop_behavior
+  nav2_drive_on_heading_behavior
+  nav2_back_up_behavior
+)
+ament_export_dependencies(${dependencies})
+ament_package()

navigation2/nav2_behaviors/README.md

@@ -0,0 +1,15 @@
+# Behaviors
+
+The `nav2_behaviors` package implements a task server for executing behaviors.
+
+The package defines:
+- A `TimedBehavior` template which is used as a base class to implement specific timed behavior action server - but not required.
+- The  `Backup`, `DriveOnHeading`, `Spin` and `Wait` behaviors.
+
+The only required class a behavior must derive from is the `nav2_core/behavior.hpp` class, which implements the pluginlib interface the behavior server will use to dynamically load your behavior. The `nav2_behaviors/timed_behavior.hpp` derives from this class and implements a generic action server for a timed behavior behavior (e.g. calls an implmentation function on a regular time interval to compute a value) but **this is not required** if it is not helpful. A behavior does not even need to be an action if you do not wish, it may be a service or other interface. However, most motion and behavior primitives are probably long-running and make sense to be modeled as actions, so the provided `timed_behavior.hpp` helps in managing the complexity to simplify new behavior development, described more below.
+
+The value of the centralized behavior server is to **share resources** amongst several behaviors that would otherwise be independent nodes. Subscriptions to TF, costmaps, and more can be quite heavy and add non-trivial compute costs to a robot system. By combining these independent behaviors into a single server, they may share these resources while retaining complete independence in execution and interface.
+
+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_behavior_plugin.html).
+
+See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins.

navigation2/nav2_behaviors/behavior_plugin.xml

@@ -0,0 +1,31 @@
+<class_libraries>
+	<library path="nav2_spin_behavior">
+	  <class name="nav2_behaviors/Spin" type="nav2_behaviors::Spin" base_class_type="nav2_core::Behavior">
+	    <description></description>
+	  </class>
+	</library>
+
+	<library path="nav2_back_up_behavior">
+	  <class name="nav2_behaviors/BackUp" type="nav2_behaviors::BackUp" base_class_type="nav2_core::Behavior">
+	    <description></description>
+	  </class>
+	</library>
+
+	<library path="nav2_drive_on_heading_behavior">
+	  <class name="nav2_behaviors/DriveOnHeading" type="nav2_behaviors::DriveOnHeading&lt;&gt;" base_class_type="nav2_core::Behavior">
+	    <description></description>
+	  </class>
+	</library>
+
+	<library path="nav2_wait_behavior">
+	  <class name="nav2_behaviors/Wait" type="nav2_behaviors::Wait" base_class_type="nav2_core::Behavior">
+	    <description></description>
+	  </class>
+	</library>
+
+	<library path="nav2_assisted_teleop_behavior">
+	  <class name="nav2_behaviors/AssistedTeleop" type="nav2_behaviors::AssistedTeleop" base_class_type="nav2_core::Behavior">
+	    <description></description>
+	  </class>
+	</library>
+</class_libraries>

navigation2/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp

@@ -0,0 +1,99 @@
+// 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. Reserved.
+
+
+#include <chrono>
+#include <string>
+#include <memory>
+#include <vector>
+
+#include "nav2_util/lifecycle_node.hpp"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/create_timer_ros.h"
+#include "pluginlib/class_loader.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "nav2_core/behavior.hpp"
+
+#ifndef NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_
+#define NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_
+
+namespace behavior_server
+{
+
+/**
+ * @class behavior_server::BehaviorServer
+ * @brief An server hosting a map of behavior plugins
+ */
+class BehaviorServer : public nav2_util::LifecycleNode
+{
+public:
+  /**
+   * @brief A constructor for behavior_server::BehaviorServer
+   * @param options Additional options to control creation of the node.
+   */
+  explicit BehaviorServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  ~BehaviorServer();
+
+  /**
+   * @brief Loads behavior plugins from parameter file
+   * @return bool if successfully loaded the plugins
+   */
+  bool loadBehaviorPlugins();
+
+protected:
+  /**
+   * @brief Configure lifecycle server
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Activate lifecycle server
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Deactivate lifecycle server
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Cleanup lifecycle server
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Shutdown lifecycle server
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
+
+  // Plugins
+  pluginlib::ClassLoader<nav2_core::Behavior> plugin_loader_;
+  std::vector<pluginlib::UniquePtr<nav2_core::Behavior>> behaviors_;
+  std::vector<std::string> default_ids_;
+  std::vector<std::string> default_types_;
+  std::vector<std::string> behavior_ids_;
+  std::vector<std::string> behavior_types_;
+
+  // Utilities
+  std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
+  std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
+  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
+};
+
+}  // namespace behavior_server
+
+#endif  // NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_

navigation2/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp

@@ -0,0 +1,105 @@
+// 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.
+
+#ifndef NAV2_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_
+#define NAV2_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "std_msgs/msg/empty.hpp"
+#include "nav2_behaviors/timed_behavior.hpp"
+#include "nav2_msgs/action/assisted_teleop.hpp"
+
+namespace nav2_behaviors
+{
+using AssistedTeleopAction = nav2_msgs::action::AssistedTeleop;
+
+/**
+ * @class nav2_behaviors::AssistedTeleop
+ * @brief An action server behavior for assisted teleop
+ */
+class AssistedTeleop : public TimedBehavior<AssistedTeleopAction>
+{
+public:
+  AssistedTeleop();
+
+  /**
+   * @brief Initialization to run behavior
+   * @param command Goal to execute
+   * @return Status of behavior
+   */
+  Status onRun(const std::shared_ptr<const AssistedTeleopAction::Goal> command) override;
+
+  /**
+   * @brief func to run at the completion of the action
+   */
+  void onActionCompletion() override;
+
+  /**
+   * @brief Loop function to run behavior
+   * @return Status of behavior
+   */
+  Status onCycleUpdate() override;
+
+protected:
+  /**
+   * @brief Configuration of behavior action
+   */
+  void onConfigure() override;
+
+  /**
+   * @brief project a position
+   * @param pose initial pose to project
+   * @param twist velocity to project pose by
+   * @param projection_time time to project by
+   */
+  geometry_msgs::msg::Pose2D projectPose(
+    const geometry_msgs::msg::Pose2D & pose,
+    const geometry_msgs::msg::Twist & twist,
+    double projection_time);
+
+  /**
+   * @brief Callback function for velocity subscriber
+   * @param msg received Twist message
+   */
+  void teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+
+  /**
+   * @brief Callback function to preempt assisted teleop
+   * @param msg empty message
+   */
+  void preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr msg);
+
+  AssistedTeleopAction::Feedback::SharedPtr feedback_;
+
+  // parameters
+  double projection_time_;
+  double simulation_time_step_;
+
+  geometry_msgs::msg::Twist teleop_twist_;
+  bool preempt_teleop_{false};
+
+  // subscribers
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr vel_sub_;
+  rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr preempt_teleop_sub_;
+
+  rclcpp::Duration command_time_allowance_{0, 0};
+  rclcpp::Time end_time_;
+};
+}  // namespace nav2_behaviors
+
+#endif  // NAV2_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_

navigation2/nav2_behaviors/include/nav2_behaviors/plugins/back_up.hpp

@@ -0,0 +1,35 @@
+// 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.
+
+#ifndef NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_
+#define NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_
+
+#include <memory>
+
+#include "drive_on_heading.hpp"
+#include "nav2_msgs/action/back_up.hpp"
+
+using BackUpAction = nav2_msgs::action::BackUp;
+
+
+namespace nav2_behaviors
+{
+class BackUp : public DriveOnHeading<nav2_msgs::action::BackUp>
+{
+public:
+  Status onRun(const std::shared_ptr<const BackUpAction::Goal> command) override;
+};
+}
+
+#endif  // NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_

navigation2/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

@@ -0,0 +1,275 @@
+// Copyright (c) 2018 Intel Corporation
+// 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.
+
+#ifndef NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_
+#define NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_
+
+#include <chrono>
+#include <memory>
+#include <utility>
+#include <limits>
+
+#include "nav2_behaviors/timed_behavior.hpp"
+#include "nav2_msgs/action/drive_on_heading.hpp"
+#include "nav2_msgs/action/back_up.hpp"
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_behaviors
+{
+
+/**
+ * @class nav2_behaviors::DriveOnHeading
+ * @brief An action server Behavior for spinning in
+ */
+template<typename ActionT = nav2_msgs::action::DriveOnHeading>
+class DriveOnHeading : public TimedBehavior<ActionT>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behaviors::DriveOnHeading
+   */
+  DriveOnHeading()
+  : TimedBehavior<ActionT>(),
+    feedback_(std::make_shared<typename ActionT::Feedback>()),
+    command_x_(0.0),
+    command_speed_(0.0),
+    simulate_ahead_time_(0.0)
+  {
+  }
+
+  ~DriveOnHeading() = default;
+
+  /**
+   * @brief Initialization to run behavior
+   * @param command Goal to execute
+   * @return Status of behavior
+   */
+  Status onRun(const std::shared_ptr<const typename ActionT::Goal> command) override
+  {
+    if (command->target.y != 0.0 || command->target.z != 0.0) {
+      RCLCPP_INFO(
+        this->logger_,
+        "DrivingOnHeading in Y and Z not supported, will only move in X.");
+      return Status::FAILED;
+    }
+
+    // Ensure that both the speed and direction have the same sign
+    if (!((command->target.x > 0.0) == (command->speed > 0.0)) ) {
+      RCLCPP_ERROR(this->logger_, "Speed and command sign did not match");
+      return Status::FAILED;
+    }
+
+    command_x_ = command->target.x;
+    command_speed_ = command->speed;
+    command_time_allowance_ = command->time_allowance;
+
+    end_time_ = this->clock_->now() + command_time_allowance_;
+
+    if (!nav2_util::getCurrentPose(
+        initial_pose_, *this->tf_, this->global_frame_, this->robot_base_frame_,
+        this->transform_tolerance_))
+    {
+      RCLCPP_ERROR(this->logger_, "Initial robot pose is not available.");
+      return Status::FAILED;
+    }
+
+    return Status::SUCCEEDED;
+  }
+
+  /**
+   * @brief Loop function to run behavior
+   * @return Status of behavior
+   */
+  Status onCycleUpdate() override
+  {
+    rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
+    if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
+      this->stopRobot();
+      RCLCPP_WARN(
+        this->logger_,
+        "Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading");
+      return Status::FAILED;
+    }
+
+    geometry_msgs::msg::PoseStamped current_pose;
+    if (!nav2_util::getCurrentPose(
+        current_pose, *this->tf_, this->global_frame_, this->robot_base_frame_,
+        this->transform_tolerance_))
+    {
+      RCLCPP_ERROR(this->logger_, "Current robot pose is not available.");
+      return Status::FAILED;
+    }
+
+    double diff_x = initial_pose_.pose.position.x - current_pose.pose.position.x;
+    double diff_y = initial_pose_.pose.position.y - current_pose.pose.position.y;
+    double distance = hypot(diff_x, diff_y);
+
+    feedback_->distance_traveled = distance;
+    this->action_server_->publish_feedback(feedback_);
+
+    if (distance >= std::fabs(command_x_)) {
+      this->stopRobot();
+      return Status::SUCCEEDED;
+    }
+
+    auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
+    cmd_vel->linear.y = 0.0;
+    cmd_vel->angular.z = 0.0;
+
+    bool forward = command_speed_ > 0.0;
+    if (acceleration_limit_ == 0.0 || deceleration_limit_ == 0.0) {
+      RCLCPP_INFO_ONCE(this->logger_, "DriveOnHeading: no acceleration or deceleration limits set");
+      cmd_vel->linear.x = command_speed_;
+    } else {
+      double current_speed = last_vel_ == std::numeric_limits<double>::max() ? 0.0 : last_vel_;
+      double min_feasible_speed, max_feasible_speed;
+      if (forward) {
+        min_feasible_speed = current_speed + deceleration_limit_ / this->cycle_frequency_;
+        max_feasible_speed = current_speed + acceleration_limit_ / this->cycle_frequency_;
+      } else {
+        min_feasible_speed = current_speed - acceleration_limit_ / this->cycle_frequency_;
+        max_feasible_speed = current_speed - deceleration_limit_ / this->cycle_frequency_;
+      }
+      cmd_vel->linear.x = std::clamp(command_speed_, min_feasible_speed, max_feasible_speed);
+
+      // Check if we need to slow down to avoid overshooting
+      auto remaining_distance = std::fabs(command_x_) - distance;
+      double max_vel_to_stop = std::sqrt(-2.0 * deceleration_limit_ * remaining_distance);
+      if (max_vel_to_stop < std::abs(cmd_vel->linear.x)) {
+        cmd_vel->linear.x = forward ? max_vel_to_stop : -max_vel_to_stop;
+      }
+    }
+
+    // Ensure we don't go below minimum speed
+    if (std::fabs(cmd_vel->linear.x) < minimum_speed_) {
+      cmd_vel->linear.x = forward ? minimum_speed_ : -minimum_speed_;
+    }
+
+    geometry_msgs::msg::Pose2D pose2d;
+    pose2d.x = current_pose.pose.position.x;
+    pose2d.y = current_pose.pose.position.y;
+    pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
+
+    if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) {
+      this->stopRobot();
+      RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading");
+      return Status::FAILED;
+    }
+
+    last_vel_ = cmd_vel->linear.x;
+    this->vel_pub_->publish(std::move(cmd_vel));
+
+    return Status::RUNNING;
+  }
+
+  void onCleanup() override {last_vel_ = std::numeric_limits<double>::max();}
+
+  void onActionCompletion() override
+  {
+    last_vel_ = std::numeric_limits<double>::max();
+  }
+
+protected:
+  /**
+   * @brief Check if pose is collision free
+   * @param distance Distance to check forward
+   * @param cmd_vel current commanded velocity
+   * @param pose2d Current pose
+   * @return is collision free or not
+   */
+  bool isCollisionFree(
+    const double & distance,
+    geometry_msgs::msg::Twist * cmd_vel,
+    geometry_msgs::msg::Pose2D & pose2d)
+  {
+    // Simulate ahead by simulate_ahead_time_ in this->cycle_frequency_ increments
+    int cycle_count = 0;
+    double sim_position_change;
+    const double diff_dist = abs(command_x_) - distance;
+    const int max_cycle_count = static_cast<int>(this->cycle_frequency_ * simulate_ahead_time_);
+    geometry_msgs::msg::Pose2D init_pose = pose2d;
+    bool fetch_data = true;
+
+    while (cycle_count < max_cycle_count) {
+      sim_position_change = cmd_vel->linear.x * (cycle_count / this->cycle_frequency_);
+      pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta);
+      pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta);
+      cycle_count++;
+
+      if (diff_dist - abs(sim_position_change) <= 0.) {
+        break;
+      }
+
+      if (!this->collision_checker_->isCollisionFree(pose2d, fetch_data)) {
+        return false;
+      }
+      fetch_data = false;
+    }
+    return true;
+  }
+
+  /**
+   * @brief Configuration of behavior action
+   */
+  void onConfigure() override
+  {
+    auto node = this->node_.lock();
+    if (!node) {
+      throw std::runtime_error{"Failed to lock node"};
+    }
+
+    nav2_util::declare_parameter_if_not_declared(
+      node,
+      "simulate_ahead_time", rclcpp::ParameterValue(2.0));
+    node->get_parameter("simulate_ahead_time", simulate_ahead_time_);
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, this->behavior_name_ + ".acceleration_limit",
+      rclcpp::ParameterValue(0.0));
+    nav2_util::declare_parameter_if_not_declared(
+      node, this->behavior_name_ + ".deceleration_limit",
+      rclcpp::ParameterValue(0.0));
+    nav2_util::declare_parameter_if_not_declared(
+      node, this->behavior_name_ + ".minimum_speed",
+      rclcpp::ParameterValue(0.0));
+    node->get_parameter(this->behavior_name_ + ".acceleration_limit", acceleration_limit_);
+    node->get_parameter(this->behavior_name_ + ".deceleration_limit", deceleration_limit_);
+    node->get_parameter(this->behavior_name_ + ".minimum_speed", minimum_speed_);
+    if (acceleration_limit_ < 0.0 || deceleration_limit_ > 0.0) {
+      RCLCPP_ERROR(this->logger_,
+        "DriveOnHeading: acceleration_limit and deceleration_limit must be "
+        "positive and negative respectively");
+      acceleration_limit_ = std::abs(acceleration_limit_);
+      deceleration_limit_ = -std::abs(deceleration_limit_);
+    }
+  }
+
+  typename ActionT::Feedback::SharedPtr feedback_;
+
+  geometry_msgs::msg::PoseStamped initial_pose_;
+  double command_x_;
+  double command_speed_;
+  rclcpp::Duration command_time_allowance_{0, 0};
+  rclcpp::Time end_time_;
+  double simulate_ahead_time_;
+  double acceleration_limit_;
+  double deceleration_limit_;
+  double minimum_speed_;
+  double last_vel_ = std::numeric_limits<double>::max();
+};
+
+}  // namespace nav2_behaviors
+
+#endif  // NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_

navigation2/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp

@@ -0,0 +1,89 @@
+// 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.
+
+#ifndef NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_
+#define NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_
+
+#include <chrono>
+#include <string>
+#include <memory>
+
+#include "nav2_behaviors/timed_behavior.hpp"
+#include "nav2_msgs/action/spin.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+
+namespace nav2_behaviors
+{
+using SpinAction = nav2_msgs::action::Spin;
+
+/**
+ * @class nav2_behaviors::Spin
+ * @brief An action server behavior for spinning in
+ */
+class Spin : public TimedBehavior<SpinAction>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behaviors::Spin
+   */
+  Spin();
+  ~Spin();
+
+  /**
+   * @brief Initialization to run behavior
+   * @param command Goal to execute
+   * @return Status of behavior
+   */
+  Status onRun(const std::shared_ptr<const SpinAction::Goal> command) override;
+
+  /**
+   * @brief Configuration of behavior action
+   */
+  void onConfigure() override;
+
+  /**
+   * @brief Loop function to run behavior
+   * @return Status of behavior
+   */
+  Status onCycleUpdate() override;
+
+protected:
+  /**
+   * @brief Check if pose is collision free
+   * @param distance Distance to check forward
+   * @param cmd_vel current commanded velocity
+   * @param pose2d Current pose
+   * @return is collision free or not
+   */
+  bool isCollisionFree(
+    const double & distance,
+    geometry_msgs::msg::Twist * cmd_vel,
+    geometry_msgs::msg::Pose2D & pose2d);
+
+  SpinAction::Feedback::SharedPtr feedback_;
+
+  double min_rotational_vel_;
+  double max_rotational_vel_;
+  double rotational_acc_lim_;
+  double cmd_yaw_;
+  double prev_yaw_;
+  double relative_yaw_;
+  double simulate_ahead_time_;
+  rclcpp::Duration command_time_allowance_{0, 0};
+  rclcpp::Time end_time_;
+};
+
+}  // namespace nav2_behaviors
+
+#endif  // NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_

navigation2/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp

@@ -0,0 +1,62 @@
+// 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.
+
+#ifndef NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_
+#define NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_
+
+#include <chrono>
+#include <string>
+#include <memory>
+
+#include "nav2_behaviors/timed_behavior.hpp"
+#include "nav2_msgs/action/wait.hpp"
+
+namespace nav2_behaviors
+{
+using WaitAction = nav2_msgs::action::Wait;
+
+/**
+ * @class nav2_behaviors::Wait
+ * @brief An action server behavior for waiting a fixed duration
+ */
+class Wait : public TimedBehavior<WaitAction>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behaviors::Wait
+   */
+  Wait();
+  ~Wait();
+
+  /**
+   * @brief Initialization to run behavior
+   * @param command Goal to execute
+   * @return Status of behavior
+   */
+  Status onRun(const std::shared_ptr<const WaitAction::Goal> command) override;
+
+  /**
+   * @brief Loop function to run behavior
+   * @return Status of behavior
+   */
+  Status onCycleUpdate() override;
+
+protected:
+  rclcpp::Time wait_end_;
+  WaitAction::Feedback::SharedPtr feedback_;
+};
+
+}  // namespace nav2_behaviors
+
+#endif  // NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_

navigation2/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp

@@ -0,0 +1,275 @@
+// 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.
+
+#ifndef NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_
+#define NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_
+
+#include <memory>
+#include <string>
+#include <cmath>
+#include <chrono>
+#include <ctime>
+#include <thread>
+#include <utility>
+
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/create_timer_ros.h"
+#include "geometry_msgs/msg/twist.hpp"
+#include "nav2_util/simple_action_server.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_core/behavior.hpp"
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#include "tf2/utils.h"
+#pragma GCC diagnostic pop
+
+namespace nav2_behaviors
+{
+
+enum class Status : int8_t
+{
+  SUCCEEDED = 1,
+  FAILED = 2,
+  RUNNING = 3,
+};
+
+using namespace std::chrono_literals;  //NOLINT
+
+/**
+ * @class nav2_behaviors::Behavior
+ * @brief An action server Behavior base class implementing the action server and basic factory.
+ */
+template<typename ActionT>
+class TimedBehavior : public nav2_core::Behavior
+{
+public:
+  using ActionServer = nav2_util::SimpleActionServer<ActionT>;
+
+  /**
+   * @brief A TimedBehavior constructor
+   */
+  TimedBehavior()
+  : action_server_(nullptr),
+    cycle_frequency_(10.0),
+    enabled_(false),
+    transform_tolerance_(0.0)
+  {
+  }
+
+  virtual ~TimedBehavior() = default;
+
+  // Derived classes can override this method to catch the command and perform some checks
+  // before getting into the main loop. The method will only be called
+  // once and should return SUCCEEDED otherwise behavior will return FAILED.
+  virtual Status onRun(const std::shared_ptr<const typename ActionT::Goal> command) = 0;
+
+
+  // This is the method derived classes should mainly implement
+  // and will be called cyclically while it returns RUNNING.
+  // Implement the behavior such that it runs some unit of work on each call
+  // and provides a status. The Behavior will finish once SUCCEEDED is returned
+  // It's up to the derived class to define the final commanded velocity.
+  virtual Status onCycleUpdate() = 0;
+
+  // an opportunity for derived classes to do something on configuration
+  // if they chose
+  virtual void onConfigure()
+  {
+  }
+
+  // an opportunity for derived classes to do something on cleanup
+  // if they chose
+  virtual void onCleanup()
+  {
+  }
+
+  // an opportunity for a derived class to do something on action completion
+  virtual void onActionCompletion()
+  {
+  }
+
+  // configure the server on lifecycle setup
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker) override
+  {
+    node_ = parent;
+    auto node = node_.lock();
+    logger_ = node->get_logger();
+    clock_ = node->get_clock();
+
+    RCLCPP_INFO(logger_, "Configuring %s", name.c_str());
+
+    behavior_name_ = name;
+    tf_ = tf;
+
+    node->get_parameter("cycle_frequency", cycle_frequency_);
+    node->get_parameter("global_frame", global_frame_);
+    node->get_parameter("robot_base_frame", robot_base_frame_);
+    node->get_parameter("transform_tolerance", transform_tolerance_);
+
+    action_server_ = std::make_shared<ActionServer>(
+      node, behavior_name_,
+      std::bind(&TimedBehavior::execute, this));
+
+    collision_checker_ = collision_checker;
+
+    vel_pub_ = node->template create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+
+    onConfigure();
+  }
+
+  // Cleanup server on lifecycle transition
+  void cleanup() override
+  {
+    action_server_.reset();
+    vel_pub_.reset();
+    onCleanup();
+  }
+
+  // Activate server on lifecycle transition
+  void activate() override
+  {
+    RCLCPP_INFO(logger_, "Activating %s", behavior_name_.c_str());
+
+    vel_pub_->on_activate();
+    action_server_->activate();
+    enabled_ = true;
+  }
+
+  // Deactivate server on lifecycle transition
+  void deactivate() override
+  {
+    vel_pub_->on_deactivate();
+    action_server_->deactivate();
+    enabled_ = false;
+  }
+
+protected:
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
+
+  std::string behavior_name_;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
+  std::shared_ptr<ActionServer> action_server_;
+  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+
+  double cycle_frequency_;
+  double enabled_;
+  std::string global_frame_;
+  std::string robot_base_frame_;
+  double transform_tolerance_;
+  rclcpp::Duration elasped_time_{0, 0};
+
+  // Clock
+  rclcpp::Clock::SharedPtr clock_;
+
+  // Logger
+  rclcpp::Logger logger_{rclcpp::get_logger("nav2_behaviors")};
+
+  // Main execution callbacks for the action server implementation calling the Behavior's
+  // onRun and cycle functions to execute a specific behavior
+  void execute()
+  {
+    RCLCPP_INFO(logger_, "Running %s", behavior_name_.c_str());
+
+    if (!enabled_) {
+      RCLCPP_WARN(
+        logger_,
+        "Called while inactive, ignoring request.");
+      return;
+    }
+
+    if (onRun(action_server_->get_current_goal()) != Status::SUCCEEDED) {
+      RCLCPP_INFO(
+        logger_,
+        "Initial checks failed for %s", behavior_name_.c_str());
+      action_server_->terminate_current();
+      return;
+    }
+
+    auto start_time = clock_->now();
+
+    // Initialize the ActionT result
+    auto result = std::make_shared<typename ActionT::Result>();
+
+    rclcpp::WallRate loop_rate(cycle_frequency_);
+
+    while (rclcpp::ok()) {
+      elasped_time_ = clock_->now() - start_time;
+      if (action_server_->is_cancel_requested()) {
+        RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str());
+        stopRobot();
+        result->total_elapsed_time = elasped_time_;
+        action_server_->terminate_all(result);
+        onActionCompletion();
+        return;
+      }
+
+      // TODO(orduno) #868 Enable preempting a Behavior on-the-fly without stopping
+      if (action_server_->is_preempt_requested()) {
+        RCLCPP_ERROR(
+          logger_, "Received a preemption request for %s,"
+          " however feature is currently not implemented. Aborting and stopping.",
+          behavior_name_.c_str());
+        stopRobot();
+        result->total_elapsed_time = clock_->now() - start_time;
+        action_server_->terminate_current(result);
+        onActionCompletion();
+        return;
+      }
+
+      switch (onCycleUpdate()) {
+        case Status::SUCCEEDED:
+          RCLCPP_INFO(
+            logger_,
+            "%s completed successfully", behavior_name_.c_str());
+          result->total_elapsed_time = clock_->now() - start_time;
+          action_server_->succeeded_current(result);
+          onActionCompletion();
+          return;
+
+        case Status::FAILED:
+          RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str());
+          result->total_elapsed_time = clock_->now() - start_time;
+          action_server_->terminate_current(result);
+          onActionCompletion();
+          return;
+
+        case Status::RUNNING:
+
+        default:
+          loop_rate.sleep();
+          break;
+      }
+    }
+  }
+
+  // Stop the robot with a commanded velocity
+  void stopRobot()
+  {
+    auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
+    cmd_vel->linear.x = 0.0;
+    cmd_vel->linear.y = 0.0;
+    cmd_vel->angular.z = 0.0;
+
+    vel_pub_->publish(std::move(cmd_vel));
+  }
+};
+
+}  // namespace nav2_behaviors
+
+#endif  // NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_

navigation2/nav2_behaviors/package.xml

@@ -0,0 +1,48 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_behaviors</name>
+  <version>1.1.18</version>
+  <description>TODO</description>
+  <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
+  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_action</build_depend>
+  <build_depend>rclcpp_lifecycle</build_depend>
+  <build_depend>nav2_behavior_tree</build_depend>
+  <build_depend>nav2_util</build_depend>
+  <build_depend>nav2_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav2_costmap_2d</build_depend>
+  <build_depend>nav2_core</build_depend>
+  <build_depend>pluginlib</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_action</exec_depend>
+  <exec_depend>rclcpp_lifecycle</exec_depend>
+  <exec_depend>nav2_behavior_tree</exec_depend>
+  <exec_depend>nav2_util</exec_depend>
+  <exec_depend>nav2_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav2_costmap_2d</exec_depend>
+  <exec_depend>nav2_core</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <nav2_core plugin="${prefix}/behavior_plugin.xml" />
+  </export>
+</package>

navigation2/nav2_behaviors/plugins/assisted_teleop.cpp

@@ -0,0 +1,186 @@
+// 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 <utility>
+
+#include "nav2_behaviors/plugins/assisted_teleop.hpp"
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_behaviors
+{
+AssistedTeleop::AssistedTeleop()
+: TimedBehavior<AssistedTeleopAction>(),
+  feedback_(std::make_shared<AssistedTeleopAction::Feedback>())
+{}
+
+void AssistedTeleop::onConfigure()
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  // set up parameters
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "projection_time", rclcpp::ParameterValue(1.0));
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "simulation_time_step", rclcpp::ParameterValue(0.1));
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "cmd_vel_teleop", rclcpp::ParameterValue(std::string("cmd_vel_teleop")));
+
+  node->get_parameter("projection_time", projection_time_);
+  node->get_parameter("simulation_time_step", simulation_time_step_);
+
+  std::string cmd_vel_teleop;
+  node->get_parameter("cmd_vel_teleop", cmd_vel_teleop);
+
+  vel_sub_ = node->create_subscription<geometry_msgs::msg::Twist>(
+    cmd_vel_teleop, rclcpp::SystemDefaultsQoS(),
+    std::bind(
+      &AssistedTeleop::teleopVelocityCallback,
+      this, std::placeholders::_1));
+
+  preempt_teleop_sub_ = node->create_subscription<std_msgs::msg::Empty>(
+    "preempt_teleop", rclcpp::SystemDefaultsQoS(),
+    std::bind(
+      &AssistedTeleop::preemptTeleopCallback,
+      this, std::placeholders::_1));
+}
+
+Status AssistedTeleop::onRun(const std::shared_ptr<const AssistedTeleopAction::Goal> command)
+{
+  preempt_teleop_ = false;
+  command_time_allowance_ = command->time_allowance;
+  end_time_ = this->clock_->now() + command_time_allowance_;
+  return Status::SUCCEEDED;
+}
+
+void AssistedTeleop::onActionCompletion()
+{
+  teleop_twist_ = geometry_msgs::msg::Twist();
+  preempt_teleop_ = false;
+}
+
+Status AssistedTeleop::onCycleUpdate()
+{
+  feedback_->current_teleop_duration = elasped_time_;
+  action_server_->publish_feedback(feedback_);
+
+  rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
+  if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
+    stopRobot();
+    RCLCPP_WARN_STREAM(
+      logger_,
+      "Exceeded time allowance before reaching the " << behavior_name_.c_str() <<
+        "goal - Exiting " << behavior_name_.c_str());
+    return Status::FAILED;
+  }
+
+  // user states that teleop was successful
+  if (preempt_teleop_) {
+    stopRobot();
+    return Status::SUCCEEDED;
+  }
+
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    RCLCPP_ERROR_STREAM(
+      logger_,
+      "Current robot pose is not available for " <<
+        behavior_name_.c_str());
+    return Status::FAILED;
+  }
+  geometry_msgs::msg::Pose2D projected_pose;
+  projected_pose.x = current_pose.pose.position.x;
+  projected_pose.y = current_pose.pose.position.y;
+  projected_pose.theta = tf2::getYaw(current_pose.pose.orientation);
+
+  geometry_msgs::msg::Twist scaled_twist = teleop_twist_;
+  for (double time = simulation_time_step_; time < projection_time_;
+    time += simulation_time_step_)
+  {
+    projected_pose = projectPose(projected_pose, teleop_twist_, simulation_time_step_);
+
+    if (!collision_checker_->isCollisionFree(projected_pose)) {
+      if (time == simulation_time_step_) {
+        RCLCPP_DEBUG_STREAM_THROTTLE(
+          logger_,
+          *clock_,
+          1000,
+          behavior_name_.c_str() << " collided on first time step, setting velocity to zero");
+        scaled_twist.linear.x = 0.0f;
+        scaled_twist.linear.y = 0.0f;
+        scaled_twist.angular.z = 0.0f;
+        break;
+      } else {
+        RCLCPP_DEBUG_STREAM_THROTTLE(
+          logger_,
+          *clock_,
+          1000,
+          behavior_name_.c_str() << " collision approaching in " << time << " seconds");
+        double scale_factor = time / projection_time_;
+        scaled_twist.linear.x *= scale_factor;
+        scaled_twist.linear.y *= scale_factor;
+        scaled_twist.angular.z *= scale_factor;
+        break;
+      }
+    }
+  }
+  vel_pub_->publish(std::move(scaled_twist));
+
+  return Status::RUNNING;
+}
+
+geometry_msgs::msg::Pose2D AssistedTeleop::projectPose(
+  const geometry_msgs::msg::Pose2D & pose,
+  const geometry_msgs::msg::Twist & twist,
+  double projection_time)
+{
+  geometry_msgs::msg::Pose2D projected_pose = pose;
+
+  projected_pose.x += projection_time * (
+    twist.linear.x * cos(pose.theta) +
+    twist.linear.y * sin(pose.theta));
+
+  projected_pose.y += projection_time * (
+    twist.linear.x * sin(pose.theta) -
+    twist.linear.y * cos(pose.theta));
+
+  projected_pose.theta += projection_time * twist.angular.z;
+
+  return projected_pose;
+}
+
+void AssistedTeleop::teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
+{
+  teleop_twist_ = *msg;
+}
+
+void AssistedTeleop::preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr)
+{
+  preempt_teleop_ = true;
+}
+
+}  // namespace nav2_behaviors
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_behaviors::AssistedTeleop, nav2_core::Behavior)

navigation2/nav2_behaviors/plugins/back_up.cpp

@@ -0,0 +1,50 @@
+// 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 "nav2_behaviors/plugins/back_up.hpp"
+
+namespace nav2_behaviors
+{
+
+Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
+{
+  if (command->target.y != 0.0 || command->target.z != 0.0) {
+    RCLCPP_INFO(
+      logger_,
+      "Backing up in Y and Z not supported, will only move in X.");
+    return Status::FAILED;
+  }
+
+  // Silently ensure that both the speed and direction are negative.
+  command_x_ = -std::fabs(command->target.x);
+  command_speed_ = -std::fabs(command->speed);
+  command_time_allowance_ = command->time_allowance;
+
+  end_time_ = this->clock_->now() + command_time_allowance_;
+
+  if (!nav2_util::getCurrentPose(
+      initial_pose_, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    RCLCPP_ERROR(logger_, "Initial robot pose is not available.");
+    return Status::FAILED;
+  }
+
+  return Status::SUCCEEDED;
+}
+
+}  // namespace nav2_behaviors
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_behaviors::BackUp, nav2_core::Behavior)

navigation2/nav2_behaviors/plugins/drive_on_heading.cpp

@@ -0,0 +1,20 @@
+// Copyright (c) 2018 Intel Corporation
+// 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 <memory>
+#include "nav2_behaviors/plugins/drive_on_heading.hpp"
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_behaviors::DriveOnHeading<>, nav2_core::Behavior)

navigation2/nav2_behaviors/plugins/spin.cpp

@@ -0,0 +1,192 @@
+// Copyright (c) 2018 Intel Corporation, 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 <cmath>
+#include <thread>
+#include <algorithm>
+#include <memory>
+#include <utility>
+
+#include "nav2_behaviors/plugins/spin.hpp"
+#include "tf2/utils.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_util/node_utils.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_behaviors
+{
+
+Spin::Spin()
+: TimedBehavior<SpinAction>(),
+  feedback_(std::make_shared<SpinAction::Feedback>()),
+  min_rotational_vel_(0.0),
+  max_rotational_vel_(0.0),
+  rotational_acc_lim_(0.0),
+  cmd_yaw_(0.0),
+  prev_yaw_(0.0),
+  relative_yaw_(0.0),
+  simulate_ahead_time_(0.0)
+{
+}
+
+Spin::~Spin() = default;
+
+void Spin::onConfigure()
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "simulate_ahead_time", rclcpp::ParameterValue(2.0));
+  node->get_parameter("simulate_ahead_time", simulate_ahead_time_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "max_rotational_vel", rclcpp::ParameterValue(1.0));
+  node->get_parameter("max_rotational_vel", max_rotational_vel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "min_rotational_vel", rclcpp::ParameterValue(0.4));
+  node->get_parameter("min_rotational_vel", min_rotational_vel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    "rotational_acc_lim", rclcpp::ParameterValue(3.2));
+  node->get_parameter("rotational_acc_lim", rotational_acc_lim_);
+}
+
+Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)
+{
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    RCLCPP_ERROR(logger_, "Current robot pose is not available.");
+    return Status::FAILED;
+  }
+
+  prev_yaw_ = tf2::getYaw(current_pose.pose.orientation);
+  relative_yaw_ = 0.0;
+
+  cmd_yaw_ = command->target_yaw;
+  RCLCPP_INFO(
+    logger_, "Turning %0.2f for spin behavior.",
+    cmd_yaw_);
+
+  command_time_allowance_ = command->time_allowance;
+  end_time_ = this->clock_->now() + command_time_allowance_;
+
+  return Status::SUCCEEDED;
+}
+
+Status Spin::onCycleUpdate()
+{
+  rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
+  if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
+    stopRobot();
+    RCLCPP_WARN(
+      logger_,
+      "Exceeded time allowance before reaching the Spin goal - Exiting Spin");
+    return Status::FAILED;
+  }
+
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
+    RCLCPP_ERROR(logger_, "Current robot pose is not available.");
+    return Status::FAILED;
+  }
+
+  const double current_yaw = tf2::getYaw(current_pose.pose.orientation);
+
+  double delta_yaw = current_yaw - prev_yaw_;
+  if (abs(delta_yaw) > M_PI) {
+    delta_yaw = copysign(2 * M_PI - abs(delta_yaw), prev_yaw_);
+  }
+
+  relative_yaw_ += delta_yaw;
+  prev_yaw_ = current_yaw;
+
+  feedback_->angular_distance_traveled = static_cast<float>(relative_yaw_);
+  action_server_->publish_feedback(feedback_);
+
+  double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_);
+  if (remaining_yaw < 1e-6) {
+    stopRobot();
+    return Status::SUCCEEDED;
+  }
+
+  double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw);
+  vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
+
+  auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
+  cmd_vel->angular.z = copysign(vel, cmd_yaw_);
+
+  geometry_msgs::msg::Pose2D pose2d;
+  pose2d.x = current_pose.pose.position.x;
+  pose2d.y = current_pose.pose.position.y;
+  pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
+
+  if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) {
+    stopRobot();
+    RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin");
+    return Status::FAILED;
+  }
+
+  vel_pub_->publish(std::move(cmd_vel));
+
+  return Status::RUNNING;
+}
+
+bool Spin::isCollisionFree(
+  const double & relative_yaw,
+  geometry_msgs::msg::Twist * cmd_vel,
+  geometry_msgs::msg::Pose2D & pose2d)
+{
+  // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments
+  int cycle_count = 0;
+  double sim_position_change;
+  const int max_cycle_count = static_cast<int>(cycle_frequency_ * simulate_ahead_time_);
+  geometry_msgs::msg::Pose2D init_pose = pose2d;
+  bool fetch_data = true;
+
+  while (cycle_count < max_cycle_count) {
+    sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_);
+    pose2d.theta = init_pose.theta + sim_position_change;
+    cycle_count++;
+
+    if (abs(relative_yaw) - abs(sim_position_change) <= 0.) {
+      break;
+    }
+
+    if (!collision_checker_->isCollisionFree(pose2d, fetch_data)) {
+      return false;
+    }
+    fetch_data = false;
+  }
+  return true;
+}
+
+}  // namespace nav2_behaviors
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_behaviors::Spin, nav2_core::Behavior)

navigation2/nav2_behaviors/plugins/wait.cpp

@@ -0,0 +1,55 @@
+// 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 <chrono>
+#include <memory>
+
+#include "nav2_behaviors/plugins/wait.hpp"
+
+namespace nav2_behaviors
+{
+
+Wait::Wait()
+: TimedBehavior<WaitAction>(),
+  feedback_(std::make_shared<WaitAction::Feedback>())
+{
+}
+
+Wait::~Wait() = default;
+
+Status Wait::onRun(const std::shared_ptr<const WaitAction::Goal> command)
+{
+  wait_end_ = node_.lock()->now() + rclcpp::Duration(command->time);
+  return Status::SUCCEEDED;
+}
+
+Status Wait::onCycleUpdate()
+{
+  auto current_point = node_.lock()->now();
+  auto time_left = wait_end_ - current_point;
+
+  feedback_->time_left = time_left;
+  action_server_->publish_feedback(feedback_);
+
+  if (time_left.nanoseconds() > 0) {
+    return Status::RUNNING;
+  } else {
+    return Status::SUCCEEDED;
+  }
+}
+
+}  // namespace nav2_behaviors
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_behaviors::Wait, nav2_core::Behavior)

navigation2/nav2_behaviors/src/behavior_server.cpp

@@ -0,0 +1,192 @@
+// 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. Reserved.
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <utility>
+#include "nav2_util/node_utils.hpp"
+#include "nav2_behaviors/behavior_server.hpp"
+
+namespace behavior_server
+{
+
+BehaviorServer::BehaviorServer(const rclcpp::NodeOptions & options)
+: LifecycleNode("behavior_server", "", options),
+  plugin_loader_("nav2_core", "nav2_core::Behavior"),
+  default_ids_{"spin", "backup", "drive_on_heading", "wait"},
+  default_types_{"nav2_behaviors/Spin",
+    "nav2_behaviors/BackUp",
+    "nav2_behaviors/DriveOnHeading",
+    "nav2_behaviors/Wait"}
+{
+  declare_parameter(
+    "costmap_topic",
+    rclcpp::ParameterValue(std::string("local_costmap/costmap_raw")));
+  declare_parameter(
+    "footprint_topic",
+    rclcpp::ParameterValue(std::string("local_costmap/published_footprint")));
+  declare_parameter("cycle_frequency", rclcpp::ParameterValue(10.0));
+  declare_parameter("behavior_plugins", default_ids_);
+
+  get_parameter("behavior_plugins", behavior_ids_);
+  if (behavior_ids_ == default_ids_) {
+    for (size_t i = 0; i < default_ids_.size(); ++i) {
+      declare_parameter(default_ids_[i] + ".plugin", default_types_[i]);
+    }
+  }
+
+  declare_parameter(
+    "global_frame",
+    rclcpp::ParameterValue(std::string("odom")));
+  declare_parameter(
+    "robot_base_frame",
+    rclcpp::ParameterValue(std::string("base_link")));
+  declare_parameter(
+    "transform_tolerance",
+    rclcpp::ParameterValue(0.1));
+}
+
+
+BehaviorServer::~BehaviorServer()
+{
+  behaviors_.clear();
+}
+
+nav2_util::CallbackReturn
+BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  tf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
+  auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+    get_node_base_interface(),
+    get_node_timers_interface());
+  tf_->setCreateTimerInterface(timer_interface);
+  transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
+
+  std::string costmap_topic, footprint_topic, robot_base_frame;
+  double transform_tolerance;
+  this->get_parameter("costmap_topic", costmap_topic);
+  this->get_parameter("footprint_topic", footprint_topic);
+  this->get_parameter("transform_tolerance", transform_tolerance);
+  this->get_parameter("robot_base_frame", robot_base_frame);
+  costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(
+    shared_from_this(), costmap_topic);
+  footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
+    shared_from_this(), footprint_topic, *tf_, robot_base_frame, transform_tolerance);
+
+  collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
+    *costmap_sub_, *footprint_sub_, this->get_name());
+
+  behavior_types_.resize(behavior_ids_.size());
+  if (!loadBehaviorPlugins()) {
+    return nav2_util::CallbackReturn::FAILURE;
+  }
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+
+bool
+BehaviorServer::loadBehaviorPlugins()
+{
+  auto node = shared_from_this();
+
+  for (size_t i = 0; i != behavior_ids_.size(); i++) {
+    behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]);
+    try {
+      RCLCPP_INFO(
+        get_logger(), "Creating behavior plugin %s of type %s",
+        behavior_ids_[i].c_str(), behavior_types_[i].c_str());
+      behaviors_.push_back(plugin_loader_.createUniqueInstance(behavior_types_[i]));
+      behaviors_.back()->configure(node, behavior_ids_[i], tf_, collision_checker_);
+    } catch (const pluginlib::PluginlibException & ex) {
+      RCLCPP_FATAL(
+        get_logger(), "Failed to create behavior %s of type %s."
+        " Exception: %s", behavior_ids_[i].c_str(), behavior_types_[i].c_str(),
+        ex.what());
+      return false;
+    }
+  }
+
+  return true;
+}
+
+nav2_util::CallbackReturn
+BehaviorServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+  std::vector<pluginlib::UniquePtr<nav2_core::Behavior>>::iterator iter;
+  for (iter = behaviors_.begin(); iter != behaviors_.end(); ++iter) {
+    (*iter)->activate();
+  }
+
+  // create bond connection
+  createBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+BehaviorServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  std::vector<pluginlib::UniquePtr<nav2_core::Behavior>>::iterator iter;
+  for (iter = behaviors_.begin(); iter != behaviors_.end(); ++iter) {
+    (*iter)->deactivate();
+  }
+
+  // destroy bond connection
+  destroyBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+BehaviorServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  std::vector<pluginlib::UniquePtr<nav2_core::Behavior>>::iterator iter;
+  for (iter = behaviors_.begin(); iter != behaviors_.end(); ++iter) {
+    (*iter)->cleanup();
+  }
+
+  behaviors_.clear();
+  transform_listener_.reset();
+  tf_.reset();
+  footprint_sub_.reset();
+  costmap_sub_.reset();
+  collision_checker_.reset();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+BehaviorServer::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+}  // end namespace behavior_server
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(behavior_server::BehaviorServer)

navigation2/nav2_behaviors/src/main.cpp

@@ -0,0 +1,30 @@
+// Copyright (c) 2018 Intel Corporation
+// 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. Reserved.
+
+#include <memory>
+
+#include "nav2_behaviors/behavior_server.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto recoveries_node = std::make_shared<behavior_server::BehaviorServer>();
+
+  rclcpp::spin(recoveries_node->get_node_base_interface());
+  rclcpp::shutdown();
+
+  return 0;
+}

navigation2/nav2_behaviors/test/CMakeLists.txt

@@ -0,0 +1,7 @@
+ament_add_gtest(test_behaviors
+  test_behaviors.cpp
+)
+
+ament_target_dependencies(test_behaviors
+  ${dependencies}
+)

navigation2/nav2_behaviors/test/test_behaviors.cpp

@@ -0,0 +1,268 @@
+// 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. Reserved.
+
+#include <string>
+#include <memory>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "nav2_behaviors/timed_behavior.hpp"
+#include "nav2_msgs/action/dummy_behavior.hpp"
+
+using nav2_behaviors::TimedBehavior;
+using nav2_behaviors::Status;
+using BehaviorAction = nav2_msgs::action::DummyBehavior;
+using ClientGoalHandle = rclcpp_action::ClientGoalHandle<BehaviorAction>;
+
+using namespace std::chrono_literals;
+
+// A behavior for testing the base class
+
+class DummyBehavior : public TimedBehavior<BehaviorAction>
+{
+public:
+  DummyBehavior()
+  : TimedBehavior<BehaviorAction>(),
+    initialized_(false) {}
+
+  ~DummyBehavior() = default;
+
+  Status onRun(const std::shared_ptr<const BehaviorAction::Goal> goal) override
+  {
+    // A normal behavior would catch the command and initialize
+    initialized_ = false;
+    command_ = goal->command.data;
+    start_time_ = std::chrono::system_clock::now();
+
+    // onRun method can have various possible outcomes (success, failure, cancelled)
+    // The output is defined by the tester class on the command string.
+    if (command_ == "Testing success" || command_ == "Testing failure on run") {
+      initialized_ = true;
+      return Status::SUCCEEDED;
+    }
+
+    return Status::FAILED;
+  }
+
+  Status onCycleUpdate() override
+  {
+    // A normal behavior would set the robot in motion in the first call
+    // and check for robot states on subsequent calls to check if the movement
+    // was completed.
+
+    if (command_ != "Testing success" || !initialized_) {
+      return Status::FAILED;
+    }
+
+    // For testing, pretend the robot takes some fixed
+    // amount of time to complete the motion.
+    auto current_time = std::chrono::system_clock::now();
+    auto motion_duration = 1s;
+
+    if (current_time - start_time_ >= motion_duration) {
+      // Movement was completed
+      return Status::SUCCEEDED;
+    }
+
+    return Status::RUNNING;
+  }
+
+private:
+  bool initialized_;
+  std::string command_;
+  std::chrono::system_clock::time_point start_time_;
+};
+
+// Define a test class to hold the context for the tests
+
+class BehaviorTest : public ::testing::Test
+{
+protected:
+  BehaviorTest() {SetUp();}
+  ~BehaviorTest() = default;
+
+  void SetUp() override
+  {
+    node_lifecycle_ =
+      std::make_shared<rclcpp_lifecycle::LifecycleNode>(
+      "LifecycleBehaviorTestNode", rclcpp::NodeOptions());
+    node_lifecycle_->declare_parameter(
+      "costmap_topic",
+      rclcpp::ParameterValue(std::string("local_costmap/costmap_raw")));
+    node_lifecycle_->declare_parameter(
+      "footprint_topic",
+      rclcpp::ParameterValue(std::string("local_costmap/published_footprint")));
+
+    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_lifecycle_->get_clock());
+    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+      node_lifecycle_->get_node_base_interface(),
+      node_lifecycle_->get_node_timers_interface());
+    tf_buffer_->setCreateTimerInterface(timer_interface);
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+    std::string costmap_topic, footprint_topic;
+    node_lifecycle_->get_parameter("costmap_topic", costmap_topic);
+    node_lifecycle_->get_parameter("footprint_topic", footprint_topic);
+    std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_ =
+      std::make_shared<nav2_costmap_2d::CostmapSubscriber>(
+      node_lifecycle_, costmap_topic);
+    std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_ =
+      std::make_shared<nav2_costmap_2d::FootprintSubscriber>(
+      node_lifecycle_, footprint_topic, *tf_buffer_);
+    std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_ =
+      std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
+      *costmap_sub_, *footprint_sub_,
+      node_lifecycle_->get_name());
+
+    behavior_ = std::make_shared<DummyBehavior>();
+    behavior_->configure(node_lifecycle_, "Behavior", tf_buffer_, collision_checker_);
+    behavior_->activate();
+
+    client_ = rclcpp_action::create_client<BehaviorAction>(
+      node_lifecycle_->get_node_base_interface(),
+      node_lifecycle_->get_node_graph_interface(),
+      node_lifecycle_->get_node_logging_interface(),
+      node_lifecycle_->get_node_waitables_interface(), "Behavior");
+    std::cout << "Setup complete." << std::endl;
+  }
+
+  void TearDown() override {}
+
+  bool sendCommand(const std::string & command)
+  {
+    if (!client_->wait_for_action_server(4s)) {
+      std::cout << "Server not up" << std::endl;
+      return false;
+    }
+
+    auto goal = BehaviorAction::Goal();
+    goal.command.data = command;
+    auto future_goal = client_->async_send_goal(goal);
+
+    if (rclcpp::spin_until_future_complete(node_lifecycle_, future_goal) !=
+      rclcpp::FutureReturnCode::SUCCESS)
+    {
+      std::cout << "failed sending goal" << std::endl;
+      // failed sending the goal
+      return false;
+    }
+
+    goal_handle_ = future_goal.get();
+
+    if (!goal_handle_) {
+      std::cout << "goal was rejected" << std::endl;
+      // goal was rejected by the action server
+      return false;
+    }
+
+    return true;
+  }
+
+  Status getOutcome()
+  {
+    if (getResult().code == rclcpp_action::ResultCode::SUCCEEDED) {
+      return Status::SUCCEEDED;
+    }
+
+    return Status::FAILED;
+  }
+
+  ClientGoalHandle::WrappedResult getResult()
+  {
+    std::cout << "Getting async result..." << std::endl;
+    auto future_result = client_->async_get_result(goal_handle_);
+    std::cout << "Waiting on future..." << std::endl;
+    rclcpp::spin_until_future_complete(node_lifecycle_, future_result);
+    std::cout << "future received!" << std::endl;
+    return future_result.get();
+  }
+
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_lifecycle_;
+  std::shared_ptr<DummyBehavior> behavior_;
+  std::shared_ptr<rclcpp_action::Client<BehaviorAction>> client_;
+  std::shared_ptr<rclcpp_action::ClientGoalHandle<BehaviorAction>> goal_handle_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+};
+
+// Define the tests
+
+TEST_F(BehaviorTest, testingSuccess)
+{
+  ASSERT_TRUE(sendCommand("Testing success"));
+  EXPECT_EQ(getOutcome(), Status::SUCCEEDED);
+  SUCCEED();
+}
+
+TEST_F(BehaviorTest, testingFailureOnRun)
+{
+  ASSERT_TRUE(sendCommand("Testing failure on run"));
+  EXPECT_EQ(getOutcome(), Status::FAILED);
+  SUCCEED();
+}
+
+TEST_F(BehaviorTest, testingFailureOnInit)
+{
+  ASSERT_TRUE(sendCommand("Testing failure on init"));
+  EXPECT_EQ(getOutcome(), Status::FAILED);
+  SUCCEED();
+}
+
+TEST_F(BehaviorTest, testingSequentialFailures)
+{
+  ASSERT_TRUE(sendCommand("Testing failure on run"));
+  EXPECT_EQ(getOutcome(), Status::FAILED);
+  SUCCEED();
+}
+
+TEST_F(BehaviorTest, testingTotalElapsedTimeIsGratherThanZeroIfStarted)
+{
+  ASSERT_TRUE(sendCommand("Testing success"));
+  EXPECT_GT(getResult().result->total_elapsed_time.sec, 0.0);
+  SUCCEED();
+}
+
+TEST_F(BehaviorTest, testingTotalElapsedTimeIsZeroIfFailureOnInit)
+{
+  ASSERT_TRUE(sendCommand("Testing failure on init"));
+  EXPECT_EQ(getResult().result->total_elapsed_time.sec, 0.0);
+  SUCCEED();
+}
+
+TEST_F(BehaviorTest, testingTotalElapsedTimeIsZeroIfFailureOnRun)
+{
+  ASSERT_TRUE(sendCommand("Testing failure on run"));
+  EXPECT_EQ(getResult().result->total_elapsed_time.sec, 0.0);
+  SUCCEED();
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}