From 34f0c19e6d0341a9baa5744072e9b23bc15a3ca0 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 15 Jan 2024 10:27:09 +0900 Subject: [PATCH] fix(static_drivable_area_expansion): rename and fix return value consistency Signed-off-by: satoshi-ota --- .../static_drivable_area.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index b451d3b64ad33..03fae6864fe50 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -208,27 +208,25 @@ std::optional> intersectBound( return std::nullopt; } -double calcDistanceFromPointToSegment( +double calcSquaredDistanceFromPointToSegment( const geometry_msgs::msg::Point & segment_start_point, const geometry_msgs::msg::Point & segment_end_point, const geometry_msgs::msg::Point & target_point) { + using tier4_autoware_utils::calcSquaredDistance2d; + const auto & a = segment_start_point; const auto & b = segment_end_point; const auto & p = target_point; const double dot_val = (b.x - a.x) * (p.x - a.x) + (b.y - a.y) * (p.y - a.y); - const double squared_segment_length = tier4_autoware_utils::calcSquaredDistance2d(a, b); + const double squared_segment_length = calcSquaredDistance2d(a, b); if (0 <= dot_val && dot_val <= squared_segment_length) { - const double numerator = std::abs((p.x - a.x) * (a.y - b.y) - (p.y - a.y) * (a.x - b.x)); - const double denominator = std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2)); - return numerator / denominator; + return calcSquaredDistance2d(p, a) - dot_val * dot_val / squared_segment_length; } // target_point is outside the segment. - return std::min( - tier4_autoware_utils::calcSquaredDistance2d(a, p), - tier4_autoware_utils::calcSquaredDistance2d(b, p)); + return std::min(calcSquaredDistance2d(a, p), calcSquaredDistance2d(b, p)); } PolygonPoint transformBoundFrenetCoordinate( @@ -239,8 +237,8 @@ PolygonPoint transformBoundFrenetCoordinate( // find wrong nearest index. std::vector dist_to_bound_segment_vec; for (size_t i = 0; i < bound_points.size() - 1; ++i) { - const double dist_to_bound_segment = - calcDistanceFromPointToSegment(bound_points.at(i), bound_points.at(i + 1), target_point); + const double dist_to_bound_segment = calcSquaredDistanceFromPointToSegment( + bound_points.at(i), bound_points.at(i + 1), target_point); dist_to_bound_segment_vec.push_back(dist_to_bound_segment); }