diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 9ff638cb61876..28741344bdcea 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/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), diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 51d657e949ae6..12183353f2e98 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/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 diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 5c6d96cc0f045..555a72efe33b7 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/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> @@ -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 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);