From dad4722a309cca23304bf449cd7a266db942efb9 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 16 Oct 2023 18:11:07 +0900 Subject: [PATCH] fix(lane_change): do not use reference values of polygon outer (#5318) * fix(lane_change): do not use refelence values of polygon outer Signed-off-by: Fumiya Watanabe * fix(lane_change): fix in lane change Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../src/scene_module/lane_change/normal.cpp | 3 ++- .../src/utils/path_safety_checker/safety_check.cpp | 9 ++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 1012c228e8607..769b0d681d2ac 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -835,7 +835,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // calc distance from the current ego position double max_dist_ego_to_obj = std::numeric_limits::lowest(); double min_dist_ego_to_obj = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & polygon_p : obj_polygon_outer) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const double dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index e4698e98d46eb..e36115c62e148 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -51,7 +51,8 @@ bool isTargetObjectFront( tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); // check all edges in the polygon - for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & obj_edge : obj_polygon_outer) { const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { return true; @@ -70,7 +71,8 @@ bool isTargetObjectFront( tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; // check all edges in the polygon - for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & obj_edge : obj_polygon_outer) { const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); if (motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { return true; @@ -134,7 +136,8 @@ Polygon2d createExtendedPolygon( double min_x = std::numeric_limits::max(); double max_y = std::numeric_limits::lowest(); double min_y = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & polygon_p : obj_polygon_outer) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto transformed_p = tier4_autoware_utils::inverseTransformPoint(obj_p, obj_pose);