From db13b9cb50c9af7ba61dede7795fd898bf346c84 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Wed, 20 Nov 2024 16:39:18 +0100 Subject: [PATCH] CollisionPrevention: Added Case where velocity gets reduced to zero if we are closer to the obstacle than the minimal distance --- .../collision_prevention/CollisionPrevention.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 691f13c4dffb..0733f1b289d4 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -600,12 +600,18 @@ void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehic delay_distance += curr_vel_parallel * (data_age * 1e-6f); } - const float stop_distance = math::max(0.f, distance - _min_dist_to_keep - delay_distance); + const float stop_distance = distance - _min_dist_to_keep - delay_distance; - const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), - _param_mpc_acc_hor.get(), stop_distance, 0.f); - // we dont take the minimum of the last term because of stop_distance is zero but current velocity is not, we want the acceleration to become negative and slow us down. - const float curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * (max_vel - curr_vel_parallel); + float curr_acc_vel_constraint; + + if (stop_distance >= 0.f) { + const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), + _param_mpc_acc_hor.get(), stop_distance, 0.f); + curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * math::min(max_vel - curr_vel_parallel, 0.f); + + } else { + curr_acc_vel_constraint = -1.f * _param_mpc_xy_vel_p_acc.get() * curr_vel_parallel; + } if (curr_acc_vel_constraint < vel_comp_accel) { vel_comp_accel = curr_acc_vel_constraint;