@@ -52,14 +52,14 @@ IntersectionModule::IntersectionModule(
52
52
const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node,
53
53
const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
54
54
: SceneModuleInterface(module_id, logger, clock),
55
+ planner_param_ (planner_param),
55
56
lane_id_(lane_id),
56
57
associative_ids_(associative_ids),
57
58
turn_direction_(turn_direction),
58
59
has_traffic_light_(has_traffic_light),
59
60
occlusion_uuid_(tier4_autoware_utils::generateUUID())
60
61
{
61
62
velocity_factor_.init (PlanningBehavior::INTERSECTION);
62
- planner_param_ = planner_param;
63
63
64
64
{
65
65
collision_state_machine_.setMarginTime (
@@ -86,8 +86,6 @@ IntersectionModule::IntersectionModule(
86
86
static_occlusion_timeout_state_machine_.setState (StateMachine::State::STOP);
87
87
}
88
88
89
- decision_state_pub_ =
90
- node.create_publisher <std_msgs::msg::String>(" ~/debug/intersection/decision_state" , 1 );
91
89
ego_ttc_pub_ = node.create_publisher <tier4_debug_msgs::msg::Float64MultiArrayStamped>(
92
90
" ~/debug/intersection/ego_ttc" , 1 );
93
91
object_ttc_pub_ = node.create_publisher <tier4_debug_msgs::msg::Float64MultiArrayStamped>(
@@ -107,9 +105,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
107
105
{
108
106
const std::string decision_type = " intersection" + std::to_string (module_id_) + " : " +
109
107
intersection::formatDecisionResult (decision_result);
110
- std_msgs::msg::String decision_result_msg;
111
- decision_result_msg.data = decision_type;
112
- decision_state_pub_->publish (decision_result_msg);
108
+ internal_debug_data_.decision_type = decision_type;
113
109
}
114
110
115
111
prepareRTCStatus (decision_result, *path);
@@ -130,6 +126,25 @@ void IntersectionModule::initializeRTCStatus()
130
126
// activated_ and occlusion_activated_ must be set from manager's RTC callback
131
127
}
132
128
129
+ static std::string formatOcclusionType (const IntersectionModule::OcclusionType & type)
130
+ {
131
+ if (std::holds_alternative<IntersectionModule::NotOccluded>(type)) {
132
+ return " NotOccluded and the best occlusion clearance is " +
133
+ std::to_string (std::get<IntersectionModule::NotOccluded>(type).best_clearance_distance );
134
+ }
135
+ if (std::holds_alternative<IntersectionModule::StaticallyOccluded>(type)) {
136
+ return " StaticallyOccluded and the best occlusion clearance is " +
137
+ std::to_string (
138
+ std::get<IntersectionModule::StaticallyOccluded>(type).best_clearance_distance );
139
+ }
140
+ if (std::holds_alternative<IntersectionModule::DynamicallyOccluded>(type)) {
141
+ return " DynamicallyOccluded and the best occlusion clearance is " +
142
+ std::to_string (
143
+ std::get<IntersectionModule::DynamicallyOccluded>(type).best_clearance_distance );
144
+ }
145
+ return " RTCOccluded" ;
146
+ }
147
+
133
148
intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail (
134
149
PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason)
135
150
{
@@ -239,6 +254,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
239
254
detectCollision (is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line);
240
255
const std::string safety_diag =
241
256
generateDetectionBlameDiagnosis (too_late_detect_objects, misjudge_objects);
257
+ const std::string occlusion_diag = formatOcclusionType (occlusion_status);
258
+
242
259
if (is_permanent_go_) {
243
260
if (has_collision) {
244
261
const auto closest_idx = intersection_stoplines.closest_idx ;
@@ -287,7 +304,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
287
304
isYieldStuckStatus (*path, interpolated_path_info, intersection_stoplines);
288
305
if (is_yield_stuck_status) {
289
306
auto yield_stuck = is_yield_stuck_status.value ();
290
- yield_stuck.safety_report = safety_report ;
307
+ yield_stuck.occlusion_report = occlusion_diag ;
291
308
return yield_stuck;
292
309
}
293
310
@@ -305,12 +322,13 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
305
322
306
323
// Safe
307
324
if (!is_occlusion_state && !has_collision_with_margin) {
308
- return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx};
325
+ return intersection::Safe{
326
+ closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag};
309
327
}
310
328
// Only collision
311
329
if (!is_occlusion_state && has_collision_with_margin) {
312
330
return intersection::NonOccludedCollisionStop{
313
- closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report };
331
+ closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag };
314
332
}
315
333
// Occluded
316
334
// utility functions
@@ -384,7 +402,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
384
402
closest_idx,
385
403
first_attention_stopline_idx,
386
404
occlusion_wo_tl_pass_judge_line_idx,
387
- safety_report };
405
+ occlusion_diag };
388
406
}
389
407
390
408
// ==========================================================================================
@@ -407,7 +425,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
407
425
const bool release_static_occlusion_stuck =
408
426
(static_occlusion_timeout_state_machine_.getState () == StateMachine::State::GO);
409
427
if (!has_collision_with_margin && release_static_occlusion_stuck) {
410
- return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx};
428
+ return intersection::Safe{
429
+ closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag};
411
430
}
412
431
// occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED
413
432
const double max_timeout =
@@ -428,7 +447,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
428
447
first_attention_stopline_idx,
429
448
occlusion_stopline_idx,
430
449
static_occlusion_timeout,
431
- safety_report };
450
+ occlusion_diag };
432
451
} else {
433
452
return intersection::PeekingTowardOcclusion{
434
453
is_occlusion_cleared_with_margin,
@@ -437,15 +456,17 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
437
456
collision_stopline_idx,
438
457
first_attention_stopline_idx,
439
458
occlusion_stopline_idx,
440
- static_occlusion_timeout};
459
+ static_occlusion_timeout,
460
+ occlusion_diag};
441
461
}
442
462
} else {
443
463
const auto occlusion_stopline =
444
464
(planner_param_.occlusion .temporal_stop_before_attention_area || !has_traffic_light_)
445
465
? first_attention_stopline_idx
446
466
: occlusion_stopline_idx;
447
467
return intersection::FirstWaitBeforeOcclusion{
448
- is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline};
468
+ is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline,
469
+ occlusion_diag};
449
470
}
450
471
}
451
472
@@ -1256,6 +1277,7 @@ void IntersectionModule::updateTrafficSignalObservation()
1256
1277
return ;
1257
1278
}
1258
1279
last_tl_valid_observation_ = tl_info_opt.value ();
1280
+ internal_debug_data_.tl_observation = tl_info_opt.value ();
1259
1281
}
1260
1282
1261
1283
IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus (
0 commit comments