Skip to content

Commit 72cc84e

Browse files
committed
fix(motion_velocity_smoother): calc first point front_wheel_angle_rad
Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>
1 parent f514bca commit 72cc84e

File tree

1 file changed

+1
-2
lines changed

1 file changed

+1
-2
lines changed

planning/motion_velocity_smoother/src/smoother/smoother_base.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -215,7 +215,7 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit(
215215

216216
// Step2. Calculate steer rate for each trajectory point.
217217
std::vector<double> steer_rate_arr(output.size(), 0.0);
218-
for (size_t i = 1; i < output.size() - 1; i++) {
218+
for (size_t i = 0; i < output.size() - 1; i++) {
219219
// velocity
220220
const auto & v_front = output.at(i + 1).longitudinal_velocity_mps;
221221
const auto & v_back = output.at(i).longitudinal_velocity_mps;
@@ -234,7 +234,6 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit(
234234
steer_rate_arr.at(i) = steering_diff / dt;
235235
}
236236

237-
steer_rate_arr.at(0) = steer_rate_arr.at(1);
238237
steer_rate_arr.back() = steer_rate_arr.at((output.size() - 2));
239238

240239
// Step3. Remove noise by mean filter.

0 commit comments

Comments
 (0)