From 33e70c18c40a35cdd16cba644d594a111ce647fd Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 25 Sep 2023 14:16:22 +0900 Subject: [PATCH] fix(pid_longitudinal_controller): better slope compensation (#5095) Signed-off-by: Takayuki Murooka --- .../src/longitudinal_controller_utils.cpp | 36 +++++++------------ 1 file changed, 13 insertions(+), 23 deletions(-) diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 41bbb48ff5d08..9791a1f0b5e3e 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -91,32 +91,22 @@ double getPitchByTraj( return 0.0; } - for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = tier4_autoware_utils::calcDistance2d( - trajectory.points.at(nearest_idx), trajectory.points.at(i)); - if (dist > wheel_base) { - // calculate pitch from trajectory between rear wheel (nearest) and front center (i) - return tier4_autoware_utils::calcElevationAngle( - trajectory.points.at(nearest_idx).pose.position, trajectory.points.at(i).pose.position); + const auto [prev_idx, next_idx] = [&]() { + for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { + const double dist = tier4_autoware_utils::calcDistance2d( + trajectory.points.at(nearest_idx), trajectory.points.at(i)); + if (dist > wheel_base) { + // calculate pitch from trajectory between rear wheel (nearest) and front center (i) + return std::make_pair(nearest_idx, i); + } } - } - - // close to goal - for (size_t i = trajectory.points.size() - 1; i > 0; --i) { - const double dist = - tier4_autoware_utils::calcDistance2d(trajectory.points.back(), trajectory.points.at(i)); - - if (dist > wheel_base) { - // calculate pitch from trajectory - // between wheelbase behind the end of trajectory (i) and the end of trajectory (back) - return tier4_autoware_utils::calcElevationAngle( - trajectory.points.at(i).pose.position, trajectory.points.back().pose.position); - } - } + // NOTE: The ego pose is close to the goal. + return std::make_pair( + std::min(nearest_idx, trajectory.points.size() - 2), trajectory.points.size() - 1); + }(); - // calculate pitch from trajectory between the beginning and end of trajectory return tier4_autoware_utils::calcElevationAngle( - trajectory.points.at(0).pose.position, trajectory.points.back().pose.position); + trajectory.points.at(prev_idx).pose.position, trajectory.points.at(next_idx).pose.position); } Pose calcPoseAfterTimeDelay(