feat(control): port temporal trajectory follower and follow-ups from X2

control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/lowpass_filter.hpp

@@ -74,6 +74,12 @@ class Butterworth2dFilter
    */
   double filter(const double & u);
 
+  /**
+   * @brief Force filter internal state (y,u history) to a value without changing coefficients.
+   * Used so the steering LPF tracks the published command after post-MPC soft hold / stop freeze.
+   */
+  void resetState(const double value);
+
   /**
    * @brief filtering for time-series data
    * @param [in] t time-series data for input vector

control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp

@@ -31,6 +31,7 @@
 #include "nav_msgs/msg/odometry.hpp"
 
 #include <deque>
+#include <limits>
 #include <memory>
 #include <string>
 #include <utility>
@@ -156,6 +157,14 @@ struct MPCData
   // Pose (position and orientation) of the nearest point in the trajectory.
   Pose nearest_pose{};
 
+  // Temporal tracking debug values.
+  double temporal_predicted_time{std::numeric_limits<double>::quiet_NaN()};
+  double temporal_observed_time{std::numeric_limits<double>::quiet_NaN()};
+  double temporal_fused_time{std::numeric_limits<double>::quiet_NaN()};
+  double temporal_window_min{std::numeric_limits<double>::quiet_NaN()};
+  double temporal_window_max{std::numeric_limits<double>::quiet_NaN()};
+  bool temporal_observation_used{false};
+
   // Current steering angle.
   double steer{};
 
@@ -226,6 +235,8 @@ class MPC
   double m_yaw_error_prev = 0.0;       // Previous heading error for derivative calculation.
 
   bool m_is_forward_shift = true;  // Flag indicating if the shift is in the forward direction.
+  bool m_reference_trajectory_has_steering =
+    false;  // True when the complete received trajectory contains a nonzero steering state.
 
   rclcpp::Publisher<Trajectory>::SharedPtr m_debug_frenet_predicted_trajectory_pub;
   rclcpp::Publisher<Trajectory>::SharedPtr m_debug_resampled_reference_trajectory_pub;
@@ -437,6 +448,12 @@ class MPC
   bool m_use_delayed_initial_state =
     true;  // Flag to use x0_delayed as initial state for predicted trajectory
 
+  bool m_use_temporal_trajectory =
+    false;  // true: use timestamps; false: spatial distance/velocity time
+
+  bool m_use_trajectory_steering_for_feedforward =
+    false;  // Prefer temporal trajectory steering when the received field is populated.
+
   bool m_publish_debug_trajectories = false;  // Flag to publish predicted trajectory and
                                               // resampled reference trajectory for debug purpose
 
@@ -472,6 +489,12 @@ class MPC
    */
   void resetPrevResult(const SteeringReport & current_steer);
 
+  /**
+   * @brief Reset steering-command LPF internal state to a published command value.
+   * Keeps the filter tracking post-MPC output (soft hold / stop freeze) instead of raw Uex.
+   */
+  void resetSteeringCmdFilter(const double steering_tire_angle);
+
   /**
    * @brief Set the vehicle model for this MPC.
    * @param vehicle_model_ptr Pointer to the vehicle model.

control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -36,6 +36,7 @@
 
 #include <deque>
 #include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -102,6 +103,23 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   // Flag indicating whether to keep the steering control until it converges.
   bool m_keep_steer_control_until_converged;
 
+  // Shared duration [s]:
+  // - stop: eligibility must last longer than this before isStoppedState() is true
+  // - confidence: low-confidence soft hold expires after this without a confident refresh
+  double m_stop_state_steer_hold_duration{2.0};
+
+  std::optional<rclcpp::Time> m_stop_state_hold_started_at;
+
+  // Reference-confidence steer soft hold (outside MPC QP).
+  bool m_enable_confidence_steer_slew_limit{false};
+  double m_reference_confidence_L_ahead_min{0.4};
+  double m_reference_confidence_L_ahead_ref{2.0};
+  double m_steer_slew_rate_min_rad_s{0.02};
+  double m_ctrl_period{0.0};
+
+  // Started on first low-confidence cycle; cleared only when confidence returns.
+  std::optional<rclcpp::Time> m_confidence_hold_started_at;
+
   // MPC solver checker.
   ResultWithReason m_mpc_solved_status{true};
 
@@ -247,8 +265,9 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   [[nodiscard]] Lateral getInitialControlCommand() const;
 
   /**
-   * @brief Check if the ego car is in a stopped state.
-   * @return True if the ego car is stopped, false otherwise.
+   * @brief Check if the ego car is in a confirmed full stop.
+   * True only after stop eligibility has lasted longer than stop_state_steer_hold_duration.
+   * Until then the controller stays in control.
    */
   [[nodiscard]] bool isStoppedState() const;
 
@@ -272,6 +291,56 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
    */
   [[nodiscard]] bool isSteerConverged(const Lateral & cmd) const;
 
+  /**
+   * @brief Minimum target velocity ahead of ego over the stop-state distance margin.
+   */
+  [[nodiscard]] double getMinTargetVelocityAhead() const;
+
+  /**
+   * @brief True when ego and trajectory target speed indicate a full stop (stop-state kinematics).
+   */
+  [[nodiscard]] bool isEgoAndTrajectoryStopped() const;
+
+  /**
+   * @brief True when stop-state kinematics are met and steer convergence gate allows hold.
+   */
+  [[nodiscard]] bool isStopStateSteerHoldEligible() const;
+
+  /**
+   * @brief Update the stop-state confirmation timer from current kinematics.
+   */
+  void updateStopStateHoldTimer();
+
+  /**
+   * @brief Get spatial arc length of the reference trajectory remaining ahead of ego [m].
+   */
+  [[nodiscard]] double getReferenceRemainingArcLength() const;
+  /**
+   * @brief Compute reference confidence weight in [0, 1] from remaining spatial extent ahead of
+   * ego.
+   */
+  [[nodiscard]] double computeReferenceConfidenceWeight() const;
+
+  /**
+   * @brief True when remaining spatial extent is at/above the full-confidence threshold.
+   */
+  [[nodiscard]] bool isReferenceFullyConfident() const;
+
+  /**
+   * @brief Soft-limit steering toward MPC while reference confidence is low.
+   * ds_max blends from min_rate*period (conf=0) to |MPC delta| (conf=1).
+   * Timer starts on first low-confidence cycle and refreshes only when confidence returns;
+   * after stop_state_steer_hold_duration without refresh, soft hold expires and MPC is used.
+   * Flickering short trajectories are treated as low confidence, not as stop.
+   * @return true if the command was modified by the slew limiter
+   */
+  bool applyConfidenceSteerSlewLimit(Lateral & ctrl_cmd);
+
+  /**
+   * @brief Sync delay buffer and steering LPF to the published steer command.
+   */
+  void syncMpcSteerStateToCommand(const float steering_tire_angle);
+
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
 
   /**

control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_trajectory.hpp

@@ -16,6 +16,7 @@
 #define AUTOWARE__MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_
 
 #include "autoware_utils/geometry/geometry.hpp"
+#include "builtin_interfaces/msg/time.hpp"
 
 #include "geometry_msgs/msg/point.hpp"
 
@@ -40,6 +41,7 @@ class MPCTrajectoryPoint
   double vx;
   double k;
   double smooth_k;
+  double steer;
   double relative_time;
 };
 
@@ -48,13 +50,16 @@ class MPCTrajectoryPoint
 class MPCTrajectory
 {
 public:
+  //!< @brief absolute time origin of the source trajectory
+  builtin_interfaces::msg::Time stamp{};
   std::vector<double> x;              //!< @brief x position x vector
   std::vector<double> y;              //!< @brief y position y vector
   std::vector<double> z;              //!< @brief z position z vector
   std::vector<double> yaw;            //!< @brief yaw pose yaw vector
   std::vector<double> vx;             //!< @brief vx velocity vx vector
   std::vector<double> k;              //!< @brief k curvature k vector
   std::vector<double> smooth_k;       //!< @brief k smoothed-curvature k vector
+  std::vector<double> steer;          //!< @brief trajectory steering state vector
   std::vector<double> relative_time;  //!< @brief relative_time duration time from start point
 
   /**
@@ -64,6 +69,14 @@ class MPCTrajectory
     const double & xp, const double & yp, const double & zp, const double & yawp,
     const double & vxp, const double & kp, const double & smooth_kp, const double & tp);
 
+  /**
+   * @brief push_back for all values, including the trajectory steering state
+   */
+  void push_back(
+    const double & xp, const double & yp, const double & zp, const double & yawp,
+    const double & vxp, const double & kp, const double & smooth_kp, const double & steerp,
+    const double & tp);
+
   /**
    * @brief push_back for all values
    */
@@ -119,6 +132,7 @@ class MPCTrajectory
       point.pose.position.z = z.at(i);
       point.pose.orientation = autoware_utils::create_quaternion_from_yaw(yaw.at(i));
       point.longitudinal_velocity_mps = vx.at(i);
+      point.front_wheel_angle_rad = steer.at(i);
       points.push_back(point);
     }
     return points;

control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_utils.hpp

@@ -28,6 +28,7 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 #include <cmath>
+#include <limits>
 #include <string>
 #include <utility>
 #include <vector>
@@ -68,9 +69,12 @@ double calcLateralError(const Pose & ego_pose, const Pose & ref_pose);
 /**
  * @brief convert the given Trajectory msg to a MPCTrajectory object
  * @param [in] input trajectory to convert
+ * @param [in] use_temporal_trajectory if true, use time_from_start directly; if false, calculate
+ * from distance/velocity
  * @return resulting MPCTrajectory
  */
-MPCTrajectory convertToMPCTrajectory(const Trajectory & input);
+MPCTrajectory convertToMPCTrajectory(
+  const Trajectory & input, const bool use_temporal_trajectory = false);
 
 /**
  * @brief convert the given MPCTrajectory to a Trajectory msg
@@ -94,6 +98,15 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector<do
  */
 double calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory);
 
+/**
+ * @brief calculate spatial arc length from a start index to the trajectory end
+ * @param [in] trajectory trajectory to measure
+ * @param [in] start_idx index from which to start accumulating distance
+ * @return accumulated 2D distance from start_idx to the last point
+ */
+double calcMPCTrajectoryRemainingArcLength(
+  const MPCTrajectory & trajectory, const size_t start_idx);
+
 /**
  * @brief resample the given trajectory with the given fixed interval
  * @param [in] input trajectory to resample
@@ -130,17 +143,22 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj);
  * @param [in] acc_lim limit on the acceleration
  * @param [in] tau constant to control the smoothing (high-value = very smooth)
  * @param [inout] traj MPCTrajectory for which to calculate the smoothed velocity
+ * @param [in] use_temporal_trajectory if true, preserve timestamps; if false, recalculate time from
+ * velocity
  */
 void dynamicSmoothingVelocity(
   const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau,
-  MPCTrajectory & traj);
+  MPCTrajectory & traj, const bool use_temporal_trajectory = false);
 
 /**
  * @brief calculate yaw angle in MPCTrajectory from xy vector
  * @param [inout] traj object trajectory
  * @param [in] shift is forward or not
+ * @param [in] use_input_yaw_for_short_segment if true, preserve input yaw for very short segments
  */
-void calcTrajectoryYawFromXY(MPCTrajectory & traj, const bool is_forward_shift);
+void calcTrajectoryYawFromXY(
+  MPCTrajectory & traj, const bool is_forward_shift,
+  const bool use_input_yaw_for_short_segment = false);
 
 /**
  * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3
@@ -152,7 +170,7 @@ void calcTrajectoryYawFromXY(MPCTrajectory & traj, const bool is_forward_shift);
  */
 void calcTrajectoryCurvature(
   const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer,
-  MPCTrajectory & traj);
+  MPCTrajectory & traj, const bool use_short_segment_protection = false);
 
 /**
  * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3
@@ -162,7 +180,23 @@ void calcTrajectoryCurvature(
  * @return vector of curvatures at each point of the given trajectory
  */
 std::vector<double> calcTrajectoryCurvature(
-  const int curvature_smoothing_num, const MPCTrajectory & traj);
+  const int curvature_smoothing_num, const MPCTrajectory & traj,
+  const bool use_short_segment_protection = false);
+
+/**
+ * @brief Calculate curvature for temporal trajectories by spatially resampling.
+ * Temporal trajectories have non-uniform spatial point distribution, which makes index-based
+ * curvature calculation unreliable. This function resamples the trajectory at equal spatial
+ * intervals, calculates curvature on the resampled points, then maps the result back to the
+ * original temporal trajectory points via arc-length interpolation.
+ * @param [in] curvature_smoothing_num_traj index distance for 3 points for curvature calculation
+ * @param [in] curvature_smoothing_num_ref_steer index distance for 3 points for smoothed curvature
+ * @param [in] resample_interval_dist spatial resampling interval [m]
+ * @param [inout] traj temporal trajectory whose k and smooth_k will be updated
+ */
+void calcTrajectoryCurvatureBySpatialResample(
+  const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer,
+  const double resample_interval_dist, MPCTrajectory & traj);
 
 /**
  * @brief calculate nearest pose on MPCTrajectory with linear interpolation