From d8bd3c5ea8d196ad7f0bace1edaeaa8e55f06944 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Tue, 16 Mar 2021 15:37:07 -0700
Subject: [PATCH]  Controller plugins include goal checker when computing
 commands (#2252)

* adding goal checker to the API for controllers and the controller server

* updating DWB and RPP to include the new controller API change to include the goal checker pointer

* adding getTolerances functions for goal checkers with unset default method

* use getTolerances for goal checkers in RPP

* adding missing headers to goal stoppers

* fix linting of RPP
---
 .../plugins/simple_goal_checker.hpp           |  3 +
 .../plugins/stopped_goal_checker.hpp          |  3 +
 .../plugins/simple_goal_checker.cpp           | 25 ++++++
 .../plugins/stopped_goal_checker.cpp          | 22 +++++
 nav2_controller/src/nav2_controller.cpp       |  3 +-
 nav2_core/include/nav2_core/controller.hpp    |  5 +-
 nav2_core/include/nav2_core/goal_checker.hpp  | 15 ++++
 .../include/dwb_core/dwb_local_planner.hpp    |  4 +-
 .../dwb_core/src/dwb_local_planner.cpp        |  3 +-
 .../README.md                                 |  3 -
 .../regulated_pure_pursuit_controller.hpp     | 17 ++--
 .../src/regulated_pure_pursuit_controller.cpp | 83 +++++++++++++------
 .../test/test_regulated_pp.cpp                | 59 ++++++++-----
 13 files changed, 182 insertions(+), 63 deletions(-)

diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp
index 689aa4bf..705ee2a0 100644
--- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp
+++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp
@@ -64,6 +64,9 @@ public:
   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_;
diff --git a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp
index 160ac60f..ee573de8 100644
--- a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp
+++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp
@@ -60,6 +60,9 @@ public:
   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_;
diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp
index 0f5a7e53..e03ce8b4 100644
--- a/nav2_controller/plugins/simple_goal_checker.cpp
+++ b/nav2_controller/plugins/simple_goal_checker.cpp
@@ -34,10 +34,12 @@
 
 #include <memory>
 #include <string>
+#include <limits>
 #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"
@@ -119,6 +121,29 @@ bool SimpleGoalChecker::isGoalReached(
   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;
+}
+
 void
 SimpleGoalChecker::on_parameter_event_callback(
   const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp
index d6d409f1..03319aaf 100644
--- a/nav2_controller/plugins/stopped_goal_checker.cpp
+++ b/nav2_controller/plugins/stopped_goal_checker.cpp
@@ -35,6 +35,7 @@
 #include <cmath>
 #include <string>
 #include <memory>
+#include <limits>
 #include "nav2_controller/plugins/stopped_goal_checker.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "nav2_util/node_utils.hpp"
@@ -82,6 +83,27 @@ bool StoppedGoalChecker::isGoalReached(
          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;
+}
+
 }  // namespace nav2_controller
 
 PLUGINLIB_EXPORT_CLASS(nav2_controller::StoppedGoalChecker, nav2_core::GoalChecker)
diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp
index b3fe1dc2..c2d9e188 100644
--- a/nav2_controller/src/nav2_controller.cpp
+++ b/nav2_controller/src/nav2_controller.cpp
@@ -388,7 +388,8 @@ void ControllerServer::computeAndPublishVelocity()
   auto cmd_vel_2d =
     controllers_[current_controller_]->computeVelocityCommands(
     pose,
-    nav_2d_utils::twist2Dto3D(twist));
+    nav_2d_utils::twist2Dto3D(twist),
+    goal_checker_.get());
 
   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);
diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp
index b9eb5a45..d1fb1053 100644
--- a/nav2_core/include/nav2_core/controller.hpp
+++ b/nav2_core/include/nav2_core/controller.hpp
@@ -47,6 +47,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/path.hpp"
+#include "nav2_core/goal_checker.hpp"
 
 
 namespace nav2_core
@@ -107,11 +108,13 @@ public:
    *
    * @param pose Current robot pose
    * @param velocity Current robot velocity
+   * @param goal_checker Pointer to the current goal checker the task is utilizing
    * @return The best command for the robot to drive
    */
   virtual geometry_msgs::msg::TwistStamped computeVelocityCommands(
     const geometry_msgs::msg::PoseStamped & pose,
-    const geometry_msgs::msg::Twist & velocity) = 0;
+    const geometry_msgs::msg::Twist & velocity,
+    nav2_core::GoalChecker * goal_checker) = 0;
 
   /**
    * @brief Limits the maximum linear speed of the robot.
diff --git a/nav2_core/include/nav2_core/goal_checker.hpp b/nav2_core/include/nav2_core/goal_checker.hpp
index 2cb33168..fc1d34f0 100644
--- a/nav2_core/include/nav2_core/goal_checker.hpp
+++ b/nav2_core/include/nav2_core/goal_checker.hpp
@@ -70,6 +70,7 @@ public:
     const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
     const std::string & plugin_name) = 0;
   virtual void reset() = 0;
+
   /**
    * @brief Check whether the goal should be considered reached
    * @param query_pose The pose to check
@@ -80,6 +81,20 @@ public:
   virtual bool isGoalReached(
     const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
     const geometry_msgs::msg::Twist & velocity) = 0;
+
+  /**
+   * @brief Get the maximum possible tolerances used for goal checking in the major types.
+   * Any field without a valid entry is replaced with std::numeric_limits<double>::lowest()
+   * to indicate that it is not measured. For tolerance across multiple entries
+   * (e.x. XY tolerances), both fields will contain this value since it is the maximum tolerance
+   * that each independent field could be assuming the other has no error (e.x. X and Y).
+   * @param pose_tolerance The tolerance used for checking in Pose fields
+   * @param vel_tolerance The tolerance used for checking velocity fields
+   * @return True if the tolerances are valid to use
+   */
+  virtual bool getTolerances(
+    geometry_msgs::msg::Pose & pose_tolerance,
+    geometry_msgs::msg::Twist & vel_tolerance) = 0;
 };
 
 }  // namespace nav2_core
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
index b3a4f419..c95de462 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
+++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
@@ -93,11 +93,13 @@ public:
    *
    * @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 The best command for the robot to drive
    */
   geometry_msgs::msg::TwistStamped computeVelocityCommands(
     const geometry_msgs::msg::PoseStamped & pose,
-    const geometry_msgs::msg::Twist & velocity) override;
+    const geometry_msgs::msg::Twist & velocity,
+    nav2_core::GoalChecker * /*goal_checker*/) override;
 
   /**
    * @brief Score a given command. Can be used for testing.
diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
index d6228dff..ab7baed0 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
@@ -235,7 +235,8 @@ DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path)
 geometry_msgs::msg::TwistStamped
 DWBLocalPlanner::computeVelocityCommands(
   const geometry_msgs::msg::PoseStamped & pose,
-  const geometry_msgs::msg::Twist & velocity)
+  const geometry_msgs::msg::Twist & velocity,
+  nav2_core::GoalChecker * /*goal_checker*/)
 {
   std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> results = nullptr;
   if (pub_->shouldRecordEvaluation()) {
diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md
index 28579749..84a40e8a 100644
--- a/nav2_regulated_pure_pursuit_controller/README.md
+++ b/nav2_regulated_pure_pursuit_controller/README.md
@@ -71,8 +71,6 @@ Mixing the proximity and curvature regulated linear velocities with the time-sca
 | `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | 
 | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | 
 | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | 
-| `goal_dist_tol` | XY tolerance from goal to rotate to the goal heading, if `use_rotate_to_heading` is enabled. This should match or be smaller than the `GoalChecker`'s translational goal tolerance. | 
-
 
 Example fully-described XML with default parameter values:
 
@@ -119,7 +117,6 @@ controller_server:
       use_rotate_to_heading: true
       rotate_to_heading_min_angle: 0.785
       max_angular_accel: 3.2
-      goal_dist_tol: 0.25
       cost_scaling_dist: 0.3
       cost_scaling_gain: 1.0
       inflation_cost_scaling_factor: 3.0
diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
index 9e483e2d..3ab00a32 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
-#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
+#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_
+#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_
 
 #include <string>
 #include <vector>
@@ -84,12 +84,13 @@ public:
    *
    * @param pose      Current robot pose
    * @param velocity  Current robot velocity
-   * @param results   Output param, if not NULL, will be filled in with full evaluation results
+   * @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) override;
+    const geometry_msgs::msg::Twist & velocity,
+    nav2_core::GoalChecker * /*goal_checker*/) override;
 
   /**
    * @brief nav2_core setPlan - Sets the global plan
@@ -164,7 +165,8 @@ protected:
    * @param angle_to_path Angle of robot output relatie to carrot marker
    * @param curr_speed the current robot speed
    */
-  void rotateToHeading(double & linear_vel, double & angular_vel,
+  void rotateToHeading(
+    double & linear_vel, double & angular_vel,
     const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed);
 
   /**
@@ -251,10 +253,11 @@ protected:
 
   nav_msgs::msg::Path global_plan_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
-  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>> carrot_pub_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
+  carrot_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> carrot_arc_pub_;
 };
 
 }  // namespace nav2_regulated_pure_pursuit_controller
 
-#endif  // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
+#endif  // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_
diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
index 2a062a5d..66f7e22f 100644
--- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
@@ -16,6 +16,7 @@
 #include <algorithm>
 #include <string>
 #include <memory>
+#include <utility>
 
 #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp"
 #include "nav2_core/exceptions.hpp"
@@ -73,6 +74,7 @@ void RegulatedPurePursuitController::configure(
 
   double transform_tolerance = 0.1;
   double control_frequency = 20.0;
+  goal_dist_tol_ = 0.25;  // reasonable default before first update
 
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5));
@@ -104,7 +106,8 @@ void RegulatedPurePursuitController::configure(
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true));
   declare_parameter_if_not_declared(
-    node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true));
+    node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling",
+    rclcpp::ParameterValue(true));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6));
   declare_parameter_if_not_declared(
@@ -121,8 +124,6 @@ void RegulatedPurePursuitController::configure(
     node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
-  declare_parameter_if_not_declared(
-    node, plugin_name_ + ".goal_dist_tol", rclcpp::ParameterValue(0.25));
 
   node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_);
   base_desired_linear_vel_ = desired_linear_vel_;
@@ -132,32 +133,51 @@ void RegulatedPurePursuitController::configure(
   node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_);
   node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_);
   node->get_parameter(plugin_name_ + ".lookahead_time", lookahead_time_);
-  node->get_parameter(plugin_name_ + ".rotate_to_heading_angular_vel", rotate_to_heading_angular_vel_);
+  node->get_parameter(
+    plugin_name_ + ".rotate_to_heading_angular_vel",
+    rotate_to_heading_angular_vel_);
   node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance);
-  node->get_parameter(plugin_name_ + ".use_velocity_scaled_lookahead_dist",
+  node->get_parameter(
+    plugin_name_ + ".use_velocity_scaled_lookahead_dist",
     use_velocity_scaled_lookahead_dist_);
-  node->get_parameter(plugin_name_ + ".min_approach_linear_velocity", min_approach_linear_velocity_);
-  node->get_parameter(plugin_name_ + ".use_approach_linear_velocity_scaling", use_approach_vel_scaling_);
-  node->get_parameter(plugin_name_ + ".max_allowed_time_to_collision", max_allowed_time_to_collision_);
-  node->get_parameter(plugin_name_ + ".use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_);
-  node->get_parameter(plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_);
+  node->get_parameter(
+    plugin_name_ + ".min_approach_linear_velocity",
+    min_approach_linear_velocity_);
+  node->get_parameter(
+    plugin_name_ + ".use_approach_linear_velocity_scaling",
+    use_approach_vel_scaling_);
+  node->get_parameter(
+    plugin_name_ + ".max_allowed_time_to_collision",
+    max_allowed_time_to_collision_);
+  node->get_parameter(
+    plugin_name_ + ".use_regulated_linear_velocity_scaling",
+    use_regulated_linear_velocity_scaling_);
+  node->get_parameter(
+    plugin_name_ + ".use_cost_regulated_linear_velocity_scaling",
+    use_cost_regulated_linear_velocity_scaling_);
   node->get_parameter(plugin_name_ + ".cost_scaling_dist", cost_scaling_dist_);
   node->get_parameter(plugin_name_ + ".cost_scaling_gain", cost_scaling_gain_);
-  node->get_parameter(plugin_name_ + ".inflation_cost_scaling_factor", inflation_cost_scaling_factor_);
-  node->get_parameter(plugin_name_ + ".regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_);
-  node->get_parameter(plugin_name_ + ".regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_);
+  node->get_parameter(
+    plugin_name_ + ".inflation_cost_scaling_factor",
+    inflation_cost_scaling_factor_);
+  node->get_parameter(
+    plugin_name_ + ".regulated_linear_scaling_min_radius",
+    regulated_linear_scaling_min_radius_);
+  node->get_parameter(
+    plugin_name_ + ".regulated_linear_scaling_min_speed",
+    regulated_linear_scaling_min_speed_);
   node->get_parameter(plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_);
   node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_);
   node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_);
-  node->get_parameter(plugin_name_ + ".goal_dist_tol", goal_dist_tol_);
   node->get_parameter("controller_frequency", control_frequency);
 
   transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
   control_duration_ = 1.0 / control_frequency;
 
   if (inflation_cost_scaling_factor_ <= 0.0) {
-    RCLCPP_WARN(logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
-    "it should be >0. Disabling cost regulated linear velocity scaling.");
+    RCLCPP_WARN(
+      logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
+      "it should be >0. Disabling cost regulated linear velocity scaling.");
     use_cost_regulated_linear_velocity_scaling_ = false;
   }
 
@@ -228,8 +248,18 @@ double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs:
 
 geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands(
   const geometry_msgs::msg::PoseStamped & pose,
-  const geometry_msgs::msg::Twist & speed)
+  const geometry_msgs::msg::Twist & speed,
+  nav2_core::GoalChecker * goal_checker)
 {
+  // Update for the current goal checker's state
+  geometry_msgs::msg::Pose pose_tolerance;
+  geometry_msgs::msg::Twist vel_tolerance;
+  if (!goal_checker->getTolerances(pose_tolerance, vel_tolerance)) {
+    RCLCPP_WARN(logger_, "Unable to retrieve goal checker's tolerances!");
+  } else {
+    goal_dist_tol_ = pose_tolerance.position.x;
+  }
+
   // Transform path to robot base frame
   auto transformed_plan = transformGlobalPlan(pose);
 
@@ -267,8 +297,8 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
       lookahead_dist, curvature, speed,
       costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel);
 
-      // Apply curvature to angular velocity after constraining linear velocity
-      angular_vel = linear_vel * curvature;
+    // Apply curvature to angular velocity after constraining linear velocity
+    angular_vel = linear_vel * curvature;
   }
 
   // Collision checking on this velocity heading
@@ -366,7 +396,7 @@ bool RegulatedPurePursuitController::isCollisionImminent(
     if (i * projection_time > max_allowed_time_to_collision_) {
       break;
     }
-    
+
     i++;
 
     // apply velocity at curr_pose over distance
@@ -438,7 +468,7 @@ void RegulatedPurePursuitController::applyConstraints(
   {
     const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius();
     const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) *
-    std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius;
+      std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius;
 
     if (min_distance_to_obstacle < cost_scaling_dist_) {
       cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_;
@@ -477,7 +507,9 @@ void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path)
   global_plan_ = path;
 }
 
-void RegulatedPurePursuitController::setSpeedLimit(const double & speed_limit, const bool & percentage)
+void RegulatedPurePursuitController::setSpeedLimit(
+  const double & speed_limit,
+  const bool & percentage)
 {
   if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
     // Restore default value
@@ -502,15 +534,14 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
 
   // let's get the pose of the robot in the frame of the plan
   geometry_msgs::msg::PoseStamped robot_pose;
-  if (!transformPose(global_plan_.header.frame_id, pose, robot_pose))
-  {
+  if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) {
     throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame");
   }
 
   // We'll discard points on the plan that are outside the local costmap
   nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
   const double max_costmap_dim = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY());
-  const double max_transform_dist =  max_costmap_dim * costmap->getResolution() / 2.0;
+  const double max_transform_dist = max_costmap_dim * costmap->getResolution() / 2.0;
 
   // First find the closest pose on the path to the robot
   auto transformation_begin =
@@ -578,7 +609,7 @@ bool RegulatedPurePursuitController::transformPose(
   return false;
 }
 
-}  // namespace nav2_pure_pursuit_controller
+}  // namespace nav2_regulated_pure_pursuit_controller
 
 // Register this controller as a nav2_core plugin
 PLUGINLIB_EXPORT_CLASS(
diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
index 59905c3f..06a626a3 100644
--- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
+++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
@@ -35,33 +35,34 @@ RclCppFixture g_rclcppfixture;
 class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
 {
 public:
-  BasicAPIRPP() : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {};
+  BasicAPIRPP()
+  : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {}
 
-  nav_msgs::msg::Path getPlan() {return global_plan_;};
+  nav_msgs::msg::Path getPlan() {return global_plan_;}
 
-  double getSpeed() {return desired_linear_vel_;};
+  double getSpeed() {return desired_linear_vel_;}
 
   std::unique_ptr<geometry_msgs::msg::PointStamped> createCarrotMsgWrapper(
     const geometry_msgs::msg::PoseStamped & carrot_pose)
   {
     return createCarrotMsg(carrot_pose);
-  };
+  }
 
-  void setVelocityScaledLookAhead() {use_velocity_scaled_lookahead_dist_ = true;};
-  void setCostRegulationScaling() {use_cost_regulated_linear_velocity_scaling_ = true;};
-  void resetVelocityRegulationScaling() {use_regulated_linear_velocity_scaling_ = false;};
-  void resetVelocityApproachScaling() {use_approach_vel_scaling_ = false;};
+  void setVelocityScaledLookAhead() {use_velocity_scaled_lookahead_dist_ = true;}
+  void setCostRegulationScaling() {use_cost_regulated_linear_velocity_scaling_ = true;}
+  void resetVelocityRegulationScaling() {use_regulated_linear_velocity_scaling_ = false;}
+  void resetVelocityApproachScaling() {use_approach_vel_scaling_ = false;}
 
   double getLookAheadDistanceWrapper(const geometry_msgs::msg::Twist & twist)
   {
     return getLookAheadDistance(twist);
-  };
+  }
 
   geometry_msgs::msg::PoseStamped getLookAheadPointWrapper(
     const double & dist, const nav_msgs::msg::Path & path)
   {
     return getLookAheadPoint(dist, path);
-  };
+  }
 
   bool shouldRotateToPathWrapper(
     const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path)
@@ -74,20 +75,22 @@ public:
     return shouldRotateToGoalHeading(carrot_pose);
   }
 
-  void rotateToHeadingWrapper(double & linear_vel, double & angular_vel,
+  void rotateToHeadingWrapper(
+    double & linear_vel, double & angular_vel,
     const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed)
   {
     return rotateToHeading(linear_vel, angular_vel, angle_to_path, curr_speed);
   }
 
   void applyConstraintsWrapper(
-  const double & dist_error, const double & lookahead_dist,
-  const double & curvature, const geometry_msgs::msg::Twist & curr_speed,
-  const double & pose_cost, double & linear_vel)
+    const double & dist_error, const double & lookahead_dist,
+    const double & curvature, const geometry_msgs::msg::Twist & curr_speed,
+    const double & pose_cost, double & linear_vel)
   {
-    return applyConstraints(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
+    return applyConstraints(
+      dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
+      linear_vel);
   }
-
 };
 
 TEST(RegulatedPurePursuitTest, basicAPI)
@@ -289,19 +292,25 @@ TEST(RegulatedPurePursuitTest, applyConstraints)
 
   // test curvature regulation (default)
   curr_speed.linear.x = 0.25;
-  ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
+  ctrl->applyConstraintsWrapper(
+    dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
+    linear_vel);
   EXPECT_EQ(linear_vel, 0.25);  // min set speed
 
   linear_vel = 1.0;
   curvature = 0.7407;
   curr_speed.linear.x = 0.5;
-  ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
+  ctrl->applyConstraintsWrapper(
+    dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
+    linear_vel);
   EXPECT_NEAR(linear_vel, 0.5, 0.01);  // lower by curvature
 
   linear_vel = 1.0;
   curvature = 1000.0;
   curr_speed.linear.x = 0.25;
-  ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
+  ctrl->applyConstraintsWrapper(
+    dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
+    linear_vel);
   EXPECT_NEAR(linear_vel, 0.25, 0.01);  // min out by curvature
 
 
@@ -314,24 +323,28 @@ TEST(RegulatedPurePursuitTest, applyConstraints)
   // pose_cost = 1;
   // linear_vel = 0.5;
   // curr_speed.linear.x = 0.5;
-  // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
+  // ctrl->applyConstraintsWrapper(
+  //   dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
   // EXPECT_NEAR(linear_vel, 0.498, 0.01);
 
   // max changing cost
   // pose_cost = 127;
   // curr_speed.linear.x = 0.255;
-  // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
+  // ctrl->applyConstraintsWrapper(
+  //   dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
   // EXPECT_NEAR(linear_vel, 0.255, 0.01);
 
   // over max cost thresh
   // pose_cost = 200;
   // curr_speed.linear.x = 0.25;
-  // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
+  // ctrl->applyConstraintsWrapper(
+  //   dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
   // EXPECT_NEAR(linear_vel, 0.25, 0.01);
 
   // test kinematic clamping
   // pose_cost = 200;
   // curr_speed.linear.x = 1.0;
-  // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
+  // ctrl->applyConstraintsWrapper(
+  //   dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
   // EXPECT_NEAR(linear_vel, 0.5, 0.01);
 }
-- 
GitLab