diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 057308e4329d1..b77af629ad7c3 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -786,8 +786,10 @@ MotionVelocitySmootherNode::calcInitialMotion( // We should plan from the current vehicle speed, but if the initial value is greater than the // velocity limit, the current planning algorithm decelerates with a very high deceleration. // To avoid this, we set the initial value of the vehicle speed to be below the speed limit. + const auto p = smoother_->getBaseParam(); const auto v0 = std::min(target_vel, vehicle_speed); - const Motion initial_motion = {v0, vehicle_acceleration}; + const auto a0 = std::clamp(vehicle_acceleration, p.min_decel, p.max_accel); + const Motion initial_motion = {v0, a0}; return {initial_motion, InitializeType::EGO_VELOCITY}; } }