fix from TakaHoribe review

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

control/trajectory_follower/design/mpc_lateral_controller-design.md

@@ -93,6 +93,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 | path_smoothing_times                         | int    | number of times of applying path smoothing filter                                                                                                 | 1             |
 | curvature_smoothing_num_ref_steer            | double | index distance of points used in curvature calculation for reference steer command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 35            |
 | curvature_smoothing_num_traj                 | double | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values.              | 1             |
+| extend_trajectory_for_end_yaw_control        | bool   | trajectory extending flag for end yaw control.                                                                                                    | true          |
 | steering_lpf_cutoff_hz                       | double | cutoff frequency of lowpass filter for steering output command [hz]                                                                               | 3.0           |
 | admissible_position_error                    | double | stop vehicle when following position error is larger than this value [m].                                                                         | 5.0           |
 | admissible_yaw_error_rad                     | double | stop vehicle when following yaw angle error is larger than this value [rad].                                                                      | 1.57          |

control/trajectory_follower/include/trajectory_follower/mpc.hpp

@@ -400,7 +400,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
     const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
     const double traj_resample_dist, const bool enable_path_smoothing,
     const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj,
-    const int curvature_smoothing_num_ref_steer, const bool enable_traj_extending);
+    const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control);
   /**
    * @brief set the vehicle model of this MPC
    */

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -98,7 +98,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   double m_traj_resample_dist;
 
   //!< @brief flag of traj extending for terminal yaw
-  bool m_enable_traj_extending;
+  bool m_extend_trajectory_for_end_yaw_control;
 
   /* parameters for stop state */
   double m_stop_state_entry_ego_speed;

control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp

@@ -179,6 +179,21 @@ TRAJECTORY_FOLLOWER_PUBLIC bool calcNearestPoseInterp(
 //  */
 TRAJECTORY_FOLLOWER_PUBLIC double calcStopDistance(
   const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int origin);
+
+/**
+ * @brief extend terminal points
+ * Note: The current MPC does not properly take into account the attitude angle at the end of the
+ * path. By extending the end of the path in the attitude direction, the MPC can consider the
+ * attitude angle well, resulting in improved control performance. If the trajecoty is
+ * well-defined considring the end point attitude angle, this feature is not necessary.
+ * @param [in] terminal yaw
+ * @param [in] extend interval
+ * @param [in] flag of forward shift
+ * @param [out] extended trajectory
+ */
+void extendTrajectoryInYawDirection(
+  const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj);
+
 }  // namespace MPCUtils
 }  // namespace trajectory_follower
 }  // namespace control

control/trajectory_follower/src/mpc.cpp

@@ -180,7 +180,7 @@ void MPC::setReferenceTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
   const double traj_resample_dist, const bool enable_path_smoothing,
   const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj,
-  const int curvature_smoothing_num_ref_steer, const bool enable_traj_extending)
+  const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control)
 {
   trajectory_follower::MPCTrajectory mpc_traj_raw;        // received raw trajectory
   trajectory_follower::MPCTrajectory mpc_traj_resampled;  // resampled trajectory
@@ -217,35 +217,15 @@ void MPC::setReferenceTrajectory(
     }
   }
 
-  /* extend terminal points.
-   * the original trajectory cannot take into account the pose from the position alone
-   * due to smoothing, etc.
+  /* extend terminal points
+   * Note: The current MPC does not properly take into account the attitude angle at the end of the
+   * path. By extending the end of the path in the attitude direction, the MPC can consider the
+   * attitude angle well, resulting in improved control performance. If the trajecoty is
+   * well-defined considring the end point attitude angle, this feature is not necessary.
    */
-  if (enable_traj_extending) {
-    // set original raw termianl yaw
-    auto & traj = mpc_traj_smoothed;
-    traj.yaw.back() = mpc_traj_raw.yaw.back();
-
-    // get terminal pose
-    autoware_auto_planning_msgs::msg::Trajectory autoware_traj;
-    autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory(
-      traj, autoware_traj);
-    auto extended_pose = autoware_traj.points.back().pose;
-
-    constexpr double extend_dist = 10.0;
-    constexpr double extend_vel = 10.0;
-    const double extend_interval = traj_resample_dist;
-    const size_t num_extended_point = static_cast<size_t>(extend_dist / extend_interval);
-    for (size_t i = 0; i < num_extended_point; ++i) {
-      const double x_offset = m_is_forward_shift ? extend_interval : -extend_interval;
-      extended_pose = tier4_autoware_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0);
-      const double x = extended_pose.position.x;
-      const double y = extended_pose.position.y;
-      const double z = extended_pose.position.z;
-      const double dist = std::hypot(x - traj.x.back(), y - traj.y.back(), z - traj.z.back());
-      const double t = traj.relative_time.back() + dist / extend_vel;
-      traj.push_back(x, y, z, traj.yaw.back(), extend_vel, traj.k.back(), traj.smooth_k.back(), t);
-    }
+  if (extend_trajectory_for_end_yaw_control) {
+    trajectory_follower::MPCUtils::extendTrajectoryInYawDirection(
+      mpc_traj_raw.yaw.back(), traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
   }
 
   /* calculate yaw angle */

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -65,7 +65,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
   m_mpc.m_admissible_yaw_error_rad = node_->declare_parameter<double>("admissible_yaw_error_rad");
   m_mpc.m_use_steer_prediction = node_->declare_parameter<bool>("use_steer_prediction");
   m_mpc.m_param.steer_tau = node_->declare_parameter<double>("vehicle_model_steer_tau");
-  m_enable_traj_extending = node_->declare_parameter<bool>("enable_traj_extending");
+  m_extend_trajectory_for_end_yaw_control =
+    node_->declare_parameter<bool>("extend_trajectory_for_end_yaw_control");
 
   /* stop state parameters */
   m_stop_state_entry_ego_speed = node_->declare_parameter<double>("stop_state_entry_ego_speed");
@@ -303,7 +304,8 @@ void MpcLateralController::setTrajectory(
 
   m_mpc.setReferenceTrajectory(
     *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num,
-    m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer, m_enable_traj_extending);
+    m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer,
+    m_extend_trajectory_for_end_yaw_control);
 
   // update trajectory buffer to check the trajectory shape change.
   m_trajectory_buffer.push_back(*m_current_trajectory_ptr);

control/trajectory_follower/src/mpc_utils.cpp

@@ -403,6 +403,33 @@ double calcStopDistance(
   return stop_dist;
 }
 
+void extendTrajectoryInYawDirection(
+  const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj)
+{
+  // set terminal yaw
+  traj.yaw.back() = yaw;
+
+  // get terminal pose
+  autoware_auto_planning_msgs::msg::Trajectory autoware_traj;
+  autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory(
+    traj, autoware_traj);
+  auto extended_pose = autoware_traj.points.back().pose;
+
+  constexpr double extend_dist = 10.0;
+  constexpr double extend_vel = 10.0;
+  const double x_offset = is_forward_shift ? interval : -interval;
+  const double dt = interval / extend_vel;
+  const size_t num_extended_point = static_cast<size_t>(extend_dist / interval);
+  for (size_t i = 0; i < num_extended_point; ++i) {
+    extended_pose = tier4_autoware_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0);
+    const double x = extended_pose.position.x;
+    const double y = extended_pose.position.y;
+    const double z = extended_pose.position.z;
+    const double t = traj.relative_time.back() + dt;
+    traj.push_back(x, y, z, traj.yaw.back(), extend_vel, traj.k.back(), traj.smooth_k.back(), t);
+  }
+}
+
 }  // namespace MPCUtils
 }  // namespace trajectory_follower
 }  // namespace control

control/trajectory_follower/test/test_mpc.cpp

@@ -84,7 +84,7 @@ class MPCTest : public ::testing::Test
   int curvature_smoothing_num_ref_steer = 35;
   bool enable_path_smoothing = true;
   bool use_steer_prediction = true;
-  bool enable_traj_extending = true;
+  bool extend_trajectory_for_end_yaw_control = true;
 
   void SetUp() override
   {
@@ -177,7 +177,7 @@ class MPCTest : public ::testing::Test
     mpc.setReferenceTrajectory(
       dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing,
       path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
-      enable_traj_extending);
+      extend_trajectory_for_end_yaw_control);
   }
 };  // class MPCTest
 
@@ -236,7 +236,7 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn)
   mpc.setReferenceTrajectory(
     dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing,
     path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
-    enable_traj_extending);
+    extend_trajectory_for_end_yaw_control);
 
   // Calculate MPC
   AckermannLateralCommand ctrl_cmd;
@@ -255,7 +255,7 @@ TEST_F(MPCTest, OsqpCalculate)
   mpc.setReferenceTrajectory(
     dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing,
     path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
-    enable_traj_extending);
+    extend_trajectory_for_end_yaw_control);
 
   const std::string vehicle_model_type = "kinematics";
   std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
@@ -287,7 +287,7 @@ TEST_F(MPCTest, OsqpCalculateRightTurn)
   mpc.setReferenceTrajectory(
     dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing,
     path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
-    enable_traj_extending);
+    extend_trajectory_for_end_yaw_control);
 
   const std::string vehicle_model_type = "kinematics";
   std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
@@ -333,7 +333,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate)
   mpc.setReferenceTrajectory(
     dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing,
     path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
-    enable_traj_extending);
+    extend_trajectory_for_end_yaw_control);
   // Calculate MPC
   AckermannLateralCommand ctrl_cmd;
   Trajectory pred_traj;
@@ -351,7 +351,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn)
   mpc.setReferenceTrajectory(
     dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing,
     path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
-    enable_traj_extending);
+    extend_trajectory_for_end_yaw_control);
 
   const std::string vehicle_model_type = "kinematics_no_delay";
   std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =

control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml

@@ -13,7 +13,7 @@
     curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
 
     # -- trajectory extening --
-    enable_traj_extending: true  # flag of trajectory extending for terminal yaw
+    extend_trajectory_for_end_yaw_control: true  # flag of trajectory extending for terminal yaw control
 
     # -- mpc optimization --
     qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp)

launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml

@@ -14,7 +14,7 @@
     curvature_smoothing_num_ref_steer: 15   # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
 
     # -- trajectory extening --
-    enable_traj_extending: true  # flag of trajectory extending for terminal yaw
+    extend_trajectory_for_end_yaw_control: true  # flag of trajectory extending for terminal yaw control
 
     # -- mpc optimization --
     qp_solver_type: "osqp"                       # optimization solver option (unconstraint_fast or osqp)