From ec82e90f140a0864769321783a77c26d0c0308eb Mon Sep 17 00:00:00 2001 From: Paul Verhoeckx Date: Tue, 22 Mar 2022 15:54:53 +0100 Subject: [PATCH] fix: Enforce positive end phase time and simplify end phase distance --- src/controller.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 56bbc3f5..75047f4c 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -448,16 +448,12 @@ Controller::UpdateResult Controller::update( if ( (current_target_x_vel_ > 0.0 && current_x_vel > target_end_x_vel) || (current_target_x_vel_ < 0.0 && current_x_vel < target_end_x_vel)) { - t_end_phase_current = (target_end_x_vel - current_x_vel) / (-config_.target_x_decc); - d_end_phase = current_x_vel * t_end_phase_current - - 0.5 * (config_.target_x_decc) * t_end_phase_current * t_end_phase_current + - fabs(target_x_vel) * 2.0 * dt.toSec(); + t_end_phase_current = fabs((target_end_x_vel - current_x_vel) / config_.target_x_decc); } else { - t_end_phase_current = (target_end_x_vel - current_x_vel) / (config_.target_x_acc); - d_end_phase = current_x_vel * t_end_phase_current + - 0.5 * (config_.target_x_acc) * t_end_phase_current * t_end_phase_current + - fabs(target_x_vel) * 2.0 * dt.toSec(); + t_end_phase_current = fabs((target_end_x_vel - current_x_vel) / config_.target_x_acc); } + d_end_phase = (current_x_vel + target_end_x_vel) * 0.5 * t_end_phase_current + + target_x_vel * 2.0 * dt.toSec(); ROS_DEBUG("t_end_phase_current: %f", t_end_phase_current); ROS_DEBUG("d_end_phase: %f", d_end_phase); ROS_DEBUG("distance_to_goal: %f", distance_to_goal);