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,120 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_controller)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(angles REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav_2d_utils REQUIRED)
+find_package(nav_2d_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+set(executable_name controller_server)
+
+add_executable(${executable_name}
+  src/main.cpp
+)
+
+set(library_name ${executable_name}_core)
+
+add_library(${library_name} SHARED
+  src/controller_server.cpp
+)
+
+set(dependencies
+  angles
+  rclcpp
+  rclcpp_action
+  rclcpp_components
+  std_msgs
+  nav2_msgs
+  nav_2d_utils
+  nav_2d_msgs
+  nav2_util
+  nav2_core
+  pluginlib
+)
+
+add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp)
+ament_target_dependencies(simple_progress_checker ${dependencies})
+
+add_library(pose_progress_checker SHARED plugins/pose_progress_checker.cpp)
+target_link_libraries(pose_progress_checker simple_progress_checker)
+ament_target_dependencies(pose_progress_checker ${dependencies})
+
+add_library(simple_goal_checker SHARED plugins/simple_goal_checker.cpp)
+ament_target_dependencies(simple_goal_checker ${dependencies})
+
+add_library(stopped_goal_checker SHARED plugins/stopped_goal_checker.cpp)
+target_link_libraries(stopped_goal_checker simple_goal_checker)
+ament_target_dependencies(stopped_goal_checker ${dependencies})
+
+add_library(position_goal_checker SHARED plugins/position_goal_checker.cpp)
+ament_target_dependencies(position_goal_checker ${dependencies})
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(plugins/test)
+endif()
+
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+
+target_link_libraries(${executable_name} ${library_name})
+
+rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer")
+
+install(TARGETS simple_progress_checker pose_progress_checker simple_goal_checker stopped_goal_checker position_goal_checker ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS ${executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(simple_progress_checker
+  pose_progress_checker
+  simple_goal_checker
+  stopped_goal_checker
+  position_goal_checker
+  ${library_name})
+ament_export_dependencies(${dependencies})
+pluginlib_export_plugin_description_file(nav2_core plugins.xml)
+
+ament_package()
@@ -0,0 +1,9 @@
+# Nav2 Controller
+
+The Nav2 Controller is a Task Server in Nav2 that implements the `nav2_msgs::action::FollowPath` action server.
+
+An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with multiple plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. It also contains progress checkers and goal checker plugins to abstract out that logic from specific controller implementations.
+
+See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available controller plugins. 
+
+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-controller-server.html) for additional parameter descriptions and a [tutorial about writing controller plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html).
@@ -0,0 +1,277 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_
+#define NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_
+
+#include <memory>
+#include <string>
+#include <thread>
+#include <unordered_map>
+#include <vector>
+#include <mutex>
+
+#include "nav2_core/controller.hpp"
+#include "nav2_core/progress_checker.hpp"
+#include "nav2_core/goal_checker.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "tf2_ros/transform_listener.h"
+#include "nav2_msgs/action/follow_path.hpp"
+#include "nav2_msgs/msg/speed_limit.hpp"
+#include "nav_2d_utils/odom_subscriber.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/simple_action_server.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+namespace nav2_controller
+{
+
+class ProgressChecker;
+/**
+ * @class nav2_controller::ControllerServer
+ * @brief This class hosts variety of plugins of different algorithms to
+ * complete control tasks from the exposed FollowPath action server.
+ */
+class ControllerServer : public nav2_util::LifecycleNode
+{
+public:
+  using ControllerMap = std::unordered_map<std::string, nav2_core::Controller::Ptr>;
+  using GoalCheckerMap = std::unordered_map<std::string, nav2_core::GoalChecker::Ptr>;
+
+  /**
+   * @brief Constructor for nav2_controller::ControllerServer
+   * @param options Additional options to control creation of the node.
+   */
+  explicit ControllerServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  /**
+   * @brief Destructor for nav2_controller::ControllerServer
+   */
+  ~ControllerServer();
+
+protected:
+  /**
+   * @brief Configures controller parameters and member variables
+   *
+   * Configures controller plugin and costmap; Initialize odom subscriber,
+   * velocity publisher and follow path action server.
+   * @param state LifeCycle Node's state
+   * @return Success or Failure
+   * @throw pluginlib::PluginlibException When failed to initialize controller
+   * plugin
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Activates member variables
+   *
+   * Activates controller, costmap, velocity publisher and follow path action
+   * server
+   * @param state LifeCycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Deactivates member variables
+   *
+   * Deactivates follow path action server, controller, costmap and velocity
+   * publisher. Before calling deactivate state, velocity is being set to zero.
+   * @param state LifeCycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Calls clean up states and resets member variables.
+   *
+   * Controller and costmap clean up state is called, and resets rest of the
+   * variables
+   * @param state LifeCycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called when in Shutdown state
+   * @param state LifeCycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+  using Action = nav2_msgs::action::FollowPath;
+  using ActionServer = nav2_util::SimpleActionServer<Action>;
+
+  // Our action server implements the FollowPath action
+  std::unique_ptr<ActionServer> action_server_;
+
+  /**
+   * @brief FollowPath action server callback. Handles action server updates and
+   * spins server until goal is reached
+   *
+   * Provides global path to controller received from action client. Twist
+   * velocities for the robot are calculated and published using controller at
+   * the specified rate till the goal is reached.
+   * @throw nav2_core::PlannerException
+   */
+  void computeControl();
+
+  /**
+   * @brief Find the valid controller ID name for the given request
+   *
+   * @param c_name The requested controller name
+   * @param name Reference to the name to use for control if any valid available
+   * @return bool Whether it found a valid controller to use
+   */
+  bool findControllerId(const std::string & c_name, std::string & name);
+
+  /**
+   * @brief Find the valid goal checker ID name for the specified parameter
+   *
+   * @param c_name The goal checker name
+   * @param name Reference to the name to use for goal checking if any valid available
+   * @return bool Whether it found a valid goal checker to use
+   */
+  bool findGoalCheckerId(const std::string & c_name, std::string & name);
+
+  /**
+   * @brief Assigns path to controller
+   * @param path Path received from action server
+   */
+  void setPlannerPath(const nav_msgs::msg::Path & path);
+  /**
+   * @brief Calculates velocity and publishes to "cmd_vel" topic
+   */
+  void computeAndPublishVelocity();
+  /**
+   * @brief Calls setPlannerPath method with an updated path received from
+   * action server
+   */
+  void updateGlobalPath();
+  /**
+   * @brief Calls velocity publisher to publish the velocity on "cmd_vel" topic
+   * @param velocity Twist velocity to be published
+   */
+  void publishVelocity(const geometry_msgs::msg::TwistStamped & velocity);
+  /**
+   * @brief Calls velocity publisher to publish zero velocity
+   */
+  void publishZeroVelocity();
+  /**
+   * @brief Checks if goal is reached
+   * @return true or false
+   */
+  bool isGoalReached();
+  /**
+   * @brief Obtain current pose of the robot
+   * @param pose To store current pose of the robot
+   * @return true if able to obtain current pose of the robot, else false
+   */
+  bool getRobotPose(geometry_msgs::msg::PoseStamped & pose);
+
+  /**
+   * @brief get the thresholded velocity
+   * @param velocity The current velocity from odometry
+   * @param threshold The minimum velocity to return non-zero
+   * @return double velocity value
+   */
+  double getThresholdedVelocity(double velocity, double threshold)
+  {
+    return (std::abs(velocity) > threshold) ? velocity : 0.0;
+  }
+
+  /**
+   * @brief get the thresholded Twist
+   * @param Twist The current Twist from odometry
+   * @return Twist Twist after thresholds applied
+   */
+  nav_2d_msgs::msg::Twist2D getThresholdedTwist(const nav_2d_msgs::msg::Twist2D & twist)
+  {
+    nav_2d_msgs::msg::Twist2D twist_thresh;
+    twist_thresh.x = getThresholdedVelocity(twist.x, min_x_velocity_threshold_);
+    twist_thresh.y = getThresholdedVelocity(twist.y, min_y_velocity_threshold_);
+    twist_thresh.theta = getThresholdedVelocity(twist.theta, min_theta_velocity_threshold_);
+    return twist_thresh;
+  }
+
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  std::mutex dynamic_params_lock_;
+
+  // The controller needs a costmap node
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
+
+  // Publishers and subscribers
+  std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;
+  rclcpp::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
+
+  // Progress Checker Plugin
+  pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
+  nav2_core::ProgressChecker::Ptr progress_checker_;
+  std::string default_progress_checker_id_;
+  std::string default_progress_checker_type_;
+  std::string progress_checker_id_;
+  std::string progress_checker_type_;
+
+  // Goal Checker Plugin
+  pluginlib::ClassLoader<nav2_core::GoalChecker> goal_checker_loader_;
+  GoalCheckerMap goal_checkers_;
+  std::vector<std::string> default_goal_checker_ids_;
+  std::vector<std::string> default_goal_checker_types_;
+  std::vector<std::string> goal_checker_ids_;
+  std::vector<std::string> goal_checker_types_;
+  std::string goal_checker_ids_concat_, current_goal_checker_;
+
+  // Controller Plugins
+  pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
+  ControllerMap controllers_;
+  std::vector<std::string> default_ids_;
+  std::vector<std::string> default_types_;
+  std::vector<std::string> controller_ids_;
+  std::vector<std::string> controller_types_;
+  std::string controller_ids_concat_, current_controller_;
+
+  double controller_frequency_;
+  double min_x_velocity_threshold_;
+  double min_y_velocity_threshold_;
+  double min_theta_velocity_threshold_;
+
+  double failure_tolerance_;
+
+  // Whether we've published the single controller warning yet
+  geometry_msgs::msg::PoseStamped end_pose_;
+
+  // Last time the controller generated a valid command
+  rclcpp::Time last_valid_cmd_time_;
+
+  // Current path container
+  nav_msgs::msg::Path current_path_;
+
+private:
+  /**
+    * @brief Callback for speed limiting messages
+    * @param msg Shared pointer to nav2_msgs::msg::SpeedLimit
+    */
+  void speedLimitCallback(const nav2_msgs::msg::SpeedLimit::SharedPtr msg);
+};
+
+}  // namespace nav2_controller
+
+#endif  // NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_
@@ -0,0 +1,67 @@
+// Copyright (c) 2023 Dexory
+//
+// 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_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_
+
+#include <string>
+#include <vector>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_controller/plugins/simple_progress_checker.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+namespace nav2_controller
+{
+/**
+* @class PoseProgressChecker
+* @brief This plugin is used to check the position and the angle of the robot to make sure
+* that it is actually progressing or rotating towards a goal.
+*/
+
+class PoseProgressChecker : public SimpleProgressChecker
+{
+public:
+  void initialize(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    const std::string & plugin_name) override;
+  bool check(geometry_msgs::msg::PoseStamped & current_pose) override;
+
+protected:
+  /**
+   * @brief Calculates robots movement from baseline pose
+   * @param pose Current pose of the robot
+   * @return true, if movement is greater than radius_, or false
+   */
+  bool isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose);
+
+  static double poseAngleDistance(
+    const geometry_msgs::msg::Pose2D &,
+    const geometry_msgs::msg::Pose2D &);
+
+  double required_movement_angle_;
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  std::string plugin_name_;
+
+  /**
+   * @brief Callback executed when a paramter change is detected
+   * @param parameters list of changed parameters
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+};
+}  // namespace nav2_controller
+
+#endif  // NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_
@@ -0,0 +1,78 @@
+// Copyright (c) 2025 Prabhav Saxena
+//
+// 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_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_
+
+#include <string>
+#include <memory>
+#include <vector>
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_core/goal_checker.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+
+namespace nav2_controller
+{
+
+/**
+ * @class PositionGoalChecker
+ * @brief Goal Checker plugin that only checks XY position, ignoring orientation
+ */
+class PositionGoalChecker : public nav2_core::GoalChecker
+{
+public:
+  PositionGoalChecker();
+  ~PositionGoalChecker() override = default;
+
+  void initialize(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    const std::string & plugin_name,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
+
+  void reset() override;
+
+  bool isGoalReached(
+    const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+    const geometry_msgs::msg::Twist & velocity) override;
+
+  bool getTolerances(
+    geometry_msgs::msg::Pose & pose_tolerance,
+    geometry_msgs::msg::Twist & vel_tolerance) override;
+
+  /**
+   * @brief Set the XY goal tolerance
+   * @param tolerance New tolerance value
+   */
+  void setXYGoalTolerance(double tolerance);
+
+protected:
+  double xy_goal_tolerance_;
+  double xy_goal_tolerance_sq_;
+  bool stateful_;
+  bool position_reached_;
+  std::string plugin_name_;
+  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+
+  /**
+  * @brief Callback executed when a parameter change is detected
+  * @param parameters list of changed parameters
+  */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+};
+
+}  // namespace nav2_controller
+
+#endif  // NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_
@@ -0,0 +1,93 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_core/goal_checker.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+namespace nav2_controller
+{
+
+/**
+ * @class SimpleGoalChecker
+ * @brief Goal Checker plugin that only checks the position difference
+ *
+ * This class can be stateful if the stateful parameter is set to true (which it is by default).
+ * This means that the goal checker will not check if the xy position matches again once it is found to be true.
+ */
+class SimpleGoalChecker : public nav2_core::GoalChecker
+{
+public:
+  SimpleGoalChecker();
+  // Standard GoalChecker Interface
+  void initialize(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    const std::string & plugin_name,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
+  void reset() override;
+  bool isGoalReached(
+    const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+    const geometry_msgs::msg::Twist & velocity) override;
+  bool getTolerances(
+    geometry_msgs::msg::Pose & pose_tolerance,
+    geometry_msgs::msg::Twist & vel_tolerance) override;
+
+protected:
+  double xy_goal_tolerance_, yaw_goal_tolerance_;
+  bool stateful_, check_xy_;
+  // Cached squared xy_goal_tolerance_
+  double xy_goal_tolerance_sq_;
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  std::string plugin_name_;
+
+  /**
+   * @brief Callback executed when a paramter change is detected
+   * @param parameters list of changed parameters
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+};
+
+}  // namespace nav2_controller
+
+#endif  // NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
@@ -0,0 +1,82 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
+
+#include <string>
+#include <vector>
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_core/progress_checker.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+
+namespace nav2_controller
+{
+/**
+* @class SimpleProgressChecker
+* @brief This plugin is used to check the position of the robot to make sure
+* that it is actually progressing towards a goal.
+*/
+
+class SimpleProgressChecker : public nav2_core::ProgressChecker
+{
+public:
+  void initialize(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    const std::string & plugin_name) override;
+  bool check(geometry_msgs::msg::PoseStamped & current_pose) override;
+  void reset() override;
+
+protected:
+  /**
+   * @brief Calculates robots movement from baseline pose
+   * @param pose Current pose of the robot
+   * @return true, if movement is greater than radius_, or false
+   */
+  bool isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose);
+  /**
+   * @brief Resets baseline pose with the current pose of the robot
+   * @param pose Current pose of the robot
+   */
+  void resetBaselinePose(const geometry_msgs::msg::Pose2D & pose);
+
+  static double pose_distance(
+    const geometry_msgs::msg::Pose2D &,
+    const geometry_msgs::msg::Pose2D &);
+
+  rclcpp::Clock::SharedPtr clock_;
+
+  double radius_;
+  rclcpp::Duration time_allowance_{0, 0};
+
+  geometry_msgs::msg::Pose2D baseline_pose_;
+  rclcpp::Time baseline_time_;
+
+  bool baseline_pose_set_{false};
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  std::string plugin_name_;
+
+  /**
+   * @brief Callback executed when a paramter change is detected
+   * @param parameters list of changed parameters
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+};
+}  // namespace nav2_controller
+
+#endif  // NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
@@ -0,0 +1,85 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_controller/plugins/simple_goal_checker.hpp"
+
+namespace nav2_controller
+{
+
+/**
+ * @class StoppedGoalChecker
+ * @brief Goal Checker plugin that checks the position difference and velocity
+ */
+class StoppedGoalChecker : public SimpleGoalChecker
+{
+public:
+  StoppedGoalChecker();
+  // Standard GoalChecker Interface
+  void initialize(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    const std::string & plugin_name,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
+  bool isGoalReached(
+    const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+    const geometry_msgs::msg::Twist & velocity) override;
+  bool getTolerances(
+    geometry_msgs::msg::Pose & pose_tolerance,
+    geometry_msgs::msg::Twist & vel_tolerance) override;
+
+protected:
+  double rot_stopped_velocity_, trans_stopped_velocity_;
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  std::string plugin_name_;
+
+  /**
+   * @brief Callback executed when a paramter change is detected
+   * @param parameters list of changed parameters
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+};
+
+}  // namespace nav2_controller
+
+#endif  // NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
@@ -0,0 +1,32 @@
+<?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_controller</name>
+  <version>1.1.18</version>
+  <description>Controller action interface</description>
+  <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
+  <depend>angles</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>std_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav_2d_utils</depend>
+  <depend>nav_2d_msgs</depend>
+  <depend>nav2_core</depend>
+  <depend>pluginlib</depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <nav2_core plugin="${prefix}/plugins.xml" />
+  </export>
+</package>
@@ -0,0 +1,27 @@
+<class_libraries>
+  <library path="simple_progress_checker">
+    <class type="nav2_controller::SimpleProgressChecker" base_class_type="nav2_core::ProgressChecker">
+      <description>Checks if distance between current and previous pose is above a threshold</description>
+    </class>
+  </library>
+  <library path="pose_progress_checker">
+    <class type="nav2_controller::PoseProgressChecker" base_class_type="nav2_core::ProgressChecker">
+      <description>Checks if distance and angle between current and previous pose is above a threshold</description>
+    </class>
+  </library>
+  <library path="simple_goal_checker">
+    <class type="nav2_controller::SimpleGoalChecker" base_class_type="nav2_core::GoalChecker">
+      <description>Checks if current pose is within goal window for x,y and yaw</description>
+    </class>
+  </library>
+  <library path="stopped_goal_checker">
+    <class type="nav2_controller::StoppedGoalChecker" base_class_type="nav2_core::GoalChecker">
+        <description>Checks linear and angular velocity after stopping</description>
+    </class>
+  </library>
+  <library path="position_goal_checker">
+    <class type="nav2_controller::PositionGoalChecker" base_class_type="nav2_core::GoalChecker">
+        <description>Goal checker that only checks XY position and ignores orientation</description>
+    </class>
+  </library>
+</class_libraries>
@@ -0,0 +1,97 @@
+// Copyright (c) 2023 Dexory
+//
+// 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_controller/plugins/pose_progress_checker.hpp"
+#include <cmath>
+#include <string>
+#include <memory>
+#include <vector>
+#include "angles/angles.h"
+#include "nav_2d_utils/conversions.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+namespace nav2_controller
+{
+
+void PoseProgressChecker::initialize(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  const std::string & plugin_name)
+{
+  plugin_name_ = plugin_name;
+  SimpleProgressChecker::initialize(parent, plugin_name);
+  auto node = parent.lock();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".required_movement_angle", rclcpp::ParameterValue(0.5));
+  node->get_parameter_or(plugin_name + ".required_movement_angle", required_movement_angle_, 0.5);
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&PoseProgressChecker::dynamicParametersCallback, this, _1));
+}
+
+bool PoseProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose)
+{
+  // relies on short circuit evaluation to not call is_robot_moved_enough if
+  // baseline_pose is not set.
+  geometry_msgs::msg::Pose2D current_pose2d;
+  current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose);
+
+  if (!baseline_pose_set_ || PoseProgressChecker::isRobotMovedEnough(current_pose2d)) {
+    resetBaselinePose(current_pose2d);
+    return true;
+  }
+  return clock_->now() - baseline_time_ <= time_allowance_;
+}
+
+bool PoseProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose)
+{
+  return pose_distance(pose, baseline_pose_) > radius_ ||
+         poseAngleDistance(pose, baseline_pose_) > required_movement_angle_;
+}
+
+double PoseProgressChecker::poseAngleDistance(
+  const geometry_msgs::msg::Pose2D & pose1,
+  const geometry_msgs::msg::Pose2D & pose2)
+{
+  return abs(angles::shortest_angular_distance(pose1.theta, pose2.theta));
+}
+
+rcl_interfaces::msg::SetParametersResult
+PoseProgressChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == plugin_name_ + ".required_movement_angle") {
+        required_movement_angle_ = parameter.as_double();
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_controller
+
+PLUGINLIB_EXPORT_CLASS(nav2_controller::PoseProgressChecker, nav2_core::ProgressChecker)
@@ -0,0 +1,147 @@
+// Copyright (c) 2025 Prabhav Saxena
+//
+// 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 <limits>
+#include "nav2_controller/plugins/position_goal_checker.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "nav2_util/node_utils.hpp"
+
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+namespace nav2_controller
+{
+
+PositionGoalChecker::PositionGoalChecker()
+: xy_goal_tolerance_(0.25),
+  xy_goal_tolerance_sq_(0.0625),
+  stateful_(true),
+  position_reached_(false)
+{
+}
+
+void PositionGoalChecker::initialize(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  const std::string & plugin_name,
+  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS>/*costmap_ros*/)
+{
+  plugin_name_ = plugin_name;
+  auto node = parent.lock();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25));
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".stateful", rclcpp::ParameterValue(true));
+
+  node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_);
+  node->get_parameter(plugin_name + ".stateful", stateful_);
+
+  xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&PositionGoalChecker::dynamicParametersCallback, this, _1));
+}
+
+void PositionGoalChecker::reset()
+{
+  position_reached_ = false;
+}
+
+bool PositionGoalChecker::isGoalReached(
+  const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+  const geometry_msgs::msg::Twist &)
+{
+  // If stateful and position was already reached, maintain state
+  if (stateful_ && position_reached_) {
+    return true;
+  }
+
+  // Check if position is within tolerance
+  double dx = query_pose.position.x - goal_pose.position.x;
+  double dy = query_pose.position.y - goal_pose.position.y;
+
+  bool position_reached = (dx * dx + dy * dy <= xy_goal_tolerance_sq_);
+
+  // If stateful, remember that we reached the position
+  if (stateful_ && position_reached) {
+    position_reached_ = true;
+  }
+
+  return position_reached;
+}
+
+bool PositionGoalChecker::getTolerances(
+  geometry_msgs::msg::Pose & pose_tolerance,
+  geometry_msgs::msg::Twist & vel_tolerance)
+{
+  double invalid_field = std::numeric_limits<double>::lowest();
+
+  pose_tolerance.position.x = xy_goal_tolerance_;
+  pose_tolerance.position.y = xy_goal_tolerance_;
+  pose_tolerance.position.z = invalid_field;
+
+  // Return zero orientation tolerance as we don't check it
+  pose_tolerance.orientation.x = 0.0;
+  pose_tolerance.orientation.y = 0.0;
+  pose_tolerance.orientation.z = 0.0;
+  pose_tolerance.orientation.w = 1.0;
+
+  vel_tolerance.linear.x = invalid_field;
+  vel_tolerance.linear.y = invalid_field;
+  vel_tolerance.linear.z = invalid_field;
+
+  vel_tolerance.angular.x = invalid_field;
+  vel_tolerance.angular.y = invalid_field;
+  vel_tolerance.angular.z = invalid_field;
+
+  return true;
+}
+
+void nav2_controller::PositionGoalChecker::setXYGoalTolerance(double tolerance)
+{
+  xy_goal_tolerance_ = tolerance;
+  xy_goal_tolerance_sq_ = tolerance * tolerance;
+}
+
+rcl_interfaces::msg::SetParametersResult
+PositionGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  for (auto & parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == plugin_name_ + ".xy_goal_tolerance") {
+        xy_goal_tolerance_ = parameter.as_double();
+        xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == plugin_name_ + ".stateful") {
+        stateful_ = parameter.as_bool();
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_controller
+
+PLUGINLIB_EXPORT_CLASS(nav2_controller::PositionGoalChecker, nav2_core::GoalChecker)
@@ -0,0 +1,170 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <memory>
+#include <string>
+#include <limits>
+#include <vector>
+#include "nav2_controller/plugins/simple_goal_checker.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "angles/angles.h"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#include "tf2/utils.h"
+#pragma GCC diagnostic pop
+
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+namespace nav2_controller
+{
+
+SimpleGoalChecker::SimpleGoalChecker()
+: xy_goal_tolerance_(0.25),
+  yaw_goal_tolerance_(0.25),
+  stateful_(true),
+  check_xy_(true),
+  xy_goal_tolerance_sq_(0.0625)
+{
+}
+
+void SimpleGoalChecker::initialize(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  const std::string & plugin_name,
+  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS>/*costmap_ros*/)
+{
+  plugin_name_ = plugin_name;
+  auto node = parent.lock();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25));
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".yaw_goal_tolerance", rclcpp::ParameterValue(0.25));
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".stateful", rclcpp::ParameterValue(true));
+
+  node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_);
+  node->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_);
+  node->get_parameter(plugin_name + ".stateful", stateful_);
+
+  xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&SimpleGoalChecker::dynamicParametersCallback, this, _1));
+}
+
+void SimpleGoalChecker::reset()
+{
+  check_xy_ = true;
+}
+
+bool SimpleGoalChecker::isGoalReached(
+  const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+  const geometry_msgs::msg::Twist &)
+{
+  if (check_xy_) {
+    double dx = query_pose.position.x - goal_pose.position.x,
+      dy = query_pose.position.y - goal_pose.position.y;
+    if (dx * dx + dy * dy > xy_goal_tolerance_sq_) {
+      return false;
+    }
+    // We are within the window
+    // If we are stateful, change the state.
+    if (stateful_) {
+      check_xy_ = false;
+    }
+  }
+  double dyaw = angles::shortest_angular_distance(
+    tf2::getYaw(query_pose.orientation),
+    tf2::getYaw(goal_pose.orientation));
+  return fabs(dyaw) < yaw_goal_tolerance_;
+}
+
+bool SimpleGoalChecker::getTolerances(
+  geometry_msgs::msg::Pose & pose_tolerance,
+  geometry_msgs::msg::Twist & vel_tolerance)
+{
+  double invalid_field = std::numeric_limits<double>::lowest();
+
+  pose_tolerance.position.x = xy_goal_tolerance_;
+  pose_tolerance.position.y = xy_goal_tolerance_;
+  pose_tolerance.position.z = invalid_field;
+  pose_tolerance.orientation =
+    nav2_util::geometry_utils::orientationAroundZAxis(yaw_goal_tolerance_);
+
+  vel_tolerance.linear.x = invalid_field;
+  vel_tolerance.linear.y = invalid_field;
+  vel_tolerance.linear.z = invalid_field;
+
+  vel_tolerance.angular.x = invalid_field;
+  vel_tolerance.angular.y = invalid_field;
+  vel_tolerance.angular.z = invalid_field;
+
+  return true;
+}
+
+rcl_interfaces::msg::SetParametersResult
+SimpleGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  for (auto & parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == plugin_name_ + ".xy_goal_tolerance") {
+        xy_goal_tolerance_ = parameter.as_double();
+        xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
+      } else if (name == plugin_name_ + ".yaw_goal_tolerance") {
+        yaw_goal_tolerance_ = parameter.as_double();
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == plugin_name_ + ".stateful") {
+        stateful_ = parameter.as_bool();
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_controller
+
+PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleGoalChecker, nav2_core::GoalChecker)
@@ -0,0 +1,119 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_controller/plugins/simple_progress_checker.hpp"
+#include <cmath>
+#include <string>
+#include <memory>
+#include <vector>
+#include "nav2_core/exceptions.hpp"
+#include "nav_2d_utils/conversions.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+namespace nav2_controller
+{
+void SimpleProgressChecker::initialize(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  const std::string & plugin_name)
+{
+  plugin_name_ = plugin_name;
+  auto node = parent.lock();
+
+  clock_ = node->get_clock();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0));
+  // Scale is set to 0 by default, so if it was not set otherwise, set to 0
+  node->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5);
+  double time_allowance_param = 0.0;
+  node->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0);
+  time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param);
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&SimpleProgressChecker::dynamicParametersCallback, this, _1));
+}
+
+bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose)
+{
+  // relies on short circuit evaluation to not call is_robot_moved_enough if
+  // baseline_pose is not set.
+  geometry_msgs::msg::Pose2D current_pose2d;
+  current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose);
+
+  if ((!baseline_pose_set_) || (isRobotMovedEnough(current_pose2d))) {
+    resetBaselinePose(current_pose2d);
+    return true;
+  }
+  return !((clock_->now() - baseline_time_) > time_allowance_);
+}
+
+void SimpleProgressChecker::reset()
+{
+  baseline_pose_set_ = false;
+}
+
+void SimpleProgressChecker::resetBaselinePose(const geometry_msgs::msg::Pose2D & pose)
+{
+  baseline_pose_ = pose;
+  baseline_time_ = clock_->now();
+  baseline_pose_set_ = true;
+}
+
+bool SimpleProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose)
+{
+  return pose_distance(pose, baseline_pose_) > radius_;
+}
+
+double SimpleProgressChecker::pose_distance(
+  const geometry_msgs::msg::Pose2D & pose1,
+  const geometry_msgs::msg::Pose2D & pose2)
+{
+  double dx = pose1.x - pose2.x;
+  double dy = pose1.y - pose2.y;
+
+  return std::hypot(dx, dy);
+}
+
+rcl_interfaces::msg::SetParametersResult
+SimpleProgressChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == plugin_name_ + ".required_movement_radius") {
+        radius_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".movement_time_allowance") {
+        time_allowance_ = rclcpp::Duration::from_seconds(parameter.as_double());
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_controller
+
+PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleProgressChecker, nav2_core::ProgressChecker)
@@ -0,0 +1,139 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <cmath>
+#include <string>
+#include <memory>
+#include <limits>
+#include <vector>
+#include "nav2_controller/plugins/stopped_goal_checker.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "nav2_util/node_utils.hpp"
+
+using std::hypot;
+using std::fabs;
+
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+namespace nav2_controller
+{
+
+StoppedGoalChecker::StoppedGoalChecker()
+: SimpleGoalChecker(), rot_stopped_velocity_(0.25), trans_stopped_velocity_(0.25)
+{
+}
+
+void StoppedGoalChecker::initialize(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  const std::string & plugin_name,
+  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  plugin_name_ = plugin_name;
+  SimpleGoalChecker::initialize(parent, plugin_name, costmap_ros);
+
+  auto node = parent.lock();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".rot_stopped_velocity", rclcpp::ParameterValue(0.25));
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".trans_stopped_velocity", rclcpp::ParameterValue(0.25));
+
+  node->get_parameter(plugin_name + ".rot_stopped_velocity", rot_stopped_velocity_);
+  node->get_parameter(plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_);
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&StoppedGoalChecker::dynamicParametersCallback, this, _1));
+}
+
+bool StoppedGoalChecker::isGoalReached(
+  const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+  const geometry_msgs::msg::Twist & velocity)
+{
+  bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity);
+  if (!ret) {
+    return ret;
+  }
+
+  return fabs(velocity.angular.z) <= rot_stopped_velocity_ &&
+         hypot(velocity.linear.x, velocity.linear.y) <= trans_stopped_velocity_;
+}
+
+bool StoppedGoalChecker::getTolerances(
+  geometry_msgs::msg::Pose & pose_tolerance,
+  geometry_msgs::msg::Twist & vel_tolerance)
+{
+  double invalid_field = std::numeric_limits<double>::lowest();
+
+  // populate the poses
+  bool rtn = SimpleGoalChecker::getTolerances(pose_tolerance, vel_tolerance);
+
+  // override the velocities
+  vel_tolerance.linear.x = trans_stopped_velocity_;
+  vel_tolerance.linear.y = trans_stopped_velocity_;
+  vel_tolerance.linear.z = invalid_field;
+
+  vel_tolerance.angular.x = invalid_field;
+  vel_tolerance.angular.y = invalid_field;
+  vel_tolerance.angular.z = rot_stopped_velocity_;
+
+  return true && rtn;
+}
+
+rcl_interfaces::msg::SetParametersResult
+StoppedGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == plugin_name_ + ".rot_stopped_velocity") {
+        rot_stopped_velocity_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".trans_stopped_velocity") {
+        trans_stopped_velocity_ = parameter.as_double();
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_controller
+
+PLUGINLIB_EXPORT_CLASS(nav2_controller::StoppedGoalChecker, nav2_core::GoalChecker)
@@ -0,0 +1,4 @@
+ament_add_gtest(pctest progress_checker.cpp)
+target_link_libraries(pctest simple_progress_checker pose_progress_checker)
+ament_add_gtest(gctest goal_checker.cpp)
+target_link_libraries(gctest simple_goal_checker stopped_goal_checker)
@@ -0,0 +1,245 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <memory>
+#include <string>
+
+#include "gtest/gtest.h"
+#include "nav2_controller/plugins/simple_goal_checker.hpp"
+#include "nav2_controller/plugins/stopped_goal_checker.hpp"
+#include "nav_2d_utils/conversions.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+
+using nav2_controller::SimpleGoalChecker;
+using nav2_controller::StoppedGoalChecker;
+
+void checkMacro(
+  nav2_core::GoalChecker & gc,
+  double x0, double y0, double theta0,
+  double x1, double y1, double theta1,
+  double xv, double yv, double thetav,
+  bool expected_result)
+{
+  gc.reset();
+  geometry_msgs::msg::Pose2D pose0, pose1;
+  pose0.x = x0;
+  pose0.y = y0;
+  pose0.theta = theta0;
+  pose1.x = x1;
+  pose1.y = y1;
+  pose1.theta = theta1;
+  nav_2d_msgs::msg::Twist2D v;
+  v.x = xv;
+  v.y = yv;
+  v.theta = thetav;
+  if (expected_result) {
+    EXPECT_TRUE(
+      gc.isGoalReached(
+        nav_2d_utils::pose2DToPose(pose0),
+        nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v)));
+  } else {
+    EXPECT_FALSE(
+      gc.isGoalReached(
+        nav_2d_utils::pose2DToPose(pose0),
+        nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v)));
+  }
+}
+
+void sameResult(
+  nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1,
+  double x0, double y0, double theta0,
+  double x1, double y1, double theta1,
+  double xv, double yv, double thetav,
+  bool expected_result)
+{
+  checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
+  checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
+}
+
+void trueFalse(
+  nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1,
+  double x0, double y0, double theta0,
+  double x1, double y1, double theta1,
+  double xv, double yv, double thetav)
+{
+  checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true);
+  checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false);
+}
+class TestLifecycleNode : public nav2_util::LifecycleNode
+{
+public:
+  explicit TestLifecycleNode(const std::string & name)
+  : nav2_util::LifecycleNode(name)
+  {
+  }
+
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+};
+
+TEST(VelocityIterator, goal_checker_reset)
+{
+  auto x = std::make_shared<TestLifecycleNode>("goal_checker");
+
+  nav2_core::GoalChecker * gc = new SimpleGoalChecker;
+  gc->reset();
+  delete gc;
+  EXPECT_TRUE(true);
+}
+
+TEST(VelocityIterator, stopped_goal_checker_reset)
+{
+  auto x = std::make_shared<TestLifecycleNode>("stopped_goal_checker");
+
+  nav2_core::GoalChecker * sgc = new StoppedGoalChecker;
+  sgc->reset();
+  delete sgc;
+  EXPECT_TRUE(true);
+}
+
+TEST(VelocityIterator, two_checks)
+{
+  auto x = std::make_shared<TestLifecycleNode>("goal_checker");
+
+  SimpleGoalChecker gc;
+  StoppedGoalChecker sgc;
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+
+  gc.initialize(x, "nav2_controller", costmap);
+  sgc.initialize(x, "nav2_controller", costmap);
+  sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true);
+  sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false);
+  sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false);
+  sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, false);
+  sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, true);
+  trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0);
+  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0);
+  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0);
+  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1);
+}
+
+TEST(StoppedGoalChecker, get_tol_and_dynamic_params)
+{
+  auto x = std::make_shared<TestLifecycleNode>("goal_checker");
+
+  SimpleGoalChecker gc;
+  StoppedGoalChecker sgc;
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+
+  sgc.initialize(x, "test", costmap);
+  gc.initialize(x, "test2", costmap);
+  geometry_msgs::msg::Pose pose_tol;
+  geometry_msgs::msg::Twist vel_tol;
+
+  // Test stopped goal checker's tolerance API
+  EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol));
+  EXPECT_EQ(vel_tol.linear.x, 0.25);
+  EXPECT_EQ(vel_tol.linear.y, 0.25);
+  EXPECT_EQ(vel_tol.angular.z, 0.25);
+
+  // Test Stopped goal checker's dynamic parameters
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    x->get_node_base_interface(), x->get_node_topics_interface(),
+    x->get_node_graph_interface(),
+    x->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("test.rot_stopped_velocity", 100.0),
+      rclcpp::Parameter("test.trans_stopped_velocity", 100.0)});
+
+  rclcpp::spin_until_future_complete(
+    x->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(x->get_parameter("test.rot_stopped_velocity").as_double(), 100.0);
+  EXPECT_EQ(x->get_parameter("test.trans_stopped_velocity").as_double(), 100.0);
+
+  // Test normal goal checker's dynamic parameters
+  results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("test2.xy_goal_tolerance", 200.0),
+      rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0),
+      rclcpp::Parameter("test2.stateful", true)});
+
+  rclcpp::spin_until_future_complete(
+    x->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(x->get_parameter("test2.xy_goal_tolerance").as_double(), 200.0);
+  EXPECT_EQ(x->get_parameter("test2.yaw_goal_tolerance").as_double(), 200.0);
+  EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true);
+
+  // Test the dynamic parameters impacted the tolerances
+  EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol));
+  EXPECT_EQ(vel_tol.linear.x, 100.0);
+  EXPECT_EQ(vel_tol.linear.y, 100.0);
+  EXPECT_EQ(vel_tol.angular.z, 100.0);
+
+  EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol));
+  EXPECT_EQ(pose_tol.position.x, 200.0);
+  EXPECT_EQ(pose_tol.position.y, 200.0);
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
@@ -0,0 +1,244 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <memory>
+#include <string>
+
+#include "gtest/gtest.h"
+#include "nav2_controller/plugins/simple_progress_checker.hpp"
+#include "nav2_controller/plugins/pose_progress_checker.hpp"
+#include "nav_2d_utils/conversions.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+using nav2_controller::SimpleProgressChecker;
+using nav2_controller::PoseProgressChecker;
+
+class TestLifecycleNode : public nav2_util::LifecycleNode
+{
+public:
+  explicit TestLifecycleNode(const std::string & name)
+  : nav2_util::LifecycleNode(name)
+  {
+  }
+
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+};
+
+void checkMacro(
+  nav2_core::ProgressChecker & pc,
+  double x0, double y0, double theta0,
+  double x1, double y1, double theta1,
+  int delay,
+  bool expected_result)
+{
+  pc.reset();
+  geometry_msgs::msg::PoseStamped pose0, pose1;
+  pose0.pose.position.x = x0;
+  pose0.pose.position.y = y0;
+  pose0.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta0);
+  pose1.pose.position.x = x1;
+  pose1.pose.position.y = y1;
+  pose1.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1);
+  EXPECT_TRUE(pc.check(pose0));
+  rclcpp::sleep_for(std::chrono::milliseconds(delay));
+  if (expected_result) {
+    EXPECT_TRUE(pc.check(pose1));
+  } else {
+    EXPECT_FALSE(pc.check(pose1));
+  }
+}
+
+TEST(SimpleProgressChecker, progress_checker_reset)
+{
+  auto x = std::make_shared<TestLifecycleNode>("progress_checker");
+
+  nav2_core::ProgressChecker * pc = new SimpleProgressChecker;
+  pc->reset();
+  delete pc;
+  EXPECT_TRUE(true);
+}
+
+TEST(SimpleProgressChecker, unit_tests)
+{
+  auto x = std::make_shared<TestLifecycleNode>("progress_checker");
+
+  SimpleProgressChecker pc;
+  pc.initialize(x, "nav2_controller");
+
+  double time_allowance = 0.5;
+  int half_time_allowance_ms = static_cast<int>(time_allowance * 0.5 * 1000);
+  int twice_time_allowance_ms = static_cast<int>(time_allowance * 2.0 * 1000);
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    x->get_node_base_interface(), x->get_node_topics_interface(),
+    x->get_node_graph_interface(),
+    x->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)});
+
+  rclcpp::spin_until_future_complete(
+    x->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(
+    x->get_parameter("nav2_controller.movement_time_allowance").as_double(),
+    time_allowance);
+
+  // BELOW time allowance (set to time_allowance)
+  // no movement
+  checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true);
+  // translation below required_movement_radius (default 0.5)
+  checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true);
+  checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true);
+  // translation above required_movement_radius (default 0.5)
+  checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true);
+  checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true);
+
+  // ABOVE time allowance (set to time_allowance)
+  // no movement
+  checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false);
+  // translation below required_movement_radius (default 0.5)
+  checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false);
+  checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false);
+  // translation above required_movement_radius (default 0.5)
+  checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true);
+  checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true);
+}
+
+TEST(PoseProgressChecker, pose_progress_checker_reset)
+{
+  auto x = std::make_shared<TestLifecycleNode>("pose_progress_checker");
+
+  PoseProgressChecker * rpc = new PoseProgressChecker;
+  rpc->reset();
+  delete rpc;
+  EXPECT_TRUE(true);
+}
+
+TEST(PoseProgressChecker, unit_tests)
+{
+  auto x = std::make_shared<TestLifecycleNode>("pose_progress_checker");
+
+  PoseProgressChecker rpc;
+  rpc.initialize(x, "nav2_controller");
+
+  double time_allowance = 0.5;
+  int half_time_allowance_ms = static_cast<int>(time_allowance * 0.5 * 1000);
+  int twice_time_allowance_ms = static_cast<int>(time_allowance * 2.0 * 1000);
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    x->get_node_base_interface(), x->get_node_topics_interface(),
+    x->get_node_graph_interface(),
+    x->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)});
+
+  rclcpp::spin_until_future_complete(
+    x->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(
+    x->get_parameter("nav2_controller.movement_time_allowance").as_double(),
+    time_allowance);
+
+  // BELOW time allowance (set to time_allowance)
+  // no movement
+  checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true);
+  // translation below required_movement_radius (default 0.5)
+  checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true);
+  checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true);
+  // rotation below required_movement_angle (default 0.5)
+  checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms, true);
+  checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms, true);
+  // translation above required_movement_radius (default 0.5)
+  checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true);
+  checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true);
+  // rotation above required_movement_angle (default 0.5)
+  checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms, true);
+  checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms, true);
+
+  // ABOVE time allowance (set to time_allowance)
+  // no movement
+  checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false);
+  // translation below required_movement_radius (default 0.5)
+  checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false);
+  checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false);
+  // rotation below required_movement_angle (default 0.5)
+  checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms, false);
+  checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms, false);
+  // translation above required_movement_radius (default 0.5)
+  checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true);
+  checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true);
+  // rotation above required_movement_angle (default 0.5)
+  checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms, true);
+  checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms, true);
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
@@ -0,0 +1,677 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <chrono>
+#include <vector>
+#include <memory>
+#include <string>
+#include <utility>
+#include <limits>
+
+#include "lifecycle_msgs/msg/state.hpp"
+#include "nav2_core/exceptions.hpp"
+#include "nav_2d_utils/conversions.hpp"
+#include "nav_2d_utils/tf_help.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_controller/controller_server.hpp"
+
+using namespace std::chrono_literals;
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+namespace nav2_controller
+{
+
+ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
+: nav2_util::LifecycleNode("controller_server", "", options),
+  progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"),
+  default_progress_checker_id_{"progress_checker"},
+  default_progress_checker_type_{"nav2_controller::SimpleProgressChecker"},
+  goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"),
+  default_goal_checker_ids_{"goal_checker"},
+  default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"},
+  lp_loader_("nav2_core", "nav2_core::Controller"),
+  default_ids_{"FollowPath"},
+  default_types_{"dwb_core::DWBLocalPlanner"}
+{
+  RCLCPP_INFO(get_logger(), "Creating controller server");
+
+  declare_parameter("controller_frequency", 20.0);
+
+  declare_parameter("progress_checker_plugin", default_progress_checker_id_);
+  declare_parameter("goal_checker_plugins", default_goal_checker_ids_);
+  declare_parameter("controller_plugins", default_ids_);
+  declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001));
+  declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001));
+  declare_parameter("min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001));
+
+  declare_parameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit"));
+
+  declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0));
+
+  // The costmap node is used in the implementation of the controller
+  costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "local_costmap", std::string{get_namespace()}, "local_costmap");
+}
+
+ControllerServer::~ControllerServer()
+{
+  progress_checker_.reset();
+  goal_checkers_.clear();
+  controllers_.clear();
+  costmap_thread_.reset();
+}
+
+nav2_util::CallbackReturn
+ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  auto node = shared_from_this();
+
+  RCLCPP_INFO(get_logger(), "Configuring controller interface");
+
+  get_parameter("progress_checker_plugin", progress_checker_id_);
+  if (progress_checker_id_ == default_progress_checker_id_) {
+    nav2_util::declare_parameter_if_not_declared(
+      node, default_progress_checker_id_ + ".plugin",
+      rclcpp::ParameterValue(default_progress_checker_type_));
+  }
+
+  RCLCPP_INFO(get_logger(), "getting goal checker plugins..");
+  get_parameter("goal_checker_plugins", goal_checker_ids_);
+  if (goal_checker_ids_ == default_goal_checker_ids_) {
+    for (size_t i = 0; i < default_goal_checker_ids_.size(); ++i) {
+      nav2_util::declare_parameter_if_not_declared(
+        node, default_goal_checker_ids_[i] + ".plugin",
+        rclcpp::ParameterValue(default_goal_checker_types_[i]));
+    }
+  }
+
+  get_parameter("controller_plugins", controller_ids_);
+  if (controller_ids_ == default_ids_) {
+    for (size_t i = 0; i < default_ids_.size(); ++i) {
+      nav2_util::declare_parameter_if_not_declared(
+        node, default_ids_[i] + ".plugin",
+        rclcpp::ParameterValue(default_types_[i]));
+    }
+  }
+
+  controller_types_.resize(controller_ids_.size());
+  goal_checker_types_.resize(goal_checker_ids_.size());
+
+  get_parameter("controller_frequency", controller_frequency_);
+  get_parameter("min_x_velocity_threshold", min_x_velocity_threshold_);
+  get_parameter("min_y_velocity_threshold", min_y_velocity_threshold_);
+  get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_);
+  RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
+
+  std::string speed_limit_topic;
+  get_parameter("speed_limit_topic", speed_limit_topic);
+  get_parameter("failure_tolerance", failure_tolerance_);
+
+  costmap_ros_->configure();
+  // Launch a thread to run the costmap node
+  costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
+
+  try {
+    progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_);
+    progress_checker_ = progress_checker_loader_.createUniqueInstance(progress_checker_type_);
+    RCLCPP_INFO(
+      get_logger(), "Created progress_checker : %s of type %s",
+      progress_checker_id_.c_str(), progress_checker_type_.c_str());
+    progress_checker_->initialize(node, progress_checker_id_);
+  } catch (const pluginlib::PluginlibException & ex) {
+    RCLCPP_FATAL(
+      get_logger(),
+      "Failed to create progress_checker. Exception: %s", ex.what());
+    return nav2_util::CallbackReturn::FAILURE;
+  }
+
+  for (size_t i = 0; i != goal_checker_ids_.size(); i++) {
+    try {
+      goal_checker_types_[i] = nav2_util::get_plugin_type_param(node, goal_checker_ids_[i]);
+      nav2_core::GoalChecker::Ptr goal_checker =
+        goal_checker_loader_.createUniqueInstance(goal_checker_types_[i]);
+      RCLCPP_INFO(
+        get_logger(), "Created goal checker : %s of type %s",
+        goal_checker_ids_[i].c_str(), goal_checker_types_[i].c_str());
+      goal_checker->initialize(node, goal_checker_ids_[i], costmap_ros_);
+      goal_checkers_.insert({goal_checker_ids_[i], goal_checker});
+    } catch (const pluginlib::PluginlibException & ex) {
+      RCLCPP_FATAL(
+        get_logger(),
+        "Failed to create goal checker. Exception: %s", ex.what());
+      return nav2_util::CallbackReturn::FAILURE;
+    }
+  }
+
+  for (size_t i = 0; i != goal_checker_ids_.size(); i++) {
+    goal_checker_ids_concat_ += goal_checker_ids_[i] + std::string(" ");
+  }
+
+  RCLCPP_INFO(
+    get_logger(),
+    "Controller Server has %s goal checkers available.", goal_checker_ids_concat_.c_str());
+
+  for (size_t i = 0; i != controller_ids_.size(); i++) {
+    try {
+      controller_types_[i] = nav2_util::get_plugin_type_param(node, controller_ids_[i]);
+      nav2_core::Controller::Ptr controller =
+        lp_loader_.createUniqueInstance(controller_types_[i]);
+      RCLCPP_INFO(
+        get_logger(), "Created controller : %s of type %s",
+        controller_ids_[i].c_str(), controller_types_[i].c_str());
+      controller->configure(
+        node, controller_ids_[i],
+        costmap_ros_->getTfBuffer(), costmap_ros_);
+      controllers_.insert({controller_ids_[i], controller});
+    } catch (const pluginlib::PluginlibException & ex) {
+      RCLCPP_FATAL(
+        get_logger(),
+        "Failed to create controller. Exception: %s", ex.what());
+      return nav2_util::CallbackReturn::FAILURE;
+    }
+  }
+
+  for (size_t i = 0; i != controller_ids_.size(); i++) {
+    controller_ids_concat_ += controller_ids_[i] + std::string(" ");
+  }
+
+  RCLCPP_INFO(
+    get_logger(),
+    "Controller Server has %s controllers available.", controller_ids_concat_.c_str());
+
+  odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node);
+  vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+
+  // Create the action server that we implement with our followPath method
+  action_server_ = std::make_unique<ActionServer>(
+    shared_from_this(),
+    "follow_path",
+    std::bind(&ControllerServer::computeControl, this),
+    nullptr,
+    std::chrono::milliseconds(500),
+    true);
+
+  // Set subscribtion to the speed limiting topic
+  speed_limit_sub_ = create_subscription<nav2_msgs::msg::SpeedLimit>(
+    speed_limit_topic, rclcpp::QoS(10),
+    std::bind(&ControllerServer::speedLimitCallback, this, std::placeholders::_1));
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+
+  costmap_ros_->activate();
+  ControllerMap::iterator it;
+  for (it = controllers_.begin(); it != controllers_.end(); ++it) {
+    it->second->activate();
+  }
+  vel_publisher_->on_activate();
+  action_server_->activate();
+
+  auto node = shared_from_this();
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&ControllerServer::dynamicParametersCallback, this, _1));
+
+  // create bond connection
+  createBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  action_server_->deactivate();
+  ControllerMap::iterator it;
+  for (it = controllers_.begin(); it != controllers_.end(); ++it) {
+    it->second->deactivate();
+  }
+
+  /*
+   * The costmap is also a lifecycle node, so it may have already fired on_deactivate
+   * via rcl preshutdown cb. Despite the rclcpp docs saying on_shutdown callbacks fire
+   * in the order added, the preshutdown callbacks clearly don't per se, due to using an
+   * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger
+   * ordering assumption: https://github.com/ros2/rclcpp/issues/2096
+   */
+  costmap_ros_->deactivate();
+
+  publishZeroVelocity();
+  vel_publisher_->on_deactivate();
+  dyn_params_handler_.reset();
+
+  // destroy bond connection
+  destroyBond();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  // Cleanup the helper classes
+  ControllerMap::iterator it;
+  for (it = controllers_.begin(); it != controllers_.end(); ++it) {
+    it->second->cleanup();
+  }
+  controllers_.clear();
+
+  goal_checkers_.clear();
+
+  costmap_ros_->cleanup();
+
+
+  // Release any allocated resources
+  action_server_.reset();
+  odom_sub_.reset();
+  costmap_thread_.reset();
+  vel_publisher_.reset();
+  speed_limit_sub_.reset();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+ControllerServer::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+bool ControllerServer::findControllerId(
+  const std::string & c_name,
+  std::string & current_controller)
+{
+  if (controllers_.find(c_name) == controllers_.end()) {
+    if (controllers_.size() == 1 && c_name.empty()) {
+      RCLCPP_WARN_ONCE(
+        get_logger(), "No controller was specified in action call."
+        " Server will use only plugin loaded %s. "
+        "This warning will appear once.", controller_ids_concat_.c_str());
+      current_controller = controllers_.begin()->first;
+    } else {
+      RCLCPP_ERROR(
+        get_logger(), "FollowPath called with controller name %s, "
+        "which does not exist. Available controllers are: %s.",
+        c_name.c_str(), controller_ids_concat_.c_str());
+      return false;
+    }
+  } else {
+    RCLCPP_DEBUG(get_logger(), "Selected controller: %s.", c_name.c_str());
+    current_controller = c_name;
+  }
+
+  return true;
+}
+
+bool ControllerServer::findGoalCheckerId(
+  const std::string & c_name,
+  std::string & current_goal_checker)
+{
+  if (goal_checkers_.find(c_name) == goal_checkers_.end()) {
+    if (goal_checkers_.size() == 1 && c_name.empty()) {
+      RCLCPP_WARN_ONCE(
+        get_logger(), "No goal checker was specified in parameter 'current_goal_checker'."
+        " Server will use only plugin loaded %s. "
+        "This warning will appear once.", goal_checker_ids_concat_.c_str());
+      current_goal_checker = goal_checkers_.begin()->first;
+    } else {
+      RCLCPP_ERROR(
+        get_logger(), "FollowPath called with goal_checker name %s in parameter"
+        " 'current_goal_checker', which does not exist. Available goal checkers are: %s.",
+        c_name.c_str(), goal_checker_ids_concat_.c_str());
+      return false;
+    }
+  } else {
+    RCLCPP_DEBUG(get_logger(), "Selected goal checker: %s.", c_name.c_str());
+    current_goal_checker = c_name;
+  }
+
+  return true;
+}
+
+void ControllerServer::computeControl()
+{
+  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
+
+  RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort.");
+
+  try {
+    std::string c_name = action_server_->get_current_goal()->controller_id;
+    std::string current_controller;
+    if (findControllerId(c_name, current_controller)) {
+      current_controller_ = current_controller;
+    } else {
+      action_server_->terminate_current();
+      return;
+    }
+
+    std::string gc_name = action_server_->get_current_goal()->goal_checker_id;
+    std::string current_goal_checker;
+    if (findGoalCheckerId(gc_name, current_goal_checker)) {
+      current_goal_checker_ = current_goal_checker;
+    } else {
+      action_server_->terminate_current();
+      return;
+    }
+
+    setPlannerPath(action_server_->get_current_goal()->path);
+    progress_checker_->reset();
+
+    last_valid_cmd_time_ = now();
+    rclcpp::WallRate loop_rate(controller_frequency_);
+    while (rclcpp::ok()) {
+      if (action_server_ == nullptr || !action_server_->is_server_active()) {
+        RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
+        return;
+      }
+
+      if (action_server_->is_cancel_requested()) {
+        RCLCPP_INFO(get_logger(), "Goal was canceled. Stopping the robot.");
+        action_server_->terminate_all();
+        publishZeroVelocity();
+        return;
+      }
+
+      // Don't compute a trajectory until costmap is valid (after clear costmap)
+      rclcpp::Rate r(100);
+      while (!costmap_ros_->isCurrent()) {
+        r.sleep();
+      }
+
+      updateGlobalPath();
+
+      computeAndPublishVelocity();
+
+      if (isGoalReached()) {
+        RCLCPP_INFO(get_logger(), "Reached the goal!");
+        break;
+      }
+
+      if (!loop_rate.sleep()) {
+        RCLCPP_WARN(
+          get_logger(), "Control loop missed its desired rate of %.4fHz",
+          controller_frequency_);
+      }
+    }
+  } catch (nav2_core::PlannerException & e) {
+    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
+    publishZeroVelocity();
+    action_server_->terminate_current();
+    return;
+  } catch (std::exception & e) {
+    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
+    publishZeroVelocity();
+    std::shared_ptr<Action::Result> result = std::make_shared<Action::Result>();
+    action_server_->terminate_current(result);
+    return;
+  }
+
+  RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result");
+
+  publishZeroVelocity();
+
+  // TODO(orduno) #861 Handle a pending preemption and set controller name
+  action_server_->succeeded_current();
+}
+
+void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
+{
+  RCLCPP_DEBUG(
+    get_logger(),
+    "Providing path to the controller %s", current_controller_.c_str());
+  if (path.poses.empty()) {
+    throw nav2_core::PlannerException("Invalid path, Path is empty.");
+  }
+  controllers_[current_controller_]->setPlan(path);
+
+  end_pose_ = path.poses.back();
+  end_pose_.header.frame_id = path.header.frame_id;
+  goal_checkers_[current_goal_checker_]->reset();
+
+  RCLCPP_DEBUG(
+    get_logger(), "Path end point is (%.2f, %.2f)",
+    end_pose_.pose.position.x, end_pose_.pose.position.y);
+
+  current_path_ = path;
+}
+
+void ControllerServer::computeAndPublishVelocity()
+{
+  geometry_msgs::msg::PoseStamped pose;
+
+  if (!getRobotPose(pose)) {
+    throw nav2_core::PlannerException("Failed to obtain robot pose");
+  }
+
+  if (!progress_checker_->check(pose)) {
+    throw nav2_core::PlannerException("Failed to make progress");
+  }
+
+  nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
+
+  geometry_msgs::msg::TwistStamped cmd_vel_2d;
+
+  try {
+    cmd_vel_2d =
+      controllers_[current_controller_]->computeVelocityCommands(
+      pose,
+      nav_2d_utils::twist2Dto3D(twist),
+      goal_checkers_[current_goal_checker_].get());
+    last_valid_cmd_time_ = now();
+  } catch (nav2_core::PlannerException & e) {
+    if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) {
+      RCLCPP_WARN(this->get_logger(), "%s", e.what());
+      cmd_vel_2d.twist.angular.x = 0;
+      cmd_vel_2d.twist.angular.y = 0;
+      cmd_vel_2d.twist.angular.z = 0;
+      cmd_vel_2d.twist.linear.x = 0;
+      cmd_vel_2d.twist.linear.y = 0;
+      cmd_vel_2d.twist.linear.z = 0;
+      cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID();
+      cmd_vel_2d.header.stamp = now();
+      if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ &&
+        failure_tolerance_ != -1.0)
+      {
+        throw nav2_core::PlannerException("Controller patience exceeded");
+      }
+    } else {
+      throw nav2_core::PlannerException(e.what());
+    }
+  }
+
+  std::shared_ptr<Action::Feedback> feedback = std::make_shared<Action::Feedback>();
+  feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y);
+
+  // Find the closest pose to current pose on global path
+  nav_msgs::msg::Path & current_path = current_path_;
+  auto find_closest_pose_idx =
+    [&pose, &current_path]() {
+      size_t closest_pose_idx = 0;
+      double curr_min_dist = std::numeric_limits<double>::max();
+      for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
+        double curr_dist = nav2_util::geometry_utils::euclidean_distance(
+          pose, current_path.poses[curr_idx]);
+        if (curr_dist < curr_min_dist) {
+          curr_min_dist = curr_dist;
+          closest_pose_idx = curr_idx;
+        }
+      }
+      return closest_pose_idx;
+    };
+
+  feedback->distance_to_goal =
+    nav2_util::geometry_utils::calculate_path_length(current_path_, find_closest_pose_idx());
+  action_server_->publish_feedback(feedback);
+
+  RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds());
+  publishVelocity(cmd_vel_2d);
+}
+
+void ControllerServer::updateGlobalPath()
+{
+  if (action_server_->is_preempt_requested()) {
+    RCLCPP_INFO(get_logger(), "Passing new path to controller.");
+    auto goal = action_server_->accept_pending_goal();
+    std::string current_controller;
+    if (findControllerId(goal->controller_id, current_controller)) {
+      current_controller_ = current_controller;
+    } else {
+      RCLCPP_INFO(
+        get_logger(), "Terminating action, invalid controller %s requested.",
+        goal->controller_id.c_str());
+      action_server_->terminate_current();
+      return;
+    }
+    std::string current_goal_checker;
+    if (findGoalCheckerId(goal->goal_checker_id, current_goal_checker)) {
+      current_goal_checker_ = current_goal_checker;
+    } else {
+      RCLCPP_INFO(
+        get_logger(), "Terminating action, invalid goal checker %s requested.",
+        goal->goal_checker_id.c_str());
+      action_server_->terminate_current();
+      return;
+    }
+    setPlannerPath(goal->path);
+  }
+}
+
+void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity)
+{
+  auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(velocity.twist);
+  if (vel_publisher_->is_activated() && vel_publisher_->get_subscription_count() > 0) {
+    vel_publisher_->publish(std::move(cmd_vel));
+  }
+}
+
+void ControllerServer::publishZeroVelocity()
+{
+  geometry_msgs::msg::TwistStamped velocity;
+  velocity.twist.angular.x = 0;
+  velocity.twist.angular.y = 0;
+  velocity.twist.angular.z = 0;
+  velocity.twist.linear.x = 0;
+  velocity.twist.linear.y = 0;
+  velocity.twist.linear.z = 0;
+  velocity.header.frame_id = costmap_ros_->getBaseFrameID();
+  velocity.header.stamp = now();
+  publishVelocity(velocity);
+}
+
+bool ControllerServer::isGoalReached()
+{
+  geometry_msgs::msg::PoseStamped pose;
+
+  if (!getRobotPose(pose)) {
+    return false;
+  }
+
+  nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
+  geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist);
+
+  geometry_msgs::msg::PoseStamped transformed_end_pose;
+  rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance()));
+  nav_2d_utils::transformPose(
+    costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(),
+    end_pose_, transformed_end_pose, tolerance);
+
+  return goal_checkers_[current_goal_checker_]->isGoalReached(
+    pose.pose, transformed_end_pose.pose,
+    velocity);
+}
+
+bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose)
+{
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!costmap_ros_->getRobotPose(current_pose)) {
+    return false;
+  }
+  pose = current_pose;
+  return true;
+}
+
+void ControllerServer::speedLimitCallback(const nav2_msgs::msg::SpeedLimit::SharedPtr msg)
+{
+  ControllerMap::iterator it;
+  for (it = controllers_.begin(); it != controllers_.end(); ++it) {
+    it->second->setSpeedLimit(msg->speed_limit, msg->percentage);
+  }
+}
+
+rcl_interfaces::msg::SetParametersResult
+ControllerServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    // If we are trying to change the parameter of a plugin we can just skip it at this point
+    // as they handle parameter changes themselves and don't need to lock the mutex
+    if (name.find('.') != std::string::npos) {
+      continue;
+    }
+
+    if (!dynamic_params_lock_.try_lock()) {
+      RCLCPP_WARN(
+        get_logger(),
+        "Unable to dynamically change Parameters while the controller is currently running");
+      result.successful = false;
+      result.reason =
+        "Unable to dynamically change Parameters while the controller is currently running";
+      return result;
+    }
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == "controller_frequency") {
+        controller_frequency_ = parameter.as_double();
+      } else if (name == "min_x_velocity_threshold") {
+        min_x_velocity_threshold_ = parameter.as_double();
+      } else if (name == "min_y_velocity_threshold") {
+        min_y_velocity_threshold_ = parameter.as_double();
+      } else if (name == "min_theta_velocity_threshold") {
+        min_theta_velocity_threshold_ = parameter.as_double();
+      } else if (name == "failure_tolerance") {
+        failure_tolerance_ = parameter.as_double();
+      }
+    }
+
+    dynamic_params_lock_.unlock();
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_controller
+
+#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(nav2_controller::ControllerServer)
@@ -0,0 +1,28 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "nav2_controller/controller_server.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<nav2_controller::ControllerServer>();
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+
+  return 0;
+}
@@ -0,0 +1,10 @@
+# Test dynamic parameters
+ament_add_gtest(test_dynamic_parameters
+  test_dynamic_parameters.cpp
+)
+ament_target_dependencies(test_dynamic_parameters
+  ${dependencies}
+)
+target_link_libraries(test_dynamic_parameters
+  ${library_name}
+)
@@ -0,0 +1,88 @@
+// 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. Reserved.
+
+#include <math.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_controller/controller_server.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class ControllerShim : public nav2_controller::ControllerServer
+{
+public:
+  ControllerShim()
+  : nav2_controller::ControllerServer(rclcpp::NodeOptions())
+  {
+  }
+
+  // Since we cannot call configure/activate due to costmaps
+  // requiring TF
+  void setDynamicCallback()
+  {
+    auto node = shared_from_this();
+    // Add callback for dynamic parameters
+    dyn_params_handler_ = node->add_on_set_parameters_callback(
+      std::bind(&ControllerShim::dynamicParamsShim, this, std::placeholders::_1));
+  }
+
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParamsShim(std::vector<rclcpp::Parameter> parameters)
+  {
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+    dynamicParametersCallback(parameters);
+    return result;
+  }
+};
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(WPTest, test_dynamic_parameters)
+{
+  auto controller = std::make_shared<ControllerShim>();
+  controller->setDynamicCallback();
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    controller->get_node_base_interface(), controller->get_node_topics_interface(),
+    controller->get_node_graph_interface(),
+    controller->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("controller_frequency", 100.0),
+      rclcpp::Parameter("min_x_velocity_threshold", 100.0),
+      rclcpp::Parameter("min_y_velocity_threshold", 100.0),
+      rclcpp::Parameter("min_theta_velocity_threshold", 100.0),
+      rclcpp::Parameter("failure_tolerance", 5.0)});
+
+  rclcpp::spin_until_future_complete(
+    controller->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(controller->get_parameter("controller_frequency").as_double(), 100.0);
+  EXPECT_EQ(controller->get_parameter("min_x_velocity_threshold").as_double(), 100.0);
+  EXPECT_EQ(controller->get_parameter("min_y_velocity_threshold").as_double(), 100.0);
+  EXPECT_EQ(controller->get_parameter("min_theta_velocity_threshold").as_double(), 100.0);
+  EXPECT_EQ(controller->get_parameter("failure_tolerance").as_double(), 5.0);
+}