Fixed goal tolerance, now it properly gets tolerance from navstate

Author: midemig

controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp

@@ -1,17 +1,21 @@
 // Copyright 2025 Intelligent Robotics Lab
 //
 // This file is part of the project Easy Navigation (EasyNav in short)
-// 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
+// licensed under the GNU General Public License v3.0.
+// See <http://www.gnu.org/licenses/> for details.
 //
-//     http://www.apache.org/licenses/LICENSE-2.0
+// Easy Navigation program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
 //
-// 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.
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 
 #ifndef EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_
 #define EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_
@@ -216,7 +220,8 @@ class SerestController : public ControllerMethodBase
     const Vec2 & robot_xy, double robot_yaw,
     double & dist_xy_goal, double & e_theta_goal,
     double & stop_r, double & slow_r, double & gamma_slow,
-    Vec2 & goal_xy, double & yaw_goal) const;
+    Vec2 & goal_xy, double & yaw_goal,
+    double goal_pos_tol) const;
 
   // Calcula límites de seguridad globales (distancia obstáculo, v_safe, v_curv).
   void safety_limits(
@@ -238,7 +243,7 @@ class SerestController : public ControllerMethodBase
   bool maybe_final_align_and_publish(
     NavState & nav_state, const nav_msgs::msg::Path & path,
     double dist_xy_goal, double stop_r, double e_theta_goal,
-    double gamma_slow, double dt);
+    double gamma_slow, double dt, double goal_yaw_tol);
 
   // Aplica rate‑limit y saturaciones finales sobre (vlin, vrot) y actualiza last_*.
   void rate_limit_and_saturate(double dt, double & vlin, double & vrot);

controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp

@@ -1,17 +1,21 @@
 // Copyright 2025 Intelligent Robotics Lab
 //
 // This file is part of the project Easy Navigation (EasyNav in short)
-// 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
+// licensed under the GNU General Public License v3.0.
+// See <http://www.gnu.org/licenses/> for details.
 //
-//     http://www.apache.org/licenses/LICENSE-2.0
+// Easy Navigation program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
 //
-// 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.
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 
 /// \file
 /// \brief Implementation of the SimpleController class.
@@ -415,7 +419,8 @@ SerestController::compute_goal_zone(
   const Vec2 & robot_xy, double robot_yaw,
   double & dist_xy_goal, double & e_theta_goal,
   double & stop_r, double & slow_r, double & gamma_slow,
-  Vec2 & goal_xy, double & yaw_goal) const
+  Vec2 & goal_xy, double & yaw_goal,
+  double goal_pos_tol) const
 {
   const auto & pgoal = path.poses.back().pose.position;
   yaw_goal = tf2::getYaw(path.poses.back().pose.orientation);
@@ -424,10 +429,8 @@ SerestController::compute_goal_zone(
   dist_xy_goal = std::hypot(robot_xy.x - goal_xy.x, robot_xy.y - goal_xy.y);
   e_theta_goal = std::atan2(std::sin(robot_yaw - yaw_goal), std::cos(robot_yaw - yaw_goal));
 
-  // Use externally provided goal position tolerance when available (via update_rt),
-  // which stores it in goal_pos_tol_. Here we only shape the radial zone using
-  // the current value of goal_pos_tol_ as stop radius.
-  stop_r = std::max(0.0, goal_pos_tol_);
+  // Use the resolved goal position tolerance (from NavState if available, else parameter).
+  stop_r = std::max(0.0, goal_pos_tol);
   slow_r = std::max(slow_radius_, stop_r + 0.05);
 
   gamma_slow = 1.0;
@@ -518,9 +521,8 @@ bool
 SerestController::maybe_final_align_and_publish(
   NavState & nav_state, const nav_msgs::msg::Path & path,
   double dist_xy_goal, double stop_r, double e_theta_goal,
-  double gamma_slow, double dt)
+  double gamma_slow, double dt, double goal_yaw_tol)
 {
-  const double goal_yaw_tol = goal_yaw_tol_deg_ * M_PI / 180.0;
 
   if (dist_xy_goal > stop_r) {
     return false;
@@ -660,7 +662,7 @@ SerestController::update_rt(NavState & nav_state)
   double dist_xy_goal = 0.0, e_theta_goal = 0.0, stop_r = 0.0, slow_r = 0.0, gamma_slow = 1.0;
   Vec2 goal_xy; double yaw_goal = 0.0;
   compute_goal_zone(path, robot_xy, yaw, dist_xy_goal, e_theta_goal,
-                    stop_r, slow_r, gamma_slow, goal_xy, yaw_goal);
+                    stop_r, slow_r, gamma_slow, goal_xy, yaw_goal, goal_pos_tol);
 
   // 6) Global safety limits derived from sensors and curvature
   double d_closest = 0.0, v_safe = 0.0, v_curv = 0.0;
@@ -763,7 +765,7 @@ SerestController::update_rt(NavState & nav_state)
 
   // 8) Final alignment inside stop zone (publishes and returns if active)
   if (maybe_final_align_and_publish(
-        nav_state, path, dist_xy_goal, stop_r, e_theta_goal, gamma_slow, dt))
+        nav_state, path, dist_xy_goal, stop_r, e_theta_goal, gamma_slow, dt, goal_yaw_tol))
   {
     return;
   }