add humble-navigation2

navigation2/nav2_rotation_shim_controller/CMakeLists.txt

@@ -0,0 +1,70 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_rotation_shim_controller)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(nav2_controller REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(angles REQUIRED)
+
+nav2_package()
+set(CMAKE_CXX_STANDARD 17)
+
+include_directories(
+  include
+)
+
+set(dependencies
+  rclcpp
+  geometry_msgs
+  nav2_costmap_2d
+  pluginlib
+  nav_msgs
+  nav2_controller
+  nav2_util
+  nav2_core
+  tf2
+  angles
+)
+
+set(library_name nav2_rotation_shim_controller)
+
+add_library(${library_name} SHARED
+        src/nav2_rotation_shim_controller.cpp)
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+install(TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+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()
+  add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+
+pluginlib_export_plugin_description_file(nav2_core nav2_rotation_shim_controller.xml)
+
+ament_package()

navigation2/nav2_rotation_shim_controller/README.md

@@ -0,0 +1,79 @@
+# Nav2 Rotation Shim Controller
+
+This is a controller (local trajectory planner) that implements a "shim" controller plugin. It was developed by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). 
+
+The Rotation Shim Controller stands between the controller server and the main controller plugin to implement a specific behavior often troublesome for other algorithms. This shim will rotate a robot in place to the rough heading of a newly received path. Afterwards, it will forward all future commands on that path to the main controller. It will take in a ``primary_controller`` parameter, containing the actual controller to use for path tracking once aligned with the path. 
+
+This is useful for situations when working with plugins that are either too specialized or tuned for a particular task that they can fail to adequately solve the full local planning problem performantly. Examples include:
+
+- Heavily tuning DWB for excellent path tracking makes it difficult to handle large deviations
+- TEB behavior tends to "whip" the robot around with small turns, in a somewhat scary way due to the elastic band approach
+- Neither TEB or DWB will simply rotate the robot in place to start tracking a new path. They instead perform a small 'spiral out' maneuver that is often clunky with odd velocities. You may prefer a clean and simple rotation in place.
+
+As such, this controller will check the rough heading difference with respect to the robot and a newly received path. If within a threshold, it will pass the request onto the controller to execute. If it is outside of the threshold, this controller will rotate the robot towards that path heading. Once it is within the tolerance, it will then pass off control-execution from this rotation shim controller onto the primary controller plugin. At this point, the robot is still going to be rotating, allowing the current plugin to take control for a smooth hand off into path tracking. It is recommended to be more generous than strict in the angular threshold to allow for a smoother transition, but should be tuned for a specific application's desired behaviors.
+
+When the `rotate_to_goal_heading` parameter is set to true, this controller is also able to take back control of the robot when reaching the XY goal tolerance of the goal checker. In this case, the robot will rotate towards the goal heading until the goal checker validate the goal and ends the current navigation task.
+
+The Rotation Shim Controller is suitable for:
+- Robots that can rotate in place, such as differential and omnidirectional robots.
+- Preference to rotate in place rather than 'spiral out' when starting to track a new path that is at a significantly different heading than the robot's current heading.
+- Using planners that are non-kinematically feasible, such as NavFn, Theta\*, or Smac 2D (Feasible planners such as Smac Hybrid-A* and State Lattice will start search from the robot's actual starting heading, requiring no rotation). 
+
+This plugin implements the `nav2_core::Controller` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). It will host an internal plugin of your actual path tracker (e.g. MPPI, RPP, DWB, TEB, etc) that will be used after the robot has rotated to the rough starting heading of the path.
+
+<p align="center">
+  <img src="https://user-images.githubusercontent.com/14944147/144323291-29e24521-674a-41f5-8a91-732121b26b47.gif">
+</p>
+
+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-rotation-shim-controller.html) for additional parameter descriptions.
+
+## Configuration
+
+| Parameter | Description | 
+|-----|----|
+| `angular_dist_threshold` | Maximum angular distance, in radians, away from the path heading to trigger rotation until within. | 
+| `forward_sampling_distance` | Forward distance, in meters, along path to select a sampling point to use to approximate path heading | 
+| `rotate_to_heading_angular_vel` | Angular rotational velocity, in rad/s, to rotate to the path heading | 
+| `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading | 
+| `max_angular_accel` | Maximum angular acceleration for rotation to heading | 
+| `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. | 
+| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading | 
+
+Example fully-described XML with default parameter values:
+
+```
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: "goal_checker"
+    controller_plugins: ["FollowPath"]
+
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    goal_checker:
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+      stateful: True
+    FollowPath:
+      plugin: "nav2_rotation_shim_controller::RotationShimController"
+      primary_controller: "dwb_core::DWBLocalPlanner"
+      angular_dist_threshold: 0.785
+      forward_sampling_distance: 0.5
+      rotate_to_heading_angular_vel: 1.8
+      max_angular_accel: 3.2
+      simulate_ahead_time: 1.0
+      rotate_to_goal_heading: false
+
+      # DWB parameters
+      ...
+      ...
+      ...
+```

navigation2/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp

@@ -0,0 +1,192 @@
+// 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.
+
+#ifndef NAV2_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_
+#define NAV2_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <algorithm>
+#include <mutex>
+#include <limits>
+
+#include "rclcpp/rclcpp.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_core/controller.hpp"
+#include "nav2_core/exceptions.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+#include "nav2_controller/plugins/position_goal_checker.hpp"
+#include "angles/angles.h"
+
+namespace nav2_rotation_shim_controller
+{
+
+/**
+ * @class nav2_rotation_shim_controller::RotationShimController
+ * @brief Rotate to rough path heading controller shim plugin
+ */
+class RotationShimController : public nav2_core::Controller
+{
+public:
+  /**
+   * @brief Constructor for nav2_rotation_shim_controller::RotationShimController
+   */
+  RotationShimController();
+
+  /**
+   * @brief Destrructor for nav2_rotation_shim_controller::RotationShimController
+   */
+  ~RotationShimController() override = default;
+
+  /**
+   * @brief Configure controller state machine
+   * @param parent WeakPtr to node
+   * @param name Name of plugin
+   * @param tf TF buffer
+   * @param costmap_ros Costmap2DROS object of environment
+   */
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
+
+  /**
+   * @brief Cleanup controller state machine
+   */
+  void cleanup() override;
+
+  /**
+   * @brief Activate controller state machine
+   */
+  void activate() override;
+
+  /**
+   * @brief Deactivate controller state machine
+   */
+  void deactivate() override;
+
+  /**
+   * @brief Compute the best command given the current pose and velocity
+   * @param pose      Current robot pose
+   * @param velocity  Current robot velocity
+   * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands
+   * @return          Best command
+   */
+  geometry_msgs::msg::TwistStamped computeVelocityCommands(
+    const geometry_msgs::msg::PoseStamped & pose,
+    const geometry_msgs::msg::Twist & velocity,
+    nav2_core::GoalChecker * /*goal_checker*/) override;
+
+  /**
+   * @brief nav2_core setPlan - Sets the global plan
+   * @param path The global plan
+   */
+  void setPlan(const nav_msgs::msg::Path & path) override;
+
+  /**
+   * @brief Limits the maximum linear speed of the robot.
+   * @param speed_limit expressed in absolute value (in m/s)
+   * or in percentage from maximum robot speed.
+   * @param percentage Setting speed limit in percentage if true
+   * or in absolute values in false case.
+   */
+  void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
+
+protected:
+  /**
+   * @brief Finds the point on the path that is roughly the sampling
+   * point distance away from the robot for use.
+   * May throw exception if a point at least that far away cannot be found
+   * @return pt location of the output point
+   */
+  geometry_msgs::msg::PoseStamped getSampledPathPt();
+
+  /**
+   * @brief Find the goal point in path
+   * May throw exception if the path is empty
+   * @return pt location of the output point
+   */
+  geometry_msgs::msg::PoseStamped getSampledPathGoal();
+
+  /**
+   * @brief Uses TF to find the location of the sampled path point in base frame
+   * @param pt location of the sampled path point
+   * @return location of the pose in base frame
+   */
+  geometry_msgs::msg::Pose transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt);
+
+  /**
+   * @brief Rotates the robot to the rough heading
+   * @param angular_distance Angular distance to the goal remaining
+   * @param pose Starting pose of robot
+   * @param velocity Starting velocity of robot
+   * @return Twist command for rotation to rough heading
+   */
+  geometry_msgs::msg::TwistStamped computeRotateToHeadingCommand(
+    const double & angular_distance,
+    const geometry_msgs::msg::PoseStamped & pose,
+    const geometry_msgs::msg::Twist & velocity);
+
+  /**
+   * @brief Checks if rotation is safe
+   * @param cmd_vel Velocity to check over
+   * @param angular_distance_to_heading Angular distance to heading requested
+   * @param pose Starting pose of robot
+   */
+  void isCollisionFree(
+    const geometry_msgs::msg::TwistStamped & cmd_vel,
+    const double & angular_distance_to_heading,
+    const geometry_msgs::msg::PoseStamped & pose);
+
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::string plugin_name_;
+  rclcpp::Logger logger_ {rclcpp::get_logger("RotationShimController")};
+  rclcpp::Clock::SharedPtr clock_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
+  collision_checker_;
+
+  pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
+  nav2_core::Controller::Ptr primary_controller_;
+  bool path_updated_;
+  nav_msgs::msg::Path current_path_;
+  double forward_sampling_distance_, angular_dist_threshold_, angular_disengage_threshold_;
+  double rotate_to_heading_angular_vel_, max_angular_accel_;
+  double control_duration_, simulate_ahead_time_;
+  bool rotate_to_goal_heading_, in_rotation_;
+  bool closed_loop_;
+  double last_angular_vel_ = std::numeric_limits<double>::max();
+
+  // Dynamic parameters handler
+  std::mutex mutex_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  std::unique_ptr<nav2_controller::PositionGoalChecker> position_goal_checker_;
+};
+
+}  // namespace nav2_rotation_shim_controller
+
+#endif  // NAV2_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_

navigation2/nav2_rotation_shim_controller/nav2_rotation_shim_controller.xml

@@ -0,0 +1,9 @@
+<class_libraries>
+    <library path="nav2_rotation_shim_controller">
+        <class type="nav2_rotation_shim_controller::RotationShimController" base_class_type="nav2_core::Controller">
+            <description>
+                nav2_rotation_shim_controller
+            </description>
+        </class>
+    </library>
+</class_libraries>

navigation2/nav2_rotation_shim_controller/package.xml

@@ -0,0 +1,35 @@
+<?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_rotation_shim_controller</name>
+  <version>1.1.18</version>
+  <description>Rotation Shim Controller</description>
+  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>nav2_common</depend>
+  <depend>nav2_core</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav2_controller</depend>
+  <depend>angles</depend>
+  <depend>pluginlib</depend>
+  <depend>tf2</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>nav2_regulated_pure_pursuit_controller</test_depend>
+  <test_depend>nav2_controller</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <nav2_core plugin="${prefix}/nav2_rotation_shim_controller.xml" />
+  </export>
+
+</package>

navigation2/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

@@ -0,0 +1,408 @@
+// Copyright (c) 2021 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <algorithm>
+#include <string>
+#include <limits>
+#include <memory>
+#include <vector>
+#include <utility>
+
+#include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp"
+
+using rcl_interfaces::msg::ParameterType;
+
+namespace nav2_rotation_shim_controller
+{
+
+RotationShimController::RotationShimController()
+: lp_loader_("nav2_core", "nav2_core::Controller"),
+  primary_controller_(nullptr),
+  path_updated_(false),
+  in_rotation_(false)
+{
+}
+
+void RotationShimController::configure(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  position_goal_checker_ = std::make_unique<nav2_controller::PositionGoalChecker>();
+  position_goal_checker_->initialize(parent, plugin_name_ + ".position_checker", costmap_ros);
+  plugin_name_ = name;
+  node_ = parent;
+  auto node = parent.lock();
+
+  tf_ = tf;
+  costmap_ros_ = costmap_ros;
+  logger_ = node->get_logger();
+  clock_ = node->get_clock();
+
+  std::string primary_controller;
+  double control_frequency;
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".angular_dist_threshold", rclcpp::ParameterValue(0.785));  // 45 deg
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".angular_disengage_threshold", rclcpp::ParameterValue(0.785));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".forward_sampling_distance", rclcpp::ParameterValue(0.5));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".simulate_ahead_time", rclcpp::ParameterValue(1.0));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING);
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".closed_loop", rclcpp::ParameterValue(true));
+
+  node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_);
+  node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_);
+  node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_);
+  node->get_parameter(
+    plugin_name_ + ".rotate_to_heading_angular_vel",
+    rotate_to_heading_angular_vel_);
+  node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_);
+  node->get_parameter(plugin_name_ + ".simulate_ahead_time", simulate_ahead_time_);
+
+  primary_controller = node->get_parameter(plugin_name_ + ".primary_controller").as_string();
+  node->get_parameter("controller_frequency", control_frequency);
+  control_duration_ = 1.0 / control_frequency;
+
+  node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_);
+  node->get_parameter(plugin_name_ + ".closed_loop", closed_loop_);
+
+  try {
+    primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
+    RCLCPP_INFO(
+      logger_, "Created internal controller for rotation shimming: %s of type %s",
+      plugin_name_.c_str(), primary_controller.c_str());
+  } catch (const pluginlib::PluginlibException & ex) {
+    RCLCPP_FATAL(
+      logger_,
+      "Failed to create internal controller for rotation shimming. Exception: %s", ex.what());
+    return;
+  }
+
+  primary_controller_->configure(parent, name, tf, costmap_ros);
+
+  // initialize collision checker and set costmap
+  collision_checker_ = std::make_unique<nav2_costmap_2d::
+      FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>(costmap_ros->getCostmap());
+}
+
+void RotationShimController::activate()
+{
+  RCLCPP_INFO(
+    logger_,
+    "Activating controller: %s of type "
+    "nav2_rotation_shim_controller::RotationShimController",
+    plugin_name_.c_str());
+
+  primary_controller_->activate();
+  in_rotation_ = false;
+  last_angular_vel_ = std::numeric_limits<double>::max();
+
+  auto node = node_.lock();
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &RotationShimController::dynamicParametersCallback,
+      this, std::placeholders::_1));
+    position_goal_checker_->reset();
+}
+
+void RotationShimController::deactivate()
+{
+  RCLCPP_INFO(
+    logger_,
+    "Deactivating controller: %s of type "
+    "nav2_rotation_shim_controller::RotationShimController",
+    plugin_name_.c_str());
+
+  primary_controller_->deactivate();
+
+  dyn_params_handler_.reset();
+}
+
+void RotationShimController::cleanup()
+{
+  RCLCPP_INFO(
+    logger_,
+    "Cleaning up controller: %s of type "
+    "nav2_rotation_shim_controller::RotationShimController",
+    plugin_name_.c_str());
+
+  primary_controller_->cleanup();
+  primary_controller_.reset();
+  position_goal_checker_.reset();
+}
+
+geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands(
+  const geometry_msgs::msg::PoseStamped & pose,
+  const geometry_msgs::msg::Twist & velocity,
+  nav2_core::GoalChecker * goal_checker)
+{
+  // Rotate to goal heading when in goal xy tolerance
+  if (rotate_to_goal_heading_) {
+    std::lock_guard<std::mutex> lock_reinit(mutex_);
+
+    try {
+      geometry_msgs::msg::PoseStamped sampled_pt_goal = getSampledPathGoal();
+
+      if (!nav2_util::transformPoseInTargetFrame(
+          sampled_pt_goal, sampled_pt_goal, *tf_,
+          pose.header.frame_id))
+      {
+        throw std::runtime_error("Failed to transform pose to base frame!");
+      }
+
+      geometry_msgs::msg::Pose pose_tolerance;
+      geometry_msgs::msg::Twist vel_tolerance;
+      goal_checker->getTolerances(pose_tolerance, vel_tolerance);
+      position_goal_checker_->setXYGoalTolerance(pose_tolerance.position.x);
+
+      if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity)) {
+        double pose_yaw = tf2::getYaw(pose.pose.orientation);
+        double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation);
+
+        double angular_distance_to_heading = angles::shortest_angular_distance(pose_yaw, goal_yaw);
+
+        auto cmd_vel = computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
+        last_angular_vel_ = cmd_vel.twist.angular.z;
+        return cmd_vel;
+      }
+    } catch (const std::runtime_error & e) {
+      RCLCPP_INFO(
+        logger_,
+        "Rotation Shim Controller was unable to find a goal point,"
+        " a rotational collision was detected, or TF failed to transform"
+        " into base frame! what(): %s", e.what());
+    }
+  }
+
+  if (path_updated_) {
+    nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
+    std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
+    std::lock_guard<std::mutex> lock_reinit(mutex_);
+    try {
+      geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(getSampledPathPt());
+
+      double angular_distance_to_heading =
+        std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x);
+
+      double angular_thresh =
+        in_rotation_ ? angular_disengage_threshold_ : angular_dist_threshold_;
+      if (abs(angular_distance_to_heading) > angular_thresh) {
+        RCLCPP_DEBUG(
+          logger_,
+          "Robot is not within the new path's rough heading, rotating to heading...");
+        in_rotation_ = true;
+        auto cmd_vel = computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
+        last_angular_vel_ = cmd_vel.twist.angular.z;
+        return cmd_vel;
+      } else {
+        RCLCPP_DEBUG(
+          logger_,
+          "Robot is at the new path's rough heading, passing to controller");
+        path_updated_ = false;
+      }
+    } catch (const std::runtime_error & e) {
+      RCLCPP_DEBUG(
+        logger_,
+        "Rotation Shim Controller was unable to find a sampling point,"
+        " a rotational collision was detected, or TF failed to transform"
+        " into base frame! what(): %s", e.what());
+      path_updated_ = false;
+    }
+  }
+
+  // If at this point, use the primary controller to path track
+  in_rotation_ = false;
+  auto cmd_vel = primary_controller_->computeVelocityCommands(pose, velocity, goal_checker);
+  last_angular_vel_ = cmd_vel.twist.angular.z;
+  return cmd_vel;
+}
+
+geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt()
+{
+  if (current_path_.poses.size() < 2) {
+    throw nav2_core::PlannerException(
+            "Path is too short to find a valid sampled path point for rotation.");
+  }
+
+  geometry_msgs::msg::Pose start = current_path_.poses.front().pose;
+  double dx, dy;
+
+  // Find the first point at least sampling distance away
+  for (unsigned int i = 1; i != current_path_.poses.size(); i++) {
+    dx = current_path_.poses[i].pose.position.x - start.position.x;
+    dy = current_path_.poses[i].pose.position.y - start.position.y;
+    if (hypot(dx, dy) >= forward_sampling_distance_) {
+      current_path_.poses[i].header.frame_id = current_path_.header.frame_id;
+      current_path_.poses[i].header.stamp = clock_->now();  // Get current time transformation
+      return current_path_.poses[i];
+    }
+  }
+
+  throw nav2_core::PlannerException(
+          std::string(
+            "Unable to find a sampling point at least %0.2f from the robot,"
+            "passing off to primary controller plugin.", forward_sampling_distance_));
+}
+
+geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal()
+{
+  if (current_path_.poses.empty()) {
+    throw std::runtime_error("Path is empty - cannot find a goal point");
+  }
+
+  auto goal = current_path_.poses.back();
+  goal.header.frame_id = current_path_.header.frame_id;
+  goal.header.stamp = clock_->now();
+  return goal;
+}
+
+geometry_msgs::msg::Pose
+RotationShimController::transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt)
+{
+  geometry_msgs::msg::PoseStamped pt_base;
+  if (!nav2_util::transformPoseInTargetFrame(pt, pt_base, *tf_, costmap_ros_->getBaseFrameID())) {
+    throw nav2_core::PlannerException("Failed to transform pose to base frame!");
+  }
+  return pt_base.pose;
+}
+
+geometry_msgs::msg::TwistStamped
+RotationShimController::computeRotateToHeadingCommand(
+  const double & angular_distance_to_heading,
+  const geometry_msgs::msg::PoseStamped & pose,
+  const geometry_msgs::msg::Twist & velocity)
+{
+  auto current = closed_loop_ ? velocity.angular.z : last_angular_vel_;
+  if (current == std::numeric_limits<double>::max()) {
+    current = 0.0;
+  }
+
+  geometry_msgs::msg::TwistStamped cmd_vel;
+  cmd_vel.header = pose.header;
+  const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
+  const double angular_vel = sign * rotate_to_heading_angular_vel_;
+  const double & dt = control_duration_;
+  const double min_feasible_angular_speed = current - max_angular_accel_ * dt;
+  const double max_feasible_angular_speed = current + max_angular_accel_ * dt;
+  cmd_vel.twist.angular.z =
+    std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
+
+  isCollisionFree(cmd_vel, angular_distance_to_heading, pose);
+  return cmd_vel;
+}
+
+void RotationShimController::isCollisionFree(
+  const geometry_msgs::msg::TwistStamped & cmd_vel,
+  const double & angular_distance_to_heading,
+  const geometry_msgs::msg::PoseStamped & pose)
+{
+  // Simulate rotation ahead by time in control frequency increments
+  double simulated_time = 0.0;
+  double initial_yaw = tf2::getYaw(pose.pose.orientation);
+  double yaw = 0.0;
+  double footprint_cost = 0.0;
+  double remaining_rotation_before_thresh =
+    fabs(angular_distance_to_heading) - angular_dist_threshold_;
+
+  while (simulated_time < simulate_ahead_time_) {
+    simulated_time += control_duration_;
+    yaw = initial_yaw + cmd_vel.twist.angular.z * simulated_time;
+
+    // Stop simulating past the point it would be passed onto the primary controller
+    if (angles::shortest_angular_distance(yaw, initial_yaw) >= remaining_rotation_before_thresh) {
+      break;
+    }
+
+    using namespace nav2_costmap_2d;  // NOLINT
+    footprint_cost = collision_checker_->footprintCostAtPose(
+      pose.pose.position.x, pose.pose.position.y,
+      yaw, costmap_ros_->getRobotFootprint());
+
+    if (footprint_cost == static_cast<double>(NO_INFORMATION) &&
+      costmap_ros_->getLayeredCostmap()->isTrackingUnknown())
+    {
+      throw std::runtime_error("RotationShimController detected a potential collision ahead!");
+    }
+
+    if (footprint_cost >= static_cast<double>(LETHAL_OBSTACLE)) {
+      throw std::runtime_error("RotationShimController detected collision ahead!");
+    }
+  }
+}
+
+void RotationShimController::setPlan(const nav_msgs::msg::Path & path)
+{
+  path_updated_ = true;
+  current_path_ = path;
+  primary_controller_->setPlan(path);
+  position_goal_checker_->reset();
+}
+
+void RotationShimController::setSpeedLimit(const double & speed_limit, const bool & percentage)
+{
+  primary_controller_->setSpeedLimit(speed_limit, percentage);
+}
+
+rcl_interfaces::msg::SetParametersResult
+RotationShimController::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  std::lock_guard<std::mutex> lock_reinit(mutex_);
+
+  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_ + ".angular_dist_threshold") {
+        angular_dist_threshold_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".forward_sampling_distance") {
+        forward_sampling_distance_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") {
+        rotate_to_heading_angular_vel_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".max_angular_accel") {
+        max_angular_accel_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".simulate_ahead_time") {
+        simulate_ahead_time_ = parameter.as_double();
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == plugin_name_ + ".rotate_to_goal_heading") {
+        rotate_to_goal_heading_ = parameter.as_bool();
+      } else if (name == plugin_name_ + ".closed_loop") {
+        closed_loop_ = parameter.as_bool();
+      }
+    }
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_rotation_shim_controller
+
+// Register this controller as a nav2_core plugin
+PLUGINLIB_EXPORT_CLASS(
+  nav2_rotation_shim_controller::RotationShimController,
+  nav2_core::Controller)

navigation2/nav2_rotation_shim_controller/test/CMakeLists.txt

@@ -0,0 +1,13 @@
+# tests
+find_package(nav2_controller REQUIRED)
+
+ament_add_gtest(test_shim_controller
+  test_shim_controller.cpp
+)
+ament_target_dependencies(test_shim_controller
+  ${dependencies}
+  nav2_controller
+)
+target_link_libraries(test_shim_controller
+  ${library_name}
+)

navigation2/nav2_rotation_shim_controller/test/test_shim_controller.cpp

@@ -0,0 +1,507 @@
+// Copyright (c) 2021 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <math.h>
+#include <memory>
+#include <string>
+#include <vector>
+#include <limits>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_controller/plugins/simple_goal_checker.hpp"
+#include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class RotationShimShim : public nav2_rotation_shim_controller::RotationShimController
+{
+public:
+  RotationShimShim()
+  : nav2_rotation_shim_controller::RotationShimController()
+  {
+  }
+
+  nav2_core::Controller::Ptr getPrimaryController()
+  {
+    return primary_controller_;
+  }
+
+  nav_msgs::msg::Path getPath()
+  {
+    return current_path_;
+  }
+
+  bool isPathUpdated()
+  {
+    return path_updated_;
+  }
+
+  geometry_msgs::msg::PoseStamped getSampledPathPtWrapper()
+  {
+    return getSampledPathPt();
+  }
+
+  geometry_msgs::msg::Pose transformPoseToBaseFrameWrapper(geometry_msgs::msg::PoseStamped pt)
+  {
+    return transformPoseToBaseFrame(pt);
+  }
+
+  geometry_msgs::msg::TwistStamped
+  computeRotateToHeadingCommandWrapper(
+    const double & param,
+    const geometry_msgs::msg::PoseStamped & pose,
+    const geometry_msgs::msg::Twist & velocity)
+  {
+    return computeRotateToHeadingCommand(param, pose, velocity);
+  }
+};
+
+TEST(RotationShimControllerTest, lifecycleTransitions)
+{
+  auto ctrl = std::make_shared<RotationShimShim>();
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
+  std::string name = "PathFollower";
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
+
+  // Should not populate primary controller, does not exist
+  EXPECT_THROW(ctrl->configure(node, name, tf, costmap), std::runtime_error);
+  EXPECT_EQ(ctrl->getPrimaryController(), nullptr);
+
+  // Add a controller to the setup
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    node->get_node_base_interface(), node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface());
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter(
+        "PathFollower.primary_controller",
+        std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"))});
+  rclcpp::spin_until_future_complete(
+    node->get_node_base_interface(),
+    results);
+
+  ctrl->configure(node, name, tf, costmap);
+  EXPECT_NE(ctrl->getPrimaryController(), nullptr);
+
+  ctrl->activate();
+
+  ctrl->setSpeedLimit(50.0, true);
+
+  ctrl->deactivate();
+  ctrl->cleanup();
+}
+
+TEST(RotationShimControllerTest, setPlanAndSampledPointsTests)
+{
+  auto ctrl = std::make_shared<RotationShimShim>();
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
+  std::string name = "PathFollower";
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
+
+  // set a valid primary controller so we can do lifecycle
+  node->declare_parameter(
+    "PathFollower.primary_controller",
+    std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"));
+
+  auto controller = std::make_shared<RotationShimShim>();
+  controller->configure(node, name, tf, costmap);
+  controller->activate();
+
+  // Test state update and path setting
+  nav_msgs::msg::Path path;
+  path.header.frame_id = "hi mate!";
+  path.poses.resize(10);
+  path.poses[1].pose.position.x = 0.1;
+  path.poses[1].pose.position.y = 0.1;
+  path.poses[2].pose.position.x = 1.0;
+  path.poses[2].pose.position.y = 1.0;
+  path.poses[3].pose.position.x = 10.0;
+  path.poses[3].pose.position.y = 10.0;
+  EXPECT_EQ(controller->isPathUpdated(), false);
+  controller->setPlan(path);
+  EXPECT_EQ(controller->getPath().header.frame_id, std::string("hi mate!"));
+  EXPECT_EQ(controller->getPath().poses.size(), 10u);
+  EXPECT_EQ(controller->isPathUpdated(), true);
+
+  // Test getting a sampled point
+  auto pose = controller->getSampledPathPtWrapper();
+  EXPECT_EQ(pose.pose.position.x, 1.0);  // default forward sampling is 0.5
+  EXPECT_EQ(pose.pose.position.y, 1.0);
+
+  nav_msgs::msg::Path path_invalid_leng;
+  controller->setPlan(path_invalid_leng);
+  EXPECT_THROW(controller->getSampledPathPtWrapper(), std::runtime_error);
+
+  nav_msgs::msg::Path path_invalid_dists;
+  path.poses.resize(10);
+  controller->setPlan(path_invalid_dists);
+  EXPECT_THROW(controller->getSampledPathPtWrapper(), std::runtime_error);
+}
+
+TEST(RotationShimControllerTest, rotationAndTransformTests)
+{
+  auto ctrl = std::make_shared<RotationShimShim>();
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
+  std::string name = "PathFollower";
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
+
+  // set a valid primary controller so we can do lifecycle
+  node->declare_parameter(
+    "PathFollower.primary_controller",
+    std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"));
+
+  auto controller = std::make_shared<RotationShimShim>();
+  controller->configure(node, name, tf, costmap);
+  controller->activate();
+
+  // Test state update and path setting
+  nav_msgs::msg::Path path;
+  path.header.frame_id = "fake_frame";
+  path.poses.resize(10);
+  path.poses[1].pose.position.x = 0.1;
+  path.poses[1].pose.position.y = 0.1;
+  path.poses[2].pose.position.x = 1.0;
+  path.poses[2].pose.position.y = 1.0;
+  path.poses[3].pose.position.x = 10.0;
+  path.poses[3].pose.position.y = 10.0;
+  controller->setPlan(path);
+
+  const geometry_msgs::msg::Twist velocity;
+  EXPECT_EQ(
+    controller->computeRotateToHeadingCommandWrapper(
+      0.7, path.poses[0], velocity).twist.angular.z, 1.8);
+  EXPECT_EQ(
+    controller->computeRotateToHeadingCommandWrapper(
+      -0.7, path.poses[0], velocity).twist.angular.z, -1.8);
+
+  EXPECT_EQ(
+    controller->computeRotateToHeadingCommandWrapper(
+      0.87, path.poses[0], velocity).twist.angular.z, 1.8);
+
+  // in base_link, so should pass through values without issue
+  geometry_msgs::msg::PoseStamped pt;
+  pt.pose.position.x = 100.0;
+  pt.header.frame_id = "base_link";
+  pt.header.stamp = rclcpp::Time();
+  auto rtn = controller->transformPoseToBaseFrameWrapper(pt);
+  EXPECT_EQ(rtn.position.x, 100.0);
+
+  // in frame that doesn't exist, shouldn't throw, but should fail
+  geometry_msgs::msg::PoseStamped pt2;
+  pt.pose.position.x = 100.0;
+  pt.header.frame_id = "fake_frame2";
+  pt.header.stamp = rclcpp::Time();
+  EXPECT_THROW(controller->transformPoseToBaseFrameWrapper(pt2), std::runtime_error);
+}
+
+TEST(RotationShimControllerTest, computeVelocityTests)
+{
+  auto ctrl = std::make_shared<RotationShimShim>();
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
+  std::string name = "PathFollower";
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  auto listener = std::make_shared<tf2_ros::TransformListener>(*tf, node, true);
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
+  auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(node);
+
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.frame_id = "base_link";
+  transform.child_frame_id = "odom";
+  transform.transform.rotation.x = 0.0;
+  transform.transform.rotation.y = 0.0;
+  transform.transform.rotation.z = 0.0;
+  transform.transform.rotation.w = 1.0;
+  tf_broadcaster->sendTransform(transform);
+
+  // set a valid primary controller so we can do lifecycle
+  node->declare_parameter(
+    "PathFollower.primary_controller",
+    std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"));
+
+  auto controller = std::make_shared<RotationShimShim>();
+  controller->configure(node, name, tf, costmap);
+  controller->activate();
+
+  // Test state update and path setting
+  nav_msgs::msg::Path path;
+  path.header.frame_id = "fake_frame";
+  path.poses.resize(10);
+
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = "base_link";
+  geometry_msgs::msg::Twist velocity;
+  nav2_controller::SimpleGoalChecker checker;
+  checker.initialize(node, "checker", costmap);
+
+  // send without setting a path - should go to RPP immediately
+  // then it should throw an exception because the path is empty and invalid
+  EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error);
+
+  // Set with a path -- should attempt to find a sampled point but throw exception
+  // because it cannot be found, then go to RPP and throw exception because it cannot be transformed
+  controller->setPlan(path);
+  EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error);
+
+  path.header.frame_id = "base_link";
+  path.poses[1].pose.position.x = 0.1;
+  path.poses[1].pose.position.y = 0.1;
+  path.poses[2].pose.position.x = -1.0;
+  path.poses[2].pose.position.y = -1.0;
+  path.poses[2].header.frame_id = "base_link";
+  path.poses[3].pose.position.x = 10.0;
+  path.poses[3].pose.position.y = 10.0;
+
+  // this should allow it to find the sampled point, then transform to base_link
+  // validly because we setup the TF for it. The -1.0 should be selected since default min
+  // is 0.5 and that should cause a rotation in place
+  controller->setPlan(path);
+  tf_broadcaster->sendTransform(transform);
+  auto effort = controller->computeVelocityCommands(pose, velocity, &checker);
+  EXPECT_EQ(fabs(effort.twist.angular.z), 1.8);
+
+  path.header.frame_id = "base_link";
+  path.poses[1].pose.position.x = 0.1;
+  path.poses[1].pose.position.y = 0.1;
+  path.poses[2].pose.position.x = 1.0;
+  path.poses[2].pose.position.y = 0.0;
+  path.poses[2].header.frame_id = "base_link";
+  path.poses[3].pose.position.x = 10.0;
+  path.poses[3].pose.position.y = 10.0;
+
+  // this should allow it to find the sampled point, then transform to base_link
+  // validly because we setup the TF for it. The 1.0 should be selected since default min
+  // is 0.5 and that should cause a pass off to the RPP controller which will throw
+  // and exception because the costmap is bogus
+  controller->setPlan(path);
+  tf_broadcaster->sendTransform(transform);
+  EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error);
+}
+
+TEST(RotationShimControllerTest, openLoopRotationTests) {
+  auto ctrl = std::make_shared<RotationShimShim>();
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
+  std::string name = "PathFollower";
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  auto listener = std::make_shared<tf2_ros::TransformListener>(*tf, node, true);
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
+  auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(node);
+
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.frame_id = "base_link";
+  transform.child_frame_id = "odom";
+  transform.transform.rotation.x = 0.0;
+  transform.transform.rotation.y = 0.0;
+  transform.transform.rotation.z = 0.0;
+  transform.transform.rotation.w = 1.0;
+  tf_broadcaster->sendTransform(transform);
+
+  // set a valid primary controller so we can do lifecycle
+  node->declare_parameter(
+    "PathFollower.primary_controller",
+    std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"));
+  node->declare_parameter(
+    "controller_frequency",
+    20.0);
+  node->declare_parameter(
+    "PathFollower.rotate_to_goal_heading",
+    true);
+  node->declare_parameter(
+    "PathFollower.closed_loop",
+    false);
+
+  auto controller = std::make_shared<RotationShimShim>();
+  controller->configure(node, name, tf, costmap);
+  controller->activate();
+
+  // Test state update and path setting
+  nav_msgs::msg::Path path;
+  path.header.frame_id = "base_link";
+  path.poses.resize(4);
+
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = "base_link";
+  geometry_msgs::msg::Twist velocity;
+  nav2_controller::SimpleGoalChecker checker;
+  node->declare_parameter(
+    "checker.xy_goal_tolerance",
+    1.0);
+  checker.initialize(node, "checker", costmap);
+
+  path.header.frame_id = "base_link";
+  path.poses[0].pose.position.x = 0.0;
+  path.poses[0].pose.position.y = 0.0;
+  path.poses[1].pose.position.x = 0.05;
+  path.poses[1].pose.position.y = 0.05;
+  path.poses[2].pose.position.x = 0.10;
+  path.poses[2].pose.position.y = 0.10;
+  // goal position within checker xy_goal_tolerance
+  path.poses[3].pose.position.x = 0.20;
+  path.poses[3].pose.position.y = 0.20;
+  // goal heading 45 degrees to the left
+  path.poses[3].pose.orientation.z = -0.3826834;
+  path.poses[3].pose.orientation.w = 0.9238795;
+  path.poses[3].header.frame_id = "base_link";
+
+  // Calculate first velocity command
+  controller->setPlan(path);
+  auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker);
+  EXPECT_NEAR(cmd_vel.twist.angular.z, -0.16, 1e-4);
+
+  // Test second velocity command with wrong odometry
+  velocity.angular.z = 1.8;
+  cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker);
+  EXPECT_NEAR(cmd_vel.twist.angular.z, -0.32, 1e-4);
+}
+
+TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) {
+  auto ctrl = std::make_shared<RotationShimShim>();
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
+  std::string name = "PathFollower";
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  auto listener = std::make_shared<tf2_ros::TransformListener>(*tf, node, true);
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
+  auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(node);
+
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.frame_id = "base_link";
+  transform.child_frame_id = "odom";
+  transform.transform.rotation.x = 0.0;
+  transform.transform.rotation.y = 0.0;
+  transform.transform.rotation.z = 0.0;
+  transform.transform.rotation.w = 1.0;
+  tf_broadcaster->sendTransform(transform);
+
+  // set a valid primary controller so we can do lifecycle
+  node->declare_parameter(
+    "PathFollower.primary_controller",
+    std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"));
+  node->declare_parameter(
+    "PathFollower.rotate_to_goal_heading",
+    true);
+
+  auto controller = std::make_shared<RotationShimShim>();
+  controller->configure(node, name, tf, costmap);
+  controller->activate();
+
+  // Test state update and path setting
+  nav_msgs::msg::Path path;
+  path.header.frame_id = "base_link";
+  path.poses.resize(4);
+
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = "base_link";
+  geometry_msgs::msg::Twist velocity;
+  nav2_controller::SimpleGoalChecker checker;
+  node->declare_parameter(
+    "checker.xy_goal_tolerance",
+    1.0);
+  checker.initialize(node, "checker", costmap);
+
+  path.header.frame_id = "base_link";
+  path.poses[0].pose.position.x = 0.0;
+  path.poses[0].pose.position.y = 0.0;
+  path.poses[1].pose.position.x = 0.05;
+  path.poses[1].pose.position.y = 0.05;
+  path.poses[2].pose.position.x = 0.10;
+  path.poses[2].pose.position.y = 0.10;
+  // goal position within checker xy_goal_tolerance
+  path.poses[3].pose.position.x = 0.20;
+  path.poses[3].pose.position.y = 0.20;
+  // goal heading 45 degrees to the left
+  path.poses[3].pose.orientation.z = -0.3826834;
+  path.poses[3].pose.orientation.w = 0.9238795;
+  path.poses[3].header.frame_id = "base_link";
+
+  controller->setPlan(path);
+  auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker);
+  EXPECT_EQ(cmd_vel.twist.angular.z, -1.8);
+
+  // goal heading 45 degrees to the right
+  path.poses[3].pose.orientation.z = 0.3826834;
+  path.poses[3].pose.orientation.w = 0.9238795;
+  controller->setPlan(path);
+  cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker);
+  EXPECT_EQ(cmd_vel.twist.angular.z, 1.8);
+}
+
+TEST(RotationShimControllerTest, testDynamicParameter)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
+  std::string name = "test";
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
+
+  // set a valid primary controller so we can do lifecycle
+  node->declare_parameter(
+    "test.primary_controller",
+    std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"));
+
+  auto controller = std::make_shared<RotationShimShim>();
+  controller->configure(node, name, tf, costmap);
+  controller->activate();
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    node->get_node_base_interface(), node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("test.angular_dist_threshold", 7.0),
+      rclcpp::Parameter("test.forward_sampling_distance", 7.0),
+      rclcpp::Parameter("test.rotate_to_heading_angular_vel", 7.0),
+      rclcpp::Parameter("test.max_angular_accel", 7.0),
+      rclcpp::Parameter("test.simulate_ahead_time", 7.0),
+      rclcpp::Parameter("test.primary_controller", std::string("HI")),
+      rclcpp::Parameter("test.rotate_to_goal_heading", true),
+      rclcpp::Parameter("test.closed_loop", false)});
+
+  rclcpp::spin_until_future_complete(
+    node->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(node->get_parameter("test.angular_dist_threshold").as_double(), 7.0);
+  EXPECT_EQ(node->get_parameter("test.forward_sampling_distance").as_double(), 7.0);
+  EXPECT_EQ(node->get_parameter("test.rotate_to_heading_angular_vel").as_double(), 7.0);
+  EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0);
+  EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0);
+  EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true);
+  EXPECT_EQ(node->get_parameter("test.closed_loop").as_bool(), false);
+}