@@ -167,18 +167,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
167
167
// ego path has just entered the entry of this intersection
168
168
// ==========================================================================================
169
169
if (!intersection_lanelets.first_attention_area ()) {
170
- return intersection::Indecisive {" attention area is empty" };
170
+ return intersection::InternalError {" attention area is empty" };
171
171
}
172
172
const auto first_attention_area = intersection_lanelets.first_attention_area ().value ();
173
173
const auto default_stopline_idx_opt = intersection_stoplines.default_stopline ;
174
174
if (!default_stopline_idx_opt) {
175
- return intersection::Indecisive {" default stop line is null" };
175
+ return intersection::InternalError {" default stop line is null" };
176
176
}
177
177
const auto default_stopline_idx = default_stopline_idx_opt.value ();
178
178
const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline ;
179
179
const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline ;
180
180
if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) {
181
- return intersection::Indecisive {" occlusion stop line is null" };
181
+ return intersection::InternalError {" occlusion stop line is null" };
182
182
}
183
183
const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value ();
184
184
const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value ();
@@ -228,25 +228,20 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
228
228
detectCollision (is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line);
229
229
if (is_permanent_go_) {
230
230
if (has_collision) {
231
- // TODO(Mamoru Sobue): diagnosis
232
- return intersection::Indecisive{
233
- " ego is over the pass judge lines and collision is detected. need acceleration to keep "
234
- " safe." };
231
+ return intersection::OverPassJudge{" TODO" , " ego needs acceleration to keep safe" };
235
232
}
236
- return intersection::Indecisive{" over pass judge lines and collision is not detected" };
233
+ return intersection::OverPassJudge{
234
+ " no collision is detected" , " ego can safely pass the intersection at this rate" };
237
235
}
238
- /*
236
+
237
+ std::string safety_report{" " };
239
238
const bool collision_on_1st_attention_lane =
240
239
has_collision && collision_position == intersection::CollisionInterval::LanePosition::FIRST;
241
240
if (
242
- is_over_1st_pass_judge_line && !is_over_2nd_pass_judge_line &&
243
- collision_on_1st_attention_lane) {
244
- // TODO(Mamoru Sobue): diagnosis
245
- return intersection::Indecisive{
246
- "ego is already over the 1st pass judge line although still before the 2nd pass judge line, "
247
- "but collision is detected on the first attention lane"};
241
+ is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line &&
242
+ is_over_2nd_pass_judge_line.value () && collision_on_1st_attention_lane) {
243
+ safety_report = " TODO" ;
248
244
}
249
- */
250
245
251
246
const auto closest_idx = intersection_stoplines.closest_idx ;
252
247
const bool is_over_default_stopline = util::isOverTargetIndex (
@@ -268,7 +263,9 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
268
263
const auto is_yield_stuck_status =
269
264
isYieldStuckStatus (*path, interpolated_path_info, intersection_stoplines);
270
265
if (is_yield_stuck_status) {
271
- return is_yield_stuck_status.value ();
266
+ auto yield_stuck = is_yield_stuck_status.value ();
267
+ yield_stuck.safety_report = safety_report;
268
+ return yield_stuck;
272
269
}
273
270
274
271
collision_state_machine_.setStateWithMarginTime (
@@ -279,7 +276,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
279
276
280
277
if (is_prioritized) {
281
278
return intersection::FullyPrioritized{
282
- has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx};
279
+ has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx,
280
+ safety_report};
283
281
}
284
282
285
283
// Safe
@@ -289,7 +287,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
289
287
// Only collision
290
288
if (!is_occlusion_state && has_collision_with_margin) {
291
289
return intersection::NonOccludedCollisionStop{
292
- closest_idx, collision_stopline_idx, occlusion_stopline_idx};
290
+ closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report };
293
291
}
294
292
// Occluded
295
293
// utility functions
@@ -343,7 +341,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
343
341
: false ;
344
342
if (!has_traffic_light_) {
345
343
if (fromEgoDist (occlusion_wo_tl_pass_judge_line_idx) < 0 ) {
346
- return intersection::Indecisive {
344
+ return intersection::InternalError {
347
345
" already passed maximum peeking line in the absence of traffic light" };
348
346
}
349
347
return intersection::OccludedAbsenceTrafficLight{
@@ -352,7 +350,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
352
350
temporal_stop_before_attention_required,
353
351
closest_idx,
354
352
first_attention_stopline_idx,
355
- occlusion_wo_tl_pass_judge_line_idx};
353
+ occlusion_wo_tl_pass_judge_line_idx,
354
+ safety_report};
356
355
}
357
356
358
357
// ==========================================================================================
@@ -395,7 +394,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
395
394
collision_stopline_idx,
396
395
first_attention_stopline_idx,
397
396
occlusion_stopline_idx,
398
- static_occlusion_timeout};
397
+ static_occlusion_timeout,
398
+ safety_report};
399
399
} else {
400
400
return intersection::PeekingTowardOcclusion{
401
401
is_occlusion_cleared_with_margin,
@@ -438,7 +438,17 @@ void prepareRTCByDecisionResult(
438
438
439
439
template <>
440
440
void prepareRTCByDecisionResult (
441
- [[maybe_unused]] const intersection::Indecisive & result,
441
+ [[maybe_unused]] const intersection::InternalError & result,
442
+ [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
443
+ [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance,
444
+ [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance)
445
+ {
446
+ return ;
447
+ }
448
+
449
+ template <>
450
+ void prepareRTCByDecisionResult (
451
+ [[maybe_unused]] const intersection::OverPassJudge & result,
442
452
[[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
443
453
[[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance,
444
454
[[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance)
@@ -646,7 +656,22 @@ template <>
646
656
void reactRTCApprovalByDecisionResult (
647
657
[[maybe_unused]] const bool rtc_default_approved,
648
658
[[maybe_unused]] const bool rtc_occlusion_approved,
649
- [[maybe_unused]] const intersection::Indecisive & decision_result,
659
+ [[maybe_unused]] const intersection::InternalError & decision_result,
660
+ [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param,
661
+ [[maybe_unused]] const double baselink2front,
662
+ [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
663
+ [[maybe_unused]] StopReason * stop_reason,
664
+ [[maybe_unused]] VelocityFactorInterface * velocity_factor,
665
+ [[maybe_unused]] IntersectionModule::DebugData * debug_data)
666
+ {
667
+ return ;
668
+ }
669
+
670
+ template <>
671
+ void reactRTCApprovalByDecisionResult (
672
+ [[maybe_unused]] const bool rtc_default_approved,
673
+ [[maybe_unused]] const bool rtc_occlusion_approved,
674
+ [[maybe_unused]] const intersection::OverPassJudge & decision_result,
650
675
[[maybe_unused]] const IntersectionModule::PlannerParam & planner_param,
651
676
[[maybe_unused]] const double baselink2front,
652
677
[[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
0 commit comments