From 28db8ed0a4d510b7895d3a1783c36286cf6dd42b Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 27 Dec 2023 17:05:34 +0900 Subject: [PATCH] fix --- planning/behavior_path_goal_planner_module/src/util.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index cc78aa2c68ae2..21d8e3b959766 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -217,11 +217,11 @@ bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_h double calcLateralDeviationBetweenPaths( const PathWithLaneId & reference_path, const PathWithLaneId & target_path) { - double lateral_deviation = std::numeric_limits::max(); + double lateral_deviation = 0.0; for (const auto & target_point : target_path.points) { const size_t nearest_index = motion_utils::findNearestIndex(reference_path.points, target_point.point.pose.position); - lateral_deviation = std::min( + lateral_deviation = std::max( lateral_deviation, std::abs(tier4_autoware_utils::calcLateralDeviation( reference_path.points[nearest_index].point.pose, target_point.point.pose.position)));