make mpc min vel param

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

control/trajectory_follower/include/trajectory_follower/mpc.hpp

@@ -69,6 +69,8 @@ struct MPCParam
   double velocity_time_constant;
   //!< @brief minimum prediction dist for low velocity
   double min_prediction_length;
+  //!< @brief minimum velocity for relative time
+  double min_vel;
   //!< @brief time constant for steer model
   double steer_tau;
   // for weight matrix Q

control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp

@@ -75,11 +75,13 @@ TRAJECTORY_FOLLOWER_PUBLIC double calcLateralError(
 /**
  * @brief convert the given Trajectory msg to a MPCTrajectory object
  * @param [in] input trajectory to convert
+ * @param [in] min_vel minimum velocity for relative time
  * @param [out] output resulting MPCTrajectory
  * @return true if the conversion was successful
  */
 TRAJECTORY_FOLLOWER_PUBLIC bool convertToMPCTrajectory(
-  const autoware_auto_planning_msgs::msg::Trajectory & input, MPCTrajectory & output);
+  const autoware_auto_planning_msgs::msg::Trajectory & input, const double min_vel,
+  MPCTrajectory & output);
 /**
  * @brief convert the given MPCTrajectory to a Trajectory msg
  * @param [in] input MPCTrajectory to convert
@@ -116,21 +118,23 @@ TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpMPCTrajectory(
   const std::vector<double> & out_index, MPCTrajectory * out_traj);
 /**
  * @brief fill the relative_time field of the given MPCTrajectory
+ * @param [in] min_vel minimum velocity for relative time
  * @param [in] traj MPCTrajectory for which to fill in the relative_time
  * @return true if the calculation was successful
  */
-TRAJECTORY_FOLLOWER_PUBLIC bool calcMPCTrajectoryTime(MPCTrajectory & traj);
+TRAJECTORY_FOLLOWER_PUBLIC bool calcMPCTrajectoryTime(const double min_vel, MPCTrajectory & traj);
 /**
  * @brief recalculate the velocity field (vx) of the MPCTrajectory with dynamic smoothing
  * @param [in] start_idx index of the trajectory point from which to start smoothing
  * @param [in] start_vel initial velocity to set at the start_idx
  * @param [in] acc_lim limit on the acceleration
  * @param [in] tau constant to control the smoothing (high-value = very smooth)
+ * @param [in] min_vel minimum velocity for relative time
  * @param [inout] traj MPCTrajectory for which to calculate the smoothed velocity
  */
 TRAJECTORY_FOLLOWER_PUBLIC void dynamicSmoothingVelocity(
   const size_t start_idx, const double start_vel, const double acc_lim, const double tau,
-  MPCTrajectory & traj);
+  const double min_vel, MPCTrajectory & traj);
 /**
  * @brief calculate yaw angle in MPCTrajectory from xy vector
  * @param [inout] traj object trajectory

control/trajectory_follower/src/mpc.cpp

@@ -187,7 +187,8 @@ void MPC::setReferenceTrajectory(
   trajectory_follower::MPCTrajectory mpc_traj_smoothed;   // smooth filtered trajectory
 
   /* resampling */
-  trajectory_follower::MPCUtils::convertToMPCTrajectory(trajectory_msg, mpc_traj_raw);
+  trajectory_follower::MPCUtils::convertToMPCTrajectory(
+    trajectory_msg, m_param.min_vel, mpc_traj_raw);
   if (!trajectory_follower::MPCUtils::resampleMPCTrajectoryByDistance(
         mpc_traj_raw, traj_resample_dist, &mpc_traj_resampled)) {
     RCLCPP_WARN(m_logger, "[setReferenceTrajectory] spline error when resampling by distance");
@@ -498,7 +499,7 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter(
 
   trajectory_follower::MPCTrajectory output = input;
   trajectory_follower::MPCUtils::dynamicSmoothingVelocity(
-    static_cast<size_t>(nearest_idx), v0, acc_lim, tau, output);
+    static_cast<size_t>(nearest_idx), v0, acc_lim, tau, m_param.min_vel, output);
   const double t_ext = 100.0;  // extra time to prevent mpc calculation failure due to short time
   const double t_end = output.relative_time.back() + t_ext;
   const double v_end = 0.0;

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -300,10 +300,14 @@ void MpcLateralController::setTrajectory(
   // consider the terminal yaw
   m_current_trajectory_ptr = std::make_shared<autoware_auto_planning_msgs::msg::Trajectory>(*msg);
   auto extended_point = msg->points.back();
-  constexpr double extend_length = 1.0;
-  extended_point.pose =
-    tier4_autoware_utils::calcOffsetPose(extended_point.pose, extend_length, 0.0, 0.0);
-  m_current_trajectory_ptr->points.push_back(extended_point);
+  constexpr double extend_dist = 10.0;
+  constexpr double extend_interval = 1.0;
+  const size_t num_extended_point = static_cast<size_t>(extend_dist / extend_interval);
+  for (size_t i = 0; i < num_extended_point; ++i) {
+    extended_point.pose =
+      tier4_autoware_utils::calcOffsetPose(extended_point.pose, extend_interval, 0.0, 0.0);
+    m_current_trajectory_ptr->points.push_back(extended_point);
+  }
 
   m_mpc.setReferenceTrajectory(
     *m_current_trajectory_ptr, m_traj_resample_dist, m_enable_path_smoothing,
@@ -444,6 +448,7 @@ void MpcLateralController::declareMPCparameters()
     node_->declare_parameter<double>("mpc_velocity_time_constant");
   m_mpc.m_param.min_prediction_length =
     node_->declare_parameter<double>("mpc_min_prediction_length");
+  m_mpc.m_param.min_vel = node_->declare_parameter<double>("mpc_min_vel");
 }
 
 rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback(

control/trajectory_follower/src/mpc_utils.cpp

@@ -224,7 +224,8 @@ std::vector<double> calcTrajectoryCurvature(
 }
 
 bool convertToMPCTrajectory(
-  const autoware_auto_planning_msgs::msg::Trajectory & input, MPCTrajectory & output)
+  const autoware_auto_planning_msgs::msg::Trajectory & input, const double min_vel,
+  MPCTrajectory & output)
 {
   output.clear();
   for (const autoware_auto_planning_msgs::msg::TrajectoryPoint & p : input.points) {
@@ -237,7 +238,7 @@ bool convertToMPCTrajectory(
     const double t = 0.0;
     output.push_back(x, y, z, yaw, vx, k, k, t);
   }
-  calcMPCTrajectoryTime(output);
+  calcMPCTrajectoryTime(min_vel, output);
   return true;
 }
 
@@ -259,7 +260,7 @@ bool convertToAutowareTrajectory(
   return true;
 }
 
-bool calcMPCTrajectoryTime(MPCTrajectory & traj)
+bool calcMPCTrajectoryTime(const double min_vel, MPCTrajectory & traj)
 {
   double t = 0.0;
   traj.relative_time.clear();
@@ -269,7 +270,7 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj)
     const double dy = traj.y.at(i + 1) - traj.y.at(i);
     const double dz = traj.z.at(i + 1) - traj.z.at(i);
     const double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
-    const double v = std::max(std::fabs(traj.vx.at(i)), 0.1);
+    const double v = std::max(std::fabs(traj.vx.at(i)), min_vel);
     t += (dist / v);
     traj.relative_time.push_back(t);
   }
@@ -278,7 +279,7 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj)
 
 void dynamicSmoothingVelocity(
   const size_t start_idx, const double start_vel, const double acc_lim, const double tau,
-  MPCTrajectory & traj)
+  const double min_vel, MPCTrajectory & traj)
 {
   double curr_v = start_vel;
   traj.vx.at(start_idx) = start_vel;
@@ -292,7 +293,7 @@ void dynamicSmoothingVelocity(
     curr_v = curr_v + dv;
     traj.vx.at(i) = curr_v;
   }
-  calcMPCTrajectoryTime(traj);
+  calcMPCTrajectoryTime(min_vel, traj);
 }
 
 bool calcNearestPoseInterp(

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

@@ -40,6 +40,7 @@
     mpc_acceleration_limit: 2.0                  # limit on the vehicle's acceleration
     mpc_velocity_time_constant: 0.3              # time constant used for velocity smoothing
     mpc_min_prediction_length: 5.0               # minimum prediction length
+    mpc_min_vel: 1.0                             # minimum velocity for relative time
 
     # -- vehicle model --
     vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics