Final alignements

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Author: fmrico

controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp

@@ -73,6 +73,10 @@ class MPCController : public ControllerMethodBase
   double max_ang_vel_{1.5};     ///< Maximum angular velocity for MPC.
   bool verbose_{false};         ///< Value to debug on terminal
 
+  // Fallback goal tolerances if GoalManager does not publish them
+  double fallback_goal_pos_tol_{0.05};   ///< Default positional tolerance (meters).
+  double fallback_goal_yaw_tol_{0.05};   ///< Default angular tolerance (radians).
+
   std::unique_ptr<MPCOptimizer> optimizer_;  ///< MPC optimizer
 
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr mpc_path_pub_;            ///< Publisher for MPC path.

controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp

@@ -43,13 +43,19 @@ MPCController::on_initialize()
   node->declare_parameter<double>(plugin_name + ".max_angular_velocity", max_ang_vel_);
   node->declare_parameter<bool>(plugin_name + ".verbose", verbose_);
 
+  node->declare_parameter<double>(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_);
+  node->declare_parameter<double>(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_);
+
   node->get_parameter<int>(plugin_name + ".horizon_steps", horizon_steps_);
   node->get_parameter<double>(plugin_name + ".dt", dt_);
   node->get_parameter<double>(plugin_name + ".safety_radius", safety_radius_);
   node->get_parameter<double>(plugin_name + ".max_linear_velocity", max_lin_vel_);
   node->get_parameter<double>(plugin_name + ".max_angular_velocity", max_ang_vel_);
   node->get_parameter<bool>(plugin_name + ".verbose", verbose_);
 
+  node->get_parameter<double>(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_);
+  node->get_parameter<double>(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_);
+
   optimizer_ = std::make_unique<MPCOptimizer>();
 
   mpc_path_pub_ =
@@ -267,6 +273,62 @@ MPCController::update_rt(NavState & nav_state)
 
   collision_checker(&params, u);
 
+  // Final alignment phase with hysteresis on distance:
+  // - Enter when dist_to_goal <= 0.5 * pos_tol
+  // - Stay in this phase (even if dist grows slightly) until dist_to_goal > pos_tol
+  {
+    const auto & goal_pose = path.poses.back().pose;
+
+    double pos_tol = fallback_goal_pos_tol_;
+    double yaw_tol = fallback_goal_yaw_tol_;
+
+    if (nav_state.has("goal_tolerance.position")) {
+      pos_tol = nav_state.get<double>("goal_tolerance.position");
+    }
+    if (nav_state.has("goal_tolerance.yaw")) {
+      yaw_tol = nav_state.get<double>("goal_tolerance.yaw");
+    }
+
+    const double dx_g = goal_pose.position.x - pose.position.x;
+    const double dy_g = goal_pose.position.y - pose.position.y;
+    const double dist_to_goal = std::hypot(dx_g, dy_g);
+
+    const double yaw_goal = std::atan2(
+      2.0 * (goal_pose.orientation.w * goal_pose.orientation.z +
+      goal_pose.orientation.x * goal_pose.orientation.y),
+      1.0 - 2.0 * (goal_pose.orientation.y * goal_pose.orientation.y +
+      goal_pose.orientation.z * goal_pose.orientation.z));
+    double e_theta_goal = std::atan2(std::sin(yaw_ - yaw_goal), std::cos(yaw_ - yaw_goal));
+
+    const double enter_dist = 0.5 * pos_tol;
+
+    // Hysteresis based on distance to goal: start aligning when we are
+    // well inside the goal radius (enter_dist) and keep aligning until
+    // we move clearly outside (dist_to_goal > pos_tol).
+    const bool inside_hysteresis_band = (dist_to_goal <= pos_tol);
+    const bool should_enter_alignment = (dist_to_goal <= enter_dist);
+
+    if ((inside_hysteresis_band && std::fabs(e_theta_goal) > yaw_tol) ||
+      should_enter_alignment)
+    {
+      // Stay in place and rotate towards the goal orientation using a simple P controller
+      const double k_align = 1.0;
+      double vlin = 0.0;
+      double vrot = -k_align * e_theta_goal;
+
+      vrot = std::clamp(vrot, -max_ang_vel_, max_ang_vel_);
+
+      cmd_vel_.header.frame_id = path.header.frame_id;
+      cmd_vel_.header.stamp = get_node()->now();
+      cmd_vel_.twist.linear.x = vlin;
+      cmd_vel_.twist.angular.z = vrot;
+
+      nav_state.set("cmd_vel", cmd_vel_);
+      publish_mpc_path(&params, u, path);
+      return;
+    }
+  }
+
   // Publish the computed velocity command
   cmd_vel_.header.frame_id = path.header.frame_id;
   cmd_vel_.header.stamp = get_node()->now();

controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp

@@ -23,6 +23,7 @@
 #include <algorithm>
 #include <limits>
 #include <cmath>
+#include <numbers>
 
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@@ -401,17 +402,16 @@ SerestController::compute_goal_zone(
   double & stop_r, double & slow_r, double & gamma_slow,
   Vec2 & goal_xy, double & yaw_goal) const
 {
-  const double PI = 3.14159265358979323846;
-  const double goal_yaw_tol = goal_yaw_tol_deg_ * PI / 180.0;  // (no se devuelve, pero útil si quieres)
-  (void)goal_yaw_tol;
-
   const auto & pgoal = path.poses.back().pose.position;
   yaw_goal = tf2::getYaw(path.poses.back().pose.orientation);
   goal_xy = Vec2{pgoal.x, pgoal.y};
 
   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_);
   slow_r = std::max(slow_radius_, stop_r + 0.05);
 
@@ -482,8 +482,7 @@ SerestController::should_turn_in_place(
 {
   // Mantenemos compatibilidad con la firma, pero ignoramos turn_in_place_thr
   // y usamos dos umbrales internos sin exponer parámetros.
-  const double PI = 3.14159265358979323846;
-  const double thr_enter = 60.0 * PI / 180.0; // entra a girar si |e_theta| > 60°
+  const double thr_enter = 60.0 * std::numbers::pi / 180.0; // entra a girar si |e_theta| > 60°
   // const double thr_exit = 35.0 * PI / 180.0; // sale de girar si |e_theta| < 35°
 
   // No permitimos “atajo” marcha atrás en esta decisión: si no permites reverse,
@@ -506,8 +505,7 @@ SerestController::maybe_final_align_and_publish(
   double dist_xy_goal, double stop_r, double e_theta_goal,
   double gamma_slow, double dt)
 {
-  const double PI = 3.14159265358979323846;
-  const double goal_yaw_tol = goal_yaw_tol_deg_ * PI / 180.0;
+  const double goal_yaw_tol = goal_yaw_tol_deg_ * std::numbers::pi / 180.0;
 
   if (dist_xy_goal > stop_r) {
     return false;
@@ -620,6 +618,16 @@ SerestController::update_rt(NavState & nav_state)
   nav_msgs::msg::Odometry odom;
   if (!fetch_required_inputs(nav_state, path, odom)) {return;}
 
+  // 1.5) Goal tolerances: prefer shared GoalManager values, fallback to local params
+  double goal_pos_tol = goal_pos_tol_;
+  double goal_yaw_tol = goal_yaw_tol_deg_ * (std::numbers::pi / 180.0);
+  if (nav_state.has("goal_tolerance.position")) {
+    goal_pos_tol = nav_state.get<double>("goal_tolerance.position");
+  }
+  if (nav_state.has("goal_tolerance.yaw")) {
+    goal_yaw_tol = nav_state.get<double>("goal_tolerance.yaw");
+  }
+
   // 2) Robot state (position + yaw)
   Vec2 robot_xy; double yaw = 0.0;
   robot_state_from_odom(odom, robot_xy, yaw);
@@ -714,8 +722,7 @@ SerestController::update_rt(NavState & nav_state)
   double v_prog_ref = v_prog_ref_free * gamma_slow;
 
   // Maintain a small cruising speed when roughly aligned and outside the stop zone (no reverse)
-  const double PI = 3.14159265358979323846;
-  const double align_thr = 30.0 * PI / 180.0;
+  const double align_thr = 30.0 * std::numbers::pi / 180.0;
   if (!allow_reverse_ && (dist_xy_goal > stop_r) && std::fabs(e_theta) < align_thr) {
     v_prog_ref = std::max(v_prog_ref, std::min(slow_min_speed_, v_prog_ref_free));
   }
@@ -766,7 +773,7 @@ SerestController::update_rt(NavState & nav_state)
 
   // Optional classic TiP safeguard using the same angular threshold
   {
-    const double turn_in_place_thr = (60.0 * PI / 180.0);
+    const double turn_in_place_thr = (60.0 * std::numbers::pi / 180.0);
     const double s_total = pd.s_acc.back();
     const double dist_to_end = s_total - prj.s_star;