From 3fa29a57fda89e32ec67081e03d381139c1ea01c Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 21 Sep 2023 11:44:33 +0900 Subject: [PATCH 1/2] fix(planning_validator): improve curvature calculation for less noise (#5049) Signed-off-by: Takamasa Horibe --- .../include/planning_validator/utils.hpp | 4 +- planning/planning_validator/src/utils.cpp | 77 +++++++++++++------ 2 files changed, 58 insertions(+), 23 deletions(-) diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/planning_validator/utils.hpp index 6c0d2e86943dd..5831993d748b6 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/planning_validator/utils.hpp @@ -36,7 +36,9 @@ std::pair getAbsMaxValAndIdx(const std::vector & v); Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_interval); -void calcCurvature(const Trajectory & trajectory, std::vector & curvatures); +void calcCurvature( + const Trajectory & trajectory, std::vector & curvatures, + const double curvature_distance = 1.0); void calcSteeringAngles( const Trajectory & trajectory, const double wheelbase, std::vector & steering_array); diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index e4c882cfaedb9..87902833743ef 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -89,26 +89,68 @@ Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_in return resampled; } -void calcCurvature(const Trajectory & trajectory, std::vector & curvature_arr) +// calculate curvature from three points with curvature_distance +void calcCurvature( + const Trajectory & trajectory, std::vector & curvature_arr, + const double curvature_distance) { if (trajectory.points.size() < 3) { curvature_arr = std::vector(trajectory.points.size(), 0.0); return; } - curvature_arr.push_back(0.0); + // calc arc length array: arc_length(3) - arc_length(0) is distance from point(3) to point(0) + std::vector arc_length(trajectory.points.size(), 0.0); + for (size_t i = 1; i < trajectory.points.size(); ++i) { + arc_length.at(i) = + arc_length.at(i - 1) + calcDistance2d(trajectory.points.at(i - 1), trajectory.points.at(i)); + } + + // initialize with 0 curvature + curvature_arr = std::vector(trajectory.points.size(), 0.0); + + size_t first_distant_index = 0; + size_t last_distant_index = trajectory.points.size() - 1; for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { - const auto p1 = getPoint(trajectory.points.at(i - 1)); + // find the previous point + size_t prev_idx = 0; + for (size_t j = i - 1; j > 0; --j) { + if (arc_length.at(i) - arc_length.at(j) > curvature_distance) { + if (first_distant_index == 0) { + first_distant_index = i; // save first index that meets distance requirement + } + prev_idx = j; + break; + } + } + + // find the next point + size_t next_idx = trajectory.points.size() - 1; + for (size_t j = i + 1; j < trajectory.points.size(); ++j) { + if (arc_length.at(j) - arc_length.at(i) > curvature_distance) { + last_distant_index = i; // save last index that meets distance requirement + next_idx = j; + break; + } + } + + const auto p1 = getPoint(trajectory.points.at(prev_idx)); const auto p2 = getPoint(trajectory.points.at(i)); - const auto p3 = getPoint(trajectory.points.at(i + 1)); + const auto p3 = getPoint(trajectory.points.at(next_idx)); try { - curvature_arr.push_back(tier4_autoware_utils::calcCurvature(p1, p2, p3)); + curvature_arr.at(i) = tier4_autoware_utils::calcCurvature(p1, p2, p3); } catch (...) { - // maybe distance is too close - curvature_arr.push_back(0.0); + curvature_arr.at(i) = 0.0; // maybe distance is too close } } - curvature_arr.push_back(0.0); + + // use previous or last curvature where the distance is not enough + for (size_t i = first_distant_index; i > 0; --i) { + curvature_arr.at(i - 1) = curvature_arr.at(i); + } + for (size_t i = last_distant_index; i < curvature_arr.size() - 1; ++i) { + curvature_arr.at(i + 1) = curvature_arr.at(i); + } } std::pair calcMaxCurvature(const Trajectory & trajectory) @@ -117,22 +159,13 @@ std::pair calcMaxCurvature(const Trajectory & trajectory) return {0.0, 0}; } - double max_curvature = 0.0; - size_t max_index = 0; - for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { - const auto p1 = getPoint(trajectory.points.at(i - 1)); - const auto p2 = getPoint(trajectory.points.at(i)); - const auto p3 = getPoint(trajectory.points.at(i + 1)); + std::vector curvature_arr; + calcCurvature(trajectory, curvature_arr); - try { - const auto k = tier4_autoware_utils::calcCurvature(p1, p2, p3); - takeBigger(max_curvature, max_index, std::abs(k), i); - } catch (...) { - // maybe distance is too close - } - } + const auto max_curvature_it = std::max_element(curvature_arr.begin(), curvature_arr.end()); + const size_t index = std::distance(curvature_arr.begin(), max_curvature_it); - return {max_curvature, max_index}; + return {*max_curvature_it, index}; } std::pair calcMaxIntervalDistance(const Trajectory & trajectory) From c8336edb8a2d527e20a08571819111dbf3f1df37 Mon Sep 17 00:00:00 2001 From: kaigohirao <127086108+kaigohirao@users.noreply.github.com> Date: Mon, 25 Mar 2024 10:47:46 +0900 Subject: [PATCH 2/2] fix(planning_validator): calculate max lateral acceleration after resampling trajectory (#6679) fix planning validator calc max lat acc after resampling Signed-off-by: kaigohirao --- planning/planning_validator/src/planning_validator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index c10aac702bc59..c1cb1330ce30e 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -247,7 +247,6 @@ void PlanningValidator::validate(const Trajectory & trajectory) s.is_valid_finite_value = checkValidFiniteValue(trajectory); s.is_valid_interval = checkValidInterval(trajectory); - s.is_valid_lateral_acc = checkValidLateralAcceleration(trajectory); s.is_valid_longitudinal_max_acc = checkValidMaxLongitudinalAcceleration(trajectory); s.is_valid_longitudinal_min_acc = checkValidMinLongitudinalAcceleration(trajectory); s.is_valid_velocity_deviation = checkValidVelocityDeviation(trajectory); @@ -260,6 +259,7 @@ void PlanningValidator::validate(const Trajectory & trajectory) s.is_valid_relative_angle = checkValidRelativeAngle(resampled); s.is_valid_curvature = checkValidCurvature(resampled); + s.is_valid_lateral_acc = checkValidLateralAcceleration(resampled); s.is_valid_steering = checkValidSteering(resampled); s.is_valid_steering_rate = checkValidSteeringRate(resampled);