Skip to content

Commit 6c3dd99

Browse files
committed
feat(intersection): add traffic signal info publisher and detailed occlusion diagnosis (autowarefoundation#6491)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent a202cab commit 6c3dd99

File tree

7 files changed

+105
-31
lines changed

7 files changed

+105
-31
lines changed

planning/behavior_velocity_intersection_module/src/decision_result.cpp

+12-9
Original file line numberDiff line numberDiff line change
@@ -24,36 +24,39 @@ std::string formatDecisionResult(const DecisionResult & decision_result)
2424
}
2525
if (std::holds_alternative<OverPassJudge>(decision_result)) {
2626
const auto & state = std::get<OverPassJudge>(decision_result);
27-
return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" +
28-
state.evasive_report;
27+
return "OverPassJudge:\nsafety_report:" + state.safety_report +
28+
"\nevasive_report:" + state.evasive_report;
2929
}
3030
if (std::holds_alternative<StuckStop>(decision_result)) {
3131
return "StuckStop";
3232
}
3333
if (std::holds_alternative<YieldStuckStop>(decision_result)) {
3434
const auto & state = std::get<YieldStuckStop>(decision_result);
35-
return "YieldStuckStop:\nsafety_report:" + state.safety_report;
35+
return "YieldStuckStop:\nocclusion_report:" + state.occlusion_report;
3636
}
3737
if (std::holds_alternative<NonOccludedCollisionStop>(decision_result)) {
3838
const auto & state = std::get<NonOccludedCollisionStop>(decision_result);
39-
return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report;
39+
return "NonOccludedCollisionStop:\nocclusion_report:" + state.occlusion_report;
4040
}
4141
if (std::holds_alternative<FirstWaitBeforeOcclusion>(decision_result)) {
42-
return "FirstWaitBeforeOcclusion";
42+
const auto & state = std::get<FirstWaitBeforeOcclusion>(decision_result);
43+
return "FirstWaitBeforeOcclusion:\nocclusion_report:" + state.occlusion_report;
4344
}
4445
if (std::holds_alternative<PeekingTowardOcclusion>(decision_result)) {
45-
return "PeekingTowardOcclusion";
46+
const auto & state = std::get<PeekingTowardOcclusion>(decision_result);
47+
return "PeekingTowardOcclusion:\nocclusion_report:" + state.occlusion_report;
4648
}
4749
if (std::holds_alternative<OccludedCollisionStop>(decision_result)) {
4850
const auto & state = std::get<OccludedCollisionStop>(decision_result);
49-
return "OccludedCollisionStop\nsafety_report:" + state.safety_report;
51+
return "OccludedCollisionStop:\nocclusion_report:" + state.occlusion_report;
5052
}
5153
if (std::holds_alternative<OccludedAbsenceTrafficLight>(decision_result)) {
5254
const auto & state = std::get<OccludedAbsenceTrafficLight>(decision_result);
53-
return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report;
55+
return "OccludedAbsenceTrafficLight:\nocclusion_report:" + state.occlusion_report;
5456
}
5557
if (std::holds_alternative<Safe>(decision_result)) {
56-
return "Safe";
58+
const auto & state = std::get<Safe>(decision_result);
59+
return "Safe:\nocclusion_report:" + state.occlusion_report;
5760
}
5861
if (std::holds_alternative<FullyPrioritized>(decision_result)) {
5962
const auto & state = std::get<FullyPrioritized>(decision_result);

planning/behavior_velocity_intersection_module/src/decision_result.hpp

+8-5
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ struct YieldStuckStop
5656
{
5757
size_t closest_idx{0};
5858
size_t stuck_stopline_idx{0};
59-
std::string safety_report;
59+
std::string occlusion_report;
6060
};
6161

6262
/**
@@ -67,7 +67,7 @@ struct NonOccludedCollisionStop
6767
size_t closest_idx{0};
6868
size_t collision_stopline_idx{0};
6969
size_t occlusion_stopline_idx{0};
70-
std::string safety_report;
70+
std::string occlusion_report;
7171
};
7272

7373
/**
@@ -79,6 +79,7 @@ struct FirstWaitBeforeOcclusion
7979
size_t closest_idx{0};
8080
size_t first_stopline_idx{0};
8181
size_t occlusion_stopline_idx{0};
82+
std::string occlusion_report;
8283
};
8384

8485
/**
@@ -98,6 +99,7 @@ struct PeekingTowardOcclusion
9899
//! contains the remaining time to release the static occlusion stuck and shows up
99100
//! intersection_occlusion(x.y)
100101
std::optional<double> static_occlusion_timeout{std::nullopt};
102+
std::string occlusion_report;
101103
};
102104

103105
/**
@@ -114,7 +116,7 @@ struct OccludedCollisionStop
114116
//! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it
115117
//! contains the remaining time to release the static occlusion stuck
116118
std::optional<double> static_occlusion_timeout{std::nullopt};
117-
std::string safety_report;
119+
std::string occlusion_report;
118120
};
119121

120122
/**
@@ -128,7 +130,7 @@ struct OccludedAbsenceTrafficLight
128130
size_t closest_idx{0};
129131
size_t first_attention_area_stopline_idx{0};
130132
size_t peeking_limit_line_idx{0};
131-
std::string safety_report;
133+
std::string occlusion_report;
132134
};
133135

134136
/**
@@ -139,6 +141,7 @@ struct Safe
139141
size_t closest_idx{0};
140142
size_t collision_stopline_idx{0};
141143
size_t occlusion_stopline_idx{0};
144+
std::string occlusion_report;
142145
};
143146

144147
/**
@@ -154,7 +157,7 @@ struct FullyPrioritized
154157
};
155158

156159
using DecisionResult = std::variant<
157-
InternalError, //! internal process error, or over the pass judge line
160+
InternalError, //! internal process error
158161
OverPassJudge, //! over the pass judge lines
159162
StuckStop, //! detected stuck vehicle
160163
YieldStuckStop, //! detected yield stuck vehicle

planning/behavior_velocity_intersection_module/src/manager.cpp

+27
Original file line numberDiff line numberDiff line change
@@ -294,6 +294,11 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
294294
}
295295

296296
ip.debug.ttc = getOrDeclareParameter<std::vector<int64_t>>(node, ns + ".debug.ttc");
297+
298+
decision_state_pub_ =
299+
node.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
300+
tl_observation_pub_ = node.create_publisher<autoware_perception_msgs::msg::TrafficSignal>(
301+
"~/debug/intersection_traffic_signal", 1);
297302
}
298303

299304
void IntersectionModuleManager::launchNewModules(
@@ -390,6 +395,10 @@ bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegister
390395

391396
void IntersectionModuleManager::sendRTC(const Time & stamp)
392397
{
398+
double min_distance = std::numeric_limits<double>::infinity();
399+
std::optional<TrafficSignalStamped> nearest_tl_observation{std::nullopt};
400+
std_msgs::msg::String decision_type;
401+
393402
for (const auto & scene_module : scene_modules_) {
394403
const auto intersection_module = std::dynamic_pointer_cast<IntersectionModule>(scene_module);
395404
const UUID uuid = getUUID(scene_module->getModuleId());
@@ -401,9 +410,27 @@ void IntersectionModuleManager::sendRTC(const Time & stamp)
401410
const auto occlusion_safety = intersection_module->getOcclusionSafety();
402411
occlusion_rtc_interface_.updateCooperateStatus(
403412
occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp);
413+
414+
// ==========================================================================================
415+
// module debug data
416+
// ==========================================================================================
417+
const auto internal_debug_data = intersection_module->getInternalDebugData();
418+
if (internal_debug_data.distance < min_distance) {
419+
min_distance = internal_debug_data.distance;
420+
nearest_tl_observation = internal_debug_data.tl_observation;
421+
}
422+
decision_type.data += (internal_debug_data.decision_type + "\n");
404423
}
405424
rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus()
406425
occlusion_rtc_interface_.publishCooperateStatus(stamp);
426+
427+
// ==========================================================================================
428+
// publish module debug data
429+
// ==========================================================================================
430+
decision_state_pub_->publish(decision_type);
431+
if (nearest_tl_observation) {
432+
tl_observation_pub_->publish(nearest_tl_observation.value().signal);
433+
}
407434
}
408435

409436
void IntersectionModuleManager::setActivation()

planning/behavior_velocity_intersection_module/src/manager.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
#include <rclcpp/rclcpp.hpp>
2525

2626
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
27+
#include <std_msgs/msg/string.hpp>
2728
#include <tier4_api_msgs/msg/intersection_status.hpp>
2829

2930
#include <functional>
@@ -57,6 +58,9 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC
5758
void setActivation() override;
5859
/* called from SceneModuleInterface::updateSceneModuleInstances */
5960
void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
61+
62+
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr decision_state_pub_;
63+
rclcpp::Publisher<autoware_perception_msgs::msg::TrafficSignal>::SharedPtr tl_observation_pub_;
6064
};
6165

6266
class MergeFromPrivateModuleManager : public SceneModuleManagerInterface

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

+36-14
Original file line numberDiff line numberDiff line change
@@ -52,14 +52,14 @@ IntersectionModule::IntersectionModule(
5252
const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node,
5353
const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
5454
: SceneModuleInterface(module_id, logger, clock),
55+
planner_param_(planner_param),
5556
lane_id_(lane_id),
5657
associative_ids_(associative_ids),
5758
turn_direction_(turn_direction),
5859
has_traffic_light_(has_traffic_light),
5960
occlusion_uuid_(tier4_autoware_utils::generateUUID())
6061
{
6162
velocity_factor_.init(PlanningBehavior::INTERSECTION);
62-
planner_param_ = planner_param;
6363

6464
{
6565
collision_state_machine_.setMarginTime(
@@ -86,8 +86,6 @@ IntersectionModule::IntersectionModule(
8686
static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP);
8787
}
8888

89-
decision_state_pub_ =
90-
node.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
9189
ego_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
9290
"~/debug/intersection/ego_ttc", 1);
9391
object_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
@@ -107,9 +105,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
107105
{
108106
const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " +
109107
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;
113109
}
114110

115111
prepareRTCStatus(decision_result, *path);
@@ -130,6 +126,25 @@ void IntersectionModule::initializeRTCStatus()
130126
// activated_ and occlusion_activated_ must be set from manager's RTC callback
131127
}
132128

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+
133148
intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
134149
PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason)
135150
{
@@ -239,6 +254,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
239254
detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line);
240255
const std::string safety_diag =
241256
generateDetectionBlameDiagnosis(too_late_detect_objects, misjudge_objects);
257+
const std::string occlusion_diag = formatOcclusionType(occlusion_status);
258+
242259
if (is_permanent_go_) {
243260
if (has_collision) {
244261
const auto closest_idx = intersection_stoplines.closest_idx;
@@ -287,7 +304,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
287304
isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines);
288305
if (is_yield_stuck_status) {
289306
auto yield_stuck = is_yield_stuck_status.value();
290-
yield_stuck.safety_report = safety_report;
307+
yield_stuck.occlusion_report = occlusion_diag;
291308
return yield_stuck;
292309
}
293310

@@ -305,12 +322,13 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
305322

306323
// Safe
307324
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};
309327
}
310328
// Only collision
311329
if (!is_occlusion_state && has_collision_with_margin) {
312330
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};
314332
}
315333
// Occluded
316334
// utility functions
@@ -384,7 +402,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
384402
closest_idx,
385403
first_attention_stopline_idx,
386404
occlusion_wo_tl_pass_judge_line_idx,
387-
safety_report};
405+
occlusion_diag};
388406
}
389407

390408
// ==========================================================================================
@@ -407,7 +425,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
407425
const bool release_static_occlusion_stuck =
408426
(static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO);
409427
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};
411430
}
412431
// occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED
413432
const double max_timeout =
@@ -428,7 +447,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
428447
first_attention_stopline_idx,
429448
occlusion_stopline_idx,
430449
static_occlusion_timeout,
431-
safety_report};
450+
occlusion_diag};
432451
} else {
433452
return intersection::PeekingTowardOcclusion{
434453
is_occlusion_cleared_with_margin,
@@ -437,15 +456,17 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
437456
collision_stopline_idx,
438457
first_attention_stopline_idx,
439458
occlusion_stopline_idx,
440-
static_occlusion_timeout};
459+
static_occlusion_timeout,
460+
occlusion_diag};
441461
}
442462
} else {
443463
const auto occlusion_stopline =
444464
(planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_)
445465
? first_attention_stopline_idx
446466
: occlusion_stopline_idx;
447467
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};
449470
}
450471
}
451472

@@ -1256,6 +1277,7 @@ void IntersectionModule::updateTrafficSignalObservation()
12561277
return;
12571278
}
12581279
last_tl_valid_observation_ = tl_info_opt.value();
1280+
internal_debug_data_.tl_observation = tl_info_opt.value();
12591281
}
12601282

12611283
IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus(

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

+12-3
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,6 @@
2828
#include <rclcpp/rclcpp.hpp>
2929

3030
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
31-
#include <std_msgs/msg/string.hpp>
3231
#include <tier4_debug_msgs/msg/float64_multi_array_stamped.hpp>
3332

3433
#include <lanelet2_core/Forward.h>
@@ -242,6 +241,13 @@ class IntersectionModule : public SceneModuleInterface
242241
traffic_light_observation{std::nullopt};
243242
};
244243

244+
struct InternalDebugData
245+
{
246+
double distance{0.0};
247+
std::string decision_type{};
248+
std::optional<TrafficSignalStamped> tl_observation{std::nullopt};
249+
};
250+
245251
using TimeDistanceArray = std::vector<std::pair<double /* time*/, double /* distance*/>>;
246252

247253
/**
@@ -326,6 +332,7 @@ class IntersectionModule : public SceneModuleInterface
326332
double getOcclusionDistance() const { return occlusion_stop_distance_; }
327333
void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; }
328334
bool isOcclusionFirstStopRequired() const { return occlusion_first_stop_required_; }
335+
InternalDebugData & getInternalDebugData() const { return internal_debug_data_; }
329336

330337
private:
331338
/**
@@ -336,6 +343,9 @@ class IntersectionModule : public SceneModuleInterface
336343
* following variables are unique to this intersection lanelet or to this module
337344
* @{
338345
*/
346+
347+
const PlannerParam planner_param_;
348+
339349
//! lanelet of this intersection
340350
const lanelet::Id lane_id_;
341351

@@ -361,7 +371,6 @@ class IntersectionModule : public SceneModuleInterface
361371
* following variables are immutable once initialized
362372
* @{
363373
*/
364-
PlannerParam planner_param_;
365374

366375
//! cache IntersectionLanelets struct
367376
std::optional<intersection::IntersectionLanelets> intersection_lanelets_{std::nullopt};
@@ -807,7 +816,7 @@ class IntersectionModule : public SceneModuleInterface
807816
/** @} */
808817

809818
mutable DebugData debug_data_;
810-
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr decision_state_pub_;
819+
mutable InternalDebugData internal_debug_data_{};
811820
rclcpp::Publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr ego_ttc_pub_;
812821
rclcpp::Publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr object_ttc_pub_;
813822
};

0 commit comments

Comments
 (0)