From 7a0bdd239e7caa3705f0d1d921c0e981099b085d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 12 Oct 2023 17:22:10 +0900 Subject: [PATCH] fix(lane_change): fixed debug visualization bug (#5284) Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 10 ++++++---- 1 file changed, 6 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 fbe857e1040f4..931369b4fc929 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 @@ -1556,12 +1556,14 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); + auto is_safe = true; for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, current_debug_data.second); if (collided_polygons.empty()) { + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); continue; } @@ -1571,20 +1573,20 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); if (!collision_in_current_lanes && !collision_in_target_lanes) { + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); continue; } + is_safe = false; path_safety_status.is_safe = false; - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); const auto & obj_pose = obj.initial_pose.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); path_safety_status.is_object_coming_from_rear |= !utils::path_safety_checker::isTargetObjectFront( path, current_pose, common_parameters.vehicle_info, obj_polygon); } - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); } return path_safety_status;