diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 50ea69e209c34..16e85641fe79b 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -71,13 +71,6 @@ double getPitchByTraj( */ double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to); -/** - * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and - * acceleration for delayed time - */ -std::pair calcDistAndVelAfterTimeDelay( - const double delay_time, const double current_vel, const double current_acc); - /** * @brief apply linear interpolation to orientation * @param [in] o_from first orientation diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 01c6591e1accb..52d6d4986a4d2 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -117,26 +117,6 @@ double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint return pitch; } -std::pair calcDistAndVelAfterTimeDelay( - const double delay_time, const double current_vel, const double current_acc) -{ - if (delay_time <= 0.0) { - return std::make_pair(0.0, 0.0); - } - - // check time to stop - const double time_to_stop = -current_vel / current_acc; - - const double delay_time_calculation = - time_to_stop > 0.0 && time_to_stop < delay_time ? time_to_stop : delay_time; - // simple linear prediction - const double vel_after_delay = current_vel + current_acc * delay_time_calculation; - const double running_distance = delay_time_calculation * current_vel + 0.5 * current_acc * - delay_time_calculation * - delay_time_calculation; - return std::make_pair(running_distance, vel_after_delay); -} - double lerp(const double v_from, const double v_to, const double ratio) { return v_from + (v_to - v_from) * ratio;