From 1be94067bc5238b93d41f89dd0e4fbb9eb572175 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 21 Nov 2022 13:21:47 +0900 Subject: [PATCH 1/2] feat(trajectory_follower): improve yaw control on curve Signed-off-by: kosuke55 --- control/trajectory_follower/src/mpc.cpp | 1 + control/trajectory_follower/src/mpc_utils.cpp | 36 +++++++------------ 2 files changed, 14 insertions(+), 23 deletions(-) diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp index 050727c40a50d..8e0dc186622f1 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/control/trajectory_follower/src/mpc.cpp @@ -219,6 +219,7 @@ void MPC::setReferenceTrajectory( /* calculate yaw angle */ trajectory_follower::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift); + mpc_traj_smoothed.yaw.back() = mpc_traj_raw.yaw.back(); trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw); /* calculate curvature */ diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index aece2545c388e..ab38113dd8c27 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -96,6 +96,10 @@ bool resampleMPCTrajectoryByDistance( for (double s = 0; s < input_arclength.back(); s += resample_interval_dist) { output_arclength.push_back(s); } + // keep last point if it is not overlapped + if (input_arclength.back() - output_arclength.back() > 0.01) { + output_arclength.push_back(input_arclength.back()); + } std::vector input_yaw = input.yaw; convertEulerAngleToMonotonic(&input_yaw); @@ -169,7 +173,7 @@ void calcTrajectoryYawFromXY(MPCTrajectory * traj, const bool is_forward_shift) } if (traj->yaw.size() > 1) { traj->yaw[0] = traj->yaw[1]; - traj->yaw.back() = traj->yaw[traj->yaw.size() - 2]; + // yaw.back() is not calculated here because it uses the traj raw yaw } } @@ -196,30 +200,16 @@ std::vector calcTrajectoryCurvature( const size_t max_smoothing_num = static_cast(std::floor(0.5 * (static_cast(traj.x.size() - 1)))); const size_t L = std::min(curvature_smoothing_num, max_smoothing_num); - for (size_t i = L; i < traj.x.size() - L; ++i) { - const size_t curr_idx = i; - const size_t prev_idx = curr_idx - L; - const size_t next_idx = curr_idx + L; - p1.x = traj.x[prev_idx]; - p2.x = traj.x[curr_idx]; - p3.x = traj.x[next_idx]; - p1.y = traj.y[prev_idx]; - p2.y = traj.y[curr_idx]; - p3.y = traj.y[next_idx]; - const double den = std::max( - distance_2d(p1, p2) * distance_2d(p2, p3) * distance_2d(p3, p1), - std::numeric_limits::epsilon()); - const double curvature = - 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den; - curvature_vec.at(curr_idx) = curvature; - } - /* first and last curvature is copied from next value */ - for (size_t i = 0; i < std::min(L, traj.x.size()); ++i) { - curvature_vec.at(i) = curvature_vec.at(std::min(L, traj.x.size() - 1)); - curvature_vec.at(traj.x.size() - i - 1) = - curvature_vec.at(std::max(traj.x.size() - L - 1, size_t(0))); + std::vector arclength{}; + calcMPCTrajectoryArclength(traj, &arclength); + + for (size_t i = 0; i < traj.x.size() - 1; ++i) { + const double curvature = (traj.yaw[i + 1] - traj.yaw[i]) / (arclength[i + 1] - arclength[i]); + curvature_vec.at(i) = curvature; } + curvature_vec.at(traj.x.size() - 1) = curvature_vec.at(traj.x.size() - 2); + return curvature_vec; } From 715cbd70845e54b0dce45b608402da879222c66a Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 22 Nov 2022 00:12:39 +0900 Subject: [PATCH 2/2] remove smooth_k Signed-off-by: kosuke55 --- .../design/mpc_lateral_controller-design.md | 34 ++++++++-------- .../include/trajectory_follower/mpc.hpp | 3 +- .../mpc_lateral_controller.hpp | 5 --- .../trajectory_follower/mpc_trajectory.hpp | 3 +- .../include/trajectory_follower/mpc_utils.hpp | 10 +---- control/trajectory_follower/src/mpc.cpp | 32 +++++---------- .../src/mpc_lateral_controller.cpp | 6 +-- .../src/mpc_trajectory.cpp | 7 +--- control/trajectory_follower/src/mpc_utils.cpp | 39 ++++++------------- control/trajectory_follower/test/test_mpc.cpp | 14 +++---- .../lateral_controller_defaults.param.yaml | 2 - .../lateral_controller.param.yaml | 2 - 12 files changed, 50 insertions(+), 107 deletions(-) diff --git a/control/trajectory_follower/design/mpc_lateral_controller-design.md b/control/trajectory_follower/design/mpc_lateral_controller-design.md index ee79cc88f206b..eb8fe35158c72 100644 --- a/control/trajectory_follower/design/mpc_lateral_controller-design.md +++ b/control/trajectory_follower/design/mpc_lateral_controller-design.md @@ -84,24 +84,22 @@ can be calculated by providing the current steer, velocity, and pose to function The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the AutonomouStuff Lexus RX 450h for under 40 km/h driving. -| Name | Type | Description | Default value | -| :------------------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------ | :------------ | -| show_debug_info | bool | display debug info | false | -| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | -| enable_path_smoothing | bool | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | true | -| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 35 | -| 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 | -| 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 | -| stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.0 | -| stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.0 | -| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | -| keep_steer_control_until_converged | bool | keep steer control until steer is converged | true | -| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | -| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | +| Name | Type | Description | Default value | +| :------------------------------------------- | :----- | :--------------------------------------------------------------------------------------------- | :------------ | +| show_debug_info | bool | display debug info | false | +| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | +| enable_path_smoothing | bool | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | true | +| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 35 | +| path_smoothing_times | int | number of times of applying path smoothing filter | 1 | +| 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 | +| stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.0 | +| stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.0 | +| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | +| keep_steer_control_until_converged | bool | keep steer control until steer is converged | true | +| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | +| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | (\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state. diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp index 8ae6dc5449fc3..2a97c556e9328 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc.hpp @@ -399,8 +399,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC void 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 int path_filter_moving_ave_num); /** * @brief set the vehicle model of this MPC */ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index d38312505fed7..afd2421fa94ac 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -89,11 +89,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController bool m_enable_path_smoothing; //!< @brief param of moving average filter for path smoothing int m_path_filter_moving_ave_num; - //!< @brief point-to-point index distance for curvature calculation for trajectory //NOLINT - int m_curvature_smoothing_num_traj; - //!< @brief point-to-point index distance for curvature calculation for reference steer command - //!< //NOLINT - int m_curvature_smoothing_num_ref_steer; //!< @brief path resampling interval [m] double m_traj_resample_dist; diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp index 9016c0adbb689..328f8fa15fa4e 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp @@ -45,7 +45,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory std::vector yaw; //!< @brief yaw pose yaw vector std::vector vx; //!< @brief vx velocity vx vector std::vector k; //!< @brief k curvature k vector - std::vector smooth_k; //!< @brief k smoothed-curvature k vector std::vector relative_time; //!< @brief relative_time duration time from start point /** @@ -53,7 +52,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory */ 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 & tp); + const double & vxp, const double & kp, const double & tp); /** * @brief clear for all values */ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp index 9f394ff9baa6f..03148b2dc22ff 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp @@ -141,23 +141,17 @@ TRAJECTORY_FOLLOWER_PUBLIC void calcTrajectoryYawFromXY( /** * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 * points when num = 1) - * @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 * calculation * @param [inout] traj object trajectory */ -TRAJECTORY_FOLLOWER_PUBLIC bool calcTrajectoryCurvature( - const size_t curvature_smoothing_num_traj, const size_t curvature_smoothing_num_ref_steer, - MPCTrajectory * traj); +TRAJECTORY_FOLLOWER_PUBLIC bool calcTrajectoryCurvature(MPCTrajectory * traj); /** * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 * points when num = 1) - * @param [in] curvature_smoothing_num index distance for 3 points for curvature calculation * @param [in] traj input trajectory * @return vector of curvatures at each point of the given trajectory */ -TRAJECTORY_FOLLOWER_PUBLIC std::vector calcTrajectoryCurvature( - const size_t curvature_smoothing_num, const MPCTrajectory & traj); +TRAJECTORY_FOLLOWER_PUBLIC std::vector calcTrajectoryCurvature(const MPCTrajectory & traj); /** * @brief calculate nearest pose on MPCTrajectory with linear interpolation * @param [in] traj reference trajectory diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp index 8e0dc186622f1..29b797ed96a83 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/control/trajectory_follower/src/mpc.cpp @@ -119,16 +119,13 @@ bool MPC::calculateMPC( const double yaw = traj.yaw[i] + yaw_error; const double vx = traj.vx[i]; const double k = traj.k[i]; - const double smooth_k = traj.smooth_k[i]; const double relative_time = traj.relative_time[i]; - mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time); + mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, relative_time); } trajectory_follower::MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj, predicted_traj); /* prepare diagnostic message */ const double nearest_k = reference_trajectory.k[static_cast(mpc_data.nearest_idx)]; - const double nearest_smooth_k = - reference_trajectory.smooth_k[static_cast(mpc_data.nearest_idx)]; const double steer_cmd = ctrl_cmd.steering_tire_angle; const double wb = m_vehicle_model_ptr->getWheelbase(); @@ -143,7 +140,7 @@ bool MPC::calculateMPC( // [2] feedforward steering value append_diag_data(mpc_matrix.Uref_ex(0)); // [3] feedforward steering value raw - append_diag_data(std::atan(nearest_smooth_k * wb)); + append_diag_data(std::atan(nearest_k * wb)); // [4] current steering angle append_diag_data(mpc_data.steer); // [5] lateral error @@ -163,14 +160,12 @@ bool MPC::calculateMPC( // [12] angvel from measured steer append_diag_data(current_velocity * tan(mpc_data.steer) / wb); // [13] angvel from path curvature - append_diag_data(current_velocity * nearest_smooth_k); - // [14] nearest path curvature (used for feedforward) - append_diag_data(nearest_smooth_k); - // [15] nearest path curvature (not smoothed) + append_diag_data(current_velocity * nearest_k); + // [14] nearest path curvature append_diag_data(nearest_k); - // [16] predicted steer + // [15] predicted steer append_diag_data(mpc_data.predicted_steer); - // [17] angvel from predicted steer + // [16] angvel from predicted steer append_diag_data(current_velocity * tan(mpc_data.predicted_steer) / wb); return true; @@ -179,8 +174,7 @@ bool MPC::calculateMPC( 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 int path_filter_moving_ave_num) { trajectory_follower::MPCTrajectory mpc_traj_raw; // received raw trajectory trajectory_follower::MPCTrajectory mpc_traj_resampled; // resampled trajectory @@ -223,9 +217,7 @@ void MPC::setReferenceTrajectory( trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw); /* calculate curvature */ - trajectory_follower::MPCUtils::calcTrajectoryCurvature( - static_cast(curvature_smoothing_num_traj), - static_cast(curvature_smoothing_num_ref_steer), &mpc_traj_smoothed); + trajectory_follower::MPCUtils::calcTrajectoryCurvature(&mpc_traj_smoothed); /* add end point with vel=0 on traj for mpc prediction */ { @@ -234,9 +226,7 @@ void MPC::setReferenceTrajectory( const double t_end = t.relative_time.back() + t_ext; const double v_end = 0.0; t.vx.back() = v_end; // set for end point - t.push_back( - t.x.back(), t.y.back(), t.z.back(), t.yaw.back(), v_end, t.k.back(), t.smooth_k.back(), - t_end); + t.push_back(t.x.back(), t.y.back(), t.z.back(), t.yaw.back(), v_end, t.k.back(), t_end); } if (!mpc_traj_smoothed.size()) { @@ -506,7 +496,7 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( output.vx.back() = v_end; // set for end point output.push_back( output.x.back(), output.y.back(), output.z.back(), output.yaw.back(), v_end, output.k.back(), - output.smooth_k.back(), t_end); + t_end); return output; } @@ -556,7 +546,6 @@ MPCMatrix MPC::generateMPCMatrix( const double ref_vx_squared = ref_vx * ref_vx; const double ref_k = reference_trajectory.k[static_cast(i)] * sign_vx; - const double ref_smooth_k = reference_trajectory.smooth_k[static_cast(i)] * sign_vx; /* get discrete state matrix A, B, C, W */ m_vehicle_model_ptr->setVelocity(ref_vx); @@ -602,7 +591,6 @@ MPCMatrix MPC::generateMPCMatrix( m.R1ex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive; /* get reference input (feed-forward) */ - m_vehicle_model_ptr->setCurvature(ref_smooth_k); m_vehicle_model_ptr->calculateReferenceInput(Uref); if (std::fabs(Uref(0, 0)) < DEG2RAD * m_param.zero_ff_steer_deg) { Uref(0, 0) = 0.0; // ignore curvature noise diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index a90fa0fd61da6..e52853376c469 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -57,9 +57,6 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} m_mpc.m_ctrl_period = node_->get_parameter("ctrl_period").as_double(); m_enable_path_smoothing = node_->declare_parameter("enable_path_smoothing"); m_path_filter_moving_ave_num = node_->declare_parameter("path_filter_moving_ave_num"); - m_curvature_smoothing_num_traj = node_->declare_parameter("curvature_smoothing_num_traj"); - m_curvature_smoothing_num_ref_steer = - node_->declare_parameter("curvature_smoothing_num_ref_steer"); m_traj_resample_dist = node_->declare_parameter("traj_resample_dist"); m_mpc.m_admissible_position_error = node_->declare_parameter("admissible_position_error"); m_mpc.m_admissible_yaw_error_rad = node_->declare_parameter("admissible_yaw_error_rad"); @@ -300,8 +297,7 @@ 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); + *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num); // update trajectory buffer to check the trajectory shape change. m_trajectory_buffer.push_back(*m_current_trajectory_ptr); diff --git a/control/trajectory_follower/src/mpc_trajectory.cpp b/control/trajectory_follower/src/mpc_trajectory.cpp index cf0a45763a7ff..91f871e34f73a 100644 --- a/control/trajectory_follower/src/mpc_trajectory.cpp +++ b/control/trajectory_follower/src/mpc_trajectory.cpp @@ -24,7 +24,7 @@ namespace trajectory_follower { void MPCTrajectory::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 & tp) + const double & kp, const double & tp) { x.push_back(xp); y.push_back(yp); @@ -32,7 +32,6 @@ void MPCTrajectory::push_back( yaw.push_back(yawp); vx.push_back(vxp); k.push_back(kp); - smooth_k.push_back(smooth_kp); relative_time.push_back(tp); } @@ -44,7 +43,6 @@ void MPCTrajectory::clear() yaw.clear(); vx.clear(); k.clear(); - smooth_k.clear(); relative_time.clear(); } @@ -52,8 +50,7 @@ size_t MPCTrajectory::size() const { if ( x.size() == y.size() && x.size() == z.size() && x.size() == yaw.size() && - x.size() == vx.size() && x.size() == k.size() && x.size() == smooth_k.size() && - x.size() == relative_time.size()) { + x.size() == vx.size() && x.size() == k.size() && x.size() == relative_time.size()) { return x.size(); } else { std::cerr << "[MPC trajectory] trajectory size is inappropriate" << std::endl; diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index ab38113dd8c27..306ade6d04810 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -110,7 +110,6 @@ bool resampleMPCTrajectoryByDistance( output->yaw = interpolation::spline(input_arclength, input.yaw, output_arclength); output->vx = interpolation::lerp(input_arclength, input.vx, output_arclength); output->k = interpolation::spline(input_arclength, input.k, output_arclength); - output->smooth_k = interpolation::spline(input_arclength, input.smooth_k, output_arclength); output->relative_time = interpolation::lerp(input_arclength, input.relative_time, output_arclength); @@ -140,7 +139,6 @@ bool linearInterpMPCTrajectory( !linearInterpolate(in_index, in_traj_yaw, out_index, out_traj->yaw) || !linearInterpolate(in_index, in_traj.vx, out_index, out_traj->vx) || !linearInterpolate(in_index, in_traj.k, out_index, out_traj->k) || - !linearInterpolate(in_index, in_traj.smooth_k, out_index, out_traj->smooth_k) || !linearInterpolate(in_index, in_traj.relative_time, out_index, out_traj->relative_time)) { std::cerr << "linearInterpMPCTrajectory error!" << std::endl; return false; @@ -177,40 +175,25 @@ void calcTrajectoryYawFromXY(MPCTrajectory * traj, const bool is_forward_shift) } } -bool calcTrajectoryCurvature( - const size_t curvature_smoothing_num_traj, const size_t curvature_smoothing_num_ref_steer, - MPCTrajectory * traj) +bool calcTrajectoryCurvature(MPCTrajectory * traj) { - if (!traj) { + if (traj->x.empty()) { return false; } - traj->k = calcTrajectoryCurvature(curvature_smoothing_num_traj, *traj); - traj->smooth_k = calcTrajectoryCurvature(curvature_smoothing_num_ref_steer, *traj); - return true; -} - -std::vector calcTrajectoryCurvature( - const size_t curvature_smoothing_num, const MPCTrajectory & traj) -{ - std::vector curvature_vec(traj.x.size()); - - /* calculate curvature by circle fitting from three points */ - geometry_msgs::msg::Point p1, p2, p3; - const size_t max_smoothing_num = - static_cast(std::floor(0.5 * (static_cast(traj.x.size() - 1)))); - const size_t L = std::min(curvature_smoothing_num, max_smoothing_num); - std::vector arclength{}; - calcMPCTrajectoryArclength(traj, &arclength); + calcMPCTrajectoryArclength(*traj, &arclength); - for (size_t i = 0; i < traj.x.size() - 1; ++i) { - const double curvature = (traj.yaw[i + 1] - traj.yaw[i]) / (arclength[i + 1] - arclength[i]); + std::vector curvature_vec(traj->x.size()); + for (size_t i = 0; i < traj->x.size() - 1; ++i) { + const double curvature = (traj->yaw[i + 1] - traj->yaw[i]) / (arclength[i + 1] - arclength[i]); curvature_vec.at(i) = curvature; } - curvature_vec.at(traj.x.size() - 1) = curvature_vec.at(traj.x.size() - 2); + curvature_vec.at(traj->x.size() - 1) = curvature_vec.at(traj->x.size() - 2); - return curvature_vec; + traj->k = curvature_vec; + + return true; } bool convertToMPCTrajectory( @@ -225,7 +208,7 @@ bool convertToMPCTrajectory( const double vx = p.longitudinal_velocity_mps; const double k = 0.0; const double t = 0.0; - output.push_back(x, y, z, yaw, vx, k, k, t); + output.push_back(x, y, z, yaw, vx, k, t); } calcMPCTrajectoryTime(output); return true; diff --git a/control/trajectory_follower/test/test_mpc.cpp b/control/trajectory_follower/test/test_mpc.cpp index 909133dcd65bd..819ea2d7ec221 100644 --- a/control/trajectory_follower/test/test_mpc.cpp +++ b/control/trajectory_follower/test/test_mpc.cpp @@ -80,8 +80,6 @@ class MPCTest : public ::testing::Test double ctrl_period = 0.03; double traj_resample_dist = 0.1; int path_filter_moving_ave_num = 35; - int curvature_smoothing_num_traj = 1; - int curvature_smoothing_num_ref_steer = 35; bool enable_path_smoothing = true; bool use_steer_prediction = true; @@ -175,7 +173,7 @@ class MPCTest : public ::testing::Test // Init trajectory 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); + path_filter_moving_ave_num); } }; // class MPCTest @@ -233,7 +231,7 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) initializeMPC(mpc); 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); + path_filter_moving_ave_num); // Calculate MPC AckermannLateralCommand ctrl_cmd; @@ -251,7 +249,7 @@ TEST_F(MPCTest, OsqpCalculate) initializeMPC(mpc); 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); + path_filter_moving_ave_num); const std::string vehicle_model_type = "kinematics"; std::shared_ptr vehicle_model_ptr = @@ -282,7 +280,7 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) initializeMPC(mpc); 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); + path_filter_moving_ave_num); const std::string vehicle_model_type = "kinematics"; std::shared_ptr vehicle_model_ptr = @@ -327,7 +325,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) // Init trajectory 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); + path_filter_moving_ave_num); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; @@ -344,7 +342,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) initializeMPC(mpc); 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); + path_filter_moving_ave_num); const std::string vehicle_model_type = "kinematics_no_delay"; std::shared_ptr vehicle_model_ptr = diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml index a6d98fa682d74..225e1a7439ecf 100644 --- a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml @@ -9,8 +9,6 @@ # -- path smoothing -- enable_path_smoothing: false # flag for path smoothing path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing - curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) - 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) # -- mpc optimization -- qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index 76b5da140bfaa..8d88e03c3115d 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -10,8 +10,6 @@ # -- path smoothing -- enable_path_smoothing: false # flag for path smoothing path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing - curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) - 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) # -- mpc optimization -- qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp)