SerestController.hpp

Go to the documentation of this file.

// 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
//
//     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 EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_
#define EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_
 
#include <vector>
#include <string>
 
#include "rclcpp/time.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
 
#include "easynav_core/ControllerMethodBase.hpp"
#include "easynav_common/types/NavState.hpp"
 
namespace easynav
{
 

class SerestController : public ControllerMethodBase
{
public:
  SerestController();
  ~SerestController() override;

  void on_initialize() override;

  void update_rt(NavState & nav_state) override;

  struct Vec2
  {
    double x; 
    double y; 

    inline Vec2 operator+(const Vec2 & b) const {return {x + b.x, y + b.y};}
    inline Vec2 operator-(const Vec2 & b) const {return {x - b.x, y - b.y};}
    inline Vec2 operator*(double s) const {return {x * s, y * s};}

    friend inline Vec2 operator*(double s, const Vec2 & a) {return {a.x * s, a.y * s};}
  };
 
private:
  struct PathData
  {
    std::vector<Vec2> pts;      
    std::vector<double> s_acc;  
  };

  struct Projection
  {
    size_t seg_idx{0};   
    double s_star{0.0};  
    Vec2   closest{0.0, 0.0}; 
    double t{0.0};       
  };

  struct RefKinematics
  {
    double psi_ref{0.0};     
    double kappa_hat{0.0};   
  };

  PathData build_path_data(const nav_msgs::msg::Path & path) const;

  Projection project_on_path(const PathData & pd, const Vec2 & p) const;

  RefKinematics ref_heading_and_curvature(
    const PathData & pd, const Projection & prj, double v_prev) const;

  void frenet_errors(
    const Vec2 & robot_xy, double robot_yaw,
    const Projection & prj, double psi_ref,
    double & e_y, double & e_theta) const;

  double closest_obstacle_distance(
    const NavState & nav_state);

  double v_safe_from_distance(double d, double slope_sin = 0.0) const;

  double v_curvature_limit(double kappa_hat) const;
 
  // Gestiona dt y actualiza last_update_ts_. Devuelve dt robusto.
  double compute_dt_and_update_clock();
 
  // Comprueba entradas mínimas. Publica stop si faltan y devuelve false.
  // Si todo ok, rellena 'path' y 'odom' y devuelve true.
  bool fetch_required_inputs(
    NavState & nav_state,
    nav_msgs::msg::Path & path,
    nav_msgs::msg::Odometry & odom);
 
  // Extrae estado del robot (xy, yaw) desde la odometría.
  void robot_state_from_odom(
    const nav_msgs::msg::Odometry & odom,
    Vec2 & robot_xy, double & yaw) const;
 
  // Calcula información de goal y zonas slow/stop.
  void compute_goal_zone(
    const nav_msgs::msg::Path & path,
    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;
 
  // Calcula límites de seguridad globales (distancia obstáculo, v_safe, v_curv).
  void safety_limits(
    const NavState & nav_state,
    const RefKinematics & rk,
    double & d_closest, double & v_safe, double & v_curv);
 
  // Aplica corner‑guard: ajusta v_prog_ref y obtiene omega_boost + término ápice.
  void apply_corner_guard(
    const RefKinematics & rk, double e_y, double e_theta,
    double & v_prog_ref, double & omega_boost, double & ey_apex_term) const;
 
  // Determina si debe hacerse giro en el sitio en esta iteración.
  bool should_turn_in_place(
    bool allow_reverse, double e_theta, double e_theta_goal,
    double dist_to_end, double turn_in_place_thr) const;
 
  // Fase de alineación final: publica y devuelve true si se ha gestionado aquí.
  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);
 
  // Aplica rate‑limit y saturaciones finales sobre (vlin, vrot) y actualiza last_*.
  void rate_limit_and_saturate(double dt, double & vlin, double & vrot);
 
  // Publica cmd_vel y variables de depuración comunes del final de update_rt.
  void publish_cmd_and_debug(
    NavState & nav_state, const nav_msgs::msg::Path & path,
    double vlin, double vrot,
    double e_y, double e_theta, double kappa_hat,
    double d_closest, double v_safe, double v_curv, double alpha,
    bool allow_reverse, double dist_to_end,
    double dist_xy_goal, double gamma_slow,
    int in_final_align, int arrived);
 
  // Publica parada inmediata con frame y stamp actuales.
  void publish_stop(NavState & nav_state, const std::string & frame_id);
 
  // --- Parameters ---

  bool   allow_reverse_{false};
  double v_progress_min_{0.05};
  double k_s_share_max_{0.5};

  double max_linear_speed_{0.6};
  double max_angular_speed_{1.5};
  double max_linear_acc_{0.8};
  double max_angular_acc_{2.0};

  double k_s_{0.8}, k_theta_{2.0}, k_y_{1.2}, ell_{0.3};
  double v_ref_{0.6};
  double eps_{1e-3};

  double a_acc_{0.8};
  double a_brake_{1.2};
  double a_lat_max_{1.5};
  double d0_margin_{0.30};
  double tau_latency_{0.10};
  double d_hard_{0.20};
  double t_emerg_{0.25};

  double blend_base_{0.6};
  double blend_k_per_v_{0.6};
  double kappa_max_{2.5};

  double dist_search_radius_{2.0};

  double goal_pos_tol_{0.05};
  double goal_yaw_tol_deg_{5.0};
  double slow_radius_{0.60};
  double slow_min_speed_{0.03};

  double final_align_k_{2.0};
  double final_align_wmax_{0.6};

  bool  corner_guard_enable_{true};
  double corner_gain_ey_{1.5};
  double corner_gain_eth_{0.7};
  double corner_gain_kappa_{0.4};
  double corner_min_alpha_{0.35};
  double corner_boost_omega_{0.8};
  double apex_ey_des_{0.05};
  double a_lat_soft_{1.1};
 
  // --- Internal state ---
 
  bool startup_anchor_active_{true};
  geometry_msgs::msg::Pose goal_pose_last_{};
  // Activa/desactiva giro en el sitio con histéresis
  bool tip_active_{false};

  double last_vlin_{0.0};
  double last_vrot_{0.0};
  rclcpp::Time last_update_ts_;
  rclcpp::Time last_input_ts_;
  geometry_msgs::msg::TwistStamped twist_stamped_;
};

inline SerestController::Vec2 v2(double x, double y) {return {x, y};}

inline double dot(const SerestController::Vec2 & a, const SerestController::Vec2 & b)
{
  return a.x * b.x + a.y * b.y;
}

inline double crossz(const SerestController::Vec2 & a, const SerestController::Vec2 & b)
{
  return a.x * b.y - a.y * b.x;
}

inline double norm(const SerestController::Vec2 & a) {return std::hypot(a.x, a.y);}

inline SerestController::Vec2 normalize(const SerestController::Vec2 & a)
{
  double n = norm(a);
  return n > 1e-9 ? SerestController::Vec2{a.x / n, a.y / n} : SerestController::Vec2{1.0, 0.0};
}
 
}  // namespace easynav
 
#endif  // EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_