fix

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -233,6 +233,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0),
     &debug_marker_array, now);
 
+  appendMarkerArray(
+    debug::createObjectsMarkerArray(
+      debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0),
+    &debug_marker_array, now);
+
   appendMarkerArray(
     debug::createObjectsMarkerArray(
       debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2),

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1356,12 +1356,14 @@ bool IntersectionModule::checkCollision(
       return false;
     }
     const double dist_to_stop_line = target_object.dist_to_stop_line.value();
+    if (dist_to_stop_line < 0) {
+      return false;
+    }
     const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x;
     const double braking_distance =
       v * v /
       (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light
                          .object_expected_deceleration));
-    return braking_distance < dist_to_stop_line;
   };
 
   // check collision between predicted_path and ego_area

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -570,7 +570,7 @@ static std::string getTurnDirection(lanelet::ConstLanelet lane)
 }
 
 /**
- * @param pair lanelets and the vector of original laneles in topological order (not reversed as
+ * @param pair lanelets and the vector of original lanelets in topological order (not reversed as
  *in generateDetectionLaneDivisions())
  **/
 static std::pair<lanelet::ConstLanelets, std::vector<lanelet::ConstLanelets>>
@@ -805,12 +805,12 @@ IntersectionLanelets getObjectiveLanelets(
     occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll);
   }
 
-  auto [attention_lanelets, original_attention_lanelet_seqs] =
+  auto [attention_lanelets, original_attention_lanelet_sequences] =
     mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr);
 
   IntersectionLanelets result;
   result.attention_ = std::move(attention_lanelets);
-  for (const auto & original_attention_lanelet_seq : original_attention_lanelet_seqs) {
+  for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) {
     // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that
     // back() exists.
     std::optional<lanelet::ConstLineString3d> stop_line{std::nullopt};
@@ -835,7 +835,7 @@ IntersectionLanelets getObjectiveLanelets(
   result.conflicting_ = std::move(conflicting_ex_ego_lanelets);
   result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids);
   // NOTE: occlusion_attention is not inverted here
-  // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occulusion lanelets as well and
+  // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and
   // then trim part of them based on curvature threshold
   result.occlusion_attention_ =
     std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction);