@@ -54,7 +54,8 @@ TrajectoryPoints applyPreProcess(
54
54
arc_length.push_back (s);
55
55
}
56
56
57
- const auto points = autoware::motion_utils::resampleTrajectory (convertToTrajectory (input), arc_length);
57
+ const auto points =
58
+ autoware::motion_utils::resampleTrajectory (convertToTrajectory (input), arc_length);
58
59
output = convertToTrajectoryPointArray (points);
59
60
output.back () = input.back (); // keep the final speed.
60
61
@@ -173,7 +174,8 @@ TrajectoryPoints SmootherBase::apply_lateral_acceleration_filter(
173
174
std::max (static_cast <int >((base_param_.curvature_calculation_distance ) / points_interval), 1 ));
174
175
175
176
// Calculate curvature assuming the trajectory points interval is constant
176
- const auto curvature_v = trajectory_utils::calc_trajectory_curvature_from_3_points (output, idx_dist);
177
+ const auto curvature_v =
178
+ trajectory_utils::calc_trajectory_curvature_from_3_points (output, idx_dist);
177
179
178
180
// Decrease speed according to lateral G
179
181
const size_t before_decel_index =
@@ -183,10 +185,11 @@ TrajectoryPoints SmootherBase::apply_lateral_acceleration_filter(
183
185
const double max_lateral_accel_abs = std::fabs (base_param_.max_lateral_accel );
184
186
185
187
const auto latacc_min_vel_arr =
186
- enable_smooth_limit ? trajectory_utils::calc_velocity_profile_with_constant_jerk_and_acceleration_limit (
187
- output, v0, a0, base_param_.min_jerk , base_param_.max_accel ,
188
- base_param_.min_decel_for_lateral_acc_lim_filter )
189
- : std::vector<double >{};
188
+ enable_smooth_limit
189
+ ? trajectory_utils::calc_velocity_profile_with_constant_jerk_and_acceleration_limit (
190
+ output, v0, a0, base_param_.min_jerk , base_param_.max_accel ,
191
+ base_param_.min_decel_for_lateral_acc_lim_filter )
192
+ : std::vector<double >{};
190
193
191
194
for (size_t i = 0 ; i < output.size (); ++i) {
192
195
double curvature = 0.0 ;
@@ -229,7 +232,8 @@ TrajectoryPoints SmootherBase::apply_steering_rate_limit(
229
232
std::max (static_cast <int >((base_param_.curvature_calculation_distance ) / points_interval), 1 ));
230
233
231
234
// Step1. Calculate curvature assuming the trajectory points interval is constant.
232
- const auto curvature_v = trajectory_utils::calc_trajectory_curvature_from_3_points (output, idx_dist);
235
+ const auto curvature_v =
236
+ trajectory_utils::calc_trajectory_curvature_from_3_points (output, idx_dist);
233
237
234
238
// Step2. Calculate steer rate for each trajectory point.
235
239
std::vector<double > steer_rate_arr (output.size (), 0.0 );
0 commit comments