fix(lane_change): fixed debug visualization bug (autowarefoundation#5284

)

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

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;