Skip to content

Commit 32cc0cb

Browse files
committed
add OberPassJudge state
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 14648ad commit 32cc0cb

8 files changed

+141
-74
lines changed

planning/behavior_velocity_intersection_module/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
1313
src/scene_intersection.cpp
1414
src/intersection_lanelets.cpp
1515
src/object_manager.cpp
16+
src/decision_result.cpp
1617
src/scene_intersection_prepare_data.cpp
1718
src/scene_intersection_stuck.cpp
1819
src/scene_intersection_occlusion.cpp
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
// Copyright 2024 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "decision_result.hpp"
16+
17+
namespace behavior_velocity_planner::intersection
18+
{
19+
std::string formatDecisionResult(const DecisionResult & decision_result)
20+
{
21+
if (std::holds_alternative<InternalError>(decision_result)) {
22+
const auto state = std::get<InternalError>(decision_result);
23+
return "InternalError because " + state.error;
24+
}
25+
if (std::holds_alternative<OverPassJudge>(decision_result)) {
26+
const auto state = std::get<OverPassJudge>(decision_result);
27+
return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" +
28+
state.evasive_report + "\n";
29+
}
30+
if (std::holds_alternative<StuckStop>(decision_result)) {
31+
return "StuckStop";
32+
}
33+
if (std::holds_alternative<YieldStuckStop>(decision_result)) {
34+
return "YieldStuckStop";
35+
}
36+
if (std::holds_alternative<NonOccludedCollisionStop>(decision_result)) {
37+
return "NonOccludedCollisionStop";
38+
}
39+
if (std::holds_alternative<FirstWaitBeforeOcclusion>(decision_result)) {
40+
return "FirstWaitBeforeOcclusion";
41+
}
42+
if (std::holds_alternative<PeekingTowardOcclusion>(decision_result)) {
43+
return "PeekingTowardOcclusion";
44+
}
45+
if (std::holds_alternative<OccludedCollisionStop>(decision_result)) {
46+
return "OccludedCollisionStop";
47+
}
48+
if (std::holds_alternative<OccludedAbsenceTrafficLight>(decision_result)) {
49+
return "OccludedAbsenceTrafficLight";
50+
}
51+
if (std::holds_alternative<Safe>(decision_result)) {
52+
return "Safe";
53+
}
54+
if (std::holds_alternative<FullyPrioritized>(decision_result)) {
55+
return "FullyPrioritized";
56+
}
57+
return "";
58+
}
59+
60+
} // namespace behavior_velocity_planner::intersection

planning/behavior_velocity_intersection_module/src/decision_result.hpp

+18-37
Original file line numberDiff line numberDiff line change
@@ -25,11 +25,20 @@ namespace behavior_velocity_planner::intersection
2525
/**
2626
* @brief internal error
2727
*/
28-
struct Indecisive
28+
struct InternalError
2929
{
3030
std::string error;
3131
};
3232

33+
/**
34+
* @brief
35+
*/
36+
struct OverPassJudge
37+
{
38+
std::string safety_report;
39+
std::string evasive_report;
40+
};
41+
3342
/**
3443
* @brief detected stuck vehicle
3544
*/
@@ -47,6 +56,7 @@ struct YieldStuckStop
4756
{
4857
size_t closest_idx{0};
4958
size_t stuck_stopline_idx{0};
59+
std::string safety_report;
5060
};
5161

5262
/**
@@ -57,6 +67,7 @@ struct NonOccludedCollisionStop
5767
size_t closest_idx{0};
5868
size_t collision_stopline_idx{0};
5969
size_t occlusion_stopline_idx{0};
70+
std::string safety_report;
6071
};
6172

6273
/**
@@ -103,6 +114,7 @@ struct OccludedCollisionStop
103114
//! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it
104115
//! contains the remaining time to release the static occlusion stuck
105116
std::optional<double> static_occlusion_timeout{std::nullopt};
117+
std::string safety_report;
106118
};
107119

108120
/**
@@ -116,6 +128,7 @@ struct OccludedAbsenceTrafficLight
116128
size_t closest_idx{0};
117129
size_t first_attention_area_stopline_idx{0};
118130
size_t peeking_limit_line_idx{0};
131+
std::string safety_report;
119132
};
120133

121134
/**
@@ -137,10 +150,12 @@ struct FullyPrioritized
137150
size_t closest_idx{0};
138151
size_t collision_stopline_idx{0};
139152
size_t occlusion_stopline_idx{0};
153+
std::string safety_report;
140154
};
141155

142156
using DecisionResult = std::variant<
143-
Indecisive, //! internal process error, or over the pass judge line
157+
InternalError, //! internal process error, or over the pass judge line
158+
OverPassJudge, //! over the pass judge lines
144159
StuckStop, //! detected stuck vehicle
145160
YieldStuckStop, //! detected yield stuck vehicle
146161
NonOccludedCollisionStop, //! detected collision while FOV is clear
@@ -152,41 +167,7 @@ using DecisionResult = std::variant<
152167
FullyPrioritized //! only detect vehicles violating traffic rules
153168
>;
154169

155-
inline std::string formatDecisionResult(const DecisionResult & decision_result)
156-
{
157-
if (std::holds_alternative<Indecisive>(decision_result)) {
158-
const auto indecisive = std::get<Indecisive>(decision_result);
159-
return "Indecisive because " + indecisive.error;
160-
}
161-
if (std::holds_alternative<StuckStop>(decision_result)) {
162-
return "StuckStop";
163-
}
164-
if (std::holds_alternative<YieldStuckStop>(decision_result)) {
165-
return "YieldStuckStop";
166-
}
167-
if (std::holds_alternative<NonOccludedCollisionStop>(decision_result)) {
168-
return "NonOccludedCollisionStop";
169-
}
170-
if (std::holds_alternative<FirstWaitBeforeOcclusion>(decision_result)) {
171-
return "FirstWaitBeforeOcclusion";
172-
}
173-
if (std::holds_alternative<PeekingTowardOcclusion>(decision_result)) {
174-
return "PeekingTowardOcclusion";
175-
}
176-
if (std::holds_alternative<OccludedCollisionStop>(decision_result)) {
177-
return "OccludedCollisionStop";
178-
}
179-
if (std::holds_alternative<OccludedAbsenceTrafficLight>(decision_result)) {
180-
return "OccludedAbsenceTrafficLight";
181-
}
182-
if (std::holds_alternative<Safe>(decision_result)) {
183-
return "Safe";
184-
}
185-
if (std::holds_alternative<FullyPrioritized>(decision_result)) {
186-
return "FullyPrioritized";
187-
}
188-
return "";
189-
}
170+
std::string formatDecisionResult(const DecisionResult & decision_result);
190171

191172
} // namespace behavior_velocity_planner::intersection
192173

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

+49-24
Original file line numberDiff line numberDiff line change
@@ -167,18 +167,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
167167
// ego path has just entered the entry of this intersection
168168
// ==========================================================================================
169169
if (!intersection_lanelets.first_attention_area()) {
170-
return intersection::Indecisive{"attention area is empty"};
170+
return intersection::InternalError{"attention area is empty"};
171171
}
172172
const auto first_attention_area = intersection_lanelets.first_attention_area().value();
173173
const auto default_stopline_idx_opt = intersection_stoplines.default_stopline;
174174
if (!default_stopline_idx_opt) {
175-
return intersection::Indecisive{"default stop line is null"};
175+
return intersection::InternalError{"default stop line is null"};
176176
}
177177
const auto default_stopline_idx = default_stopline_idx_opt.value();
178178
const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline;
179179
const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline;
180180
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"};
182182
}
183183
const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value();
184184
const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value();
@@ -228,25 +228,20 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
228228
detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line);
229229
if (is_permanent_go_) {
230230
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"};
235232
}
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"};
237235
}
238-
/*
236+
237+
std::string safety_report{""};
239238
const bool collision_on_1st_attention_lane =
240239
has_collision && collision_position == intersection::CollisionInterval::LanePosition::FIRST;
241240
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";
248244
}
249-
*/
250245

251246
const auto closest_idx = intersection_stoplines.closest_idx;
252247
const bool is_over_default_stopline = util::isOverTargetIndex(
@@ -268,7 +263,9 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
268263
const auto is_yield_stuck_status =
269264
isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines);
270265
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;
272269
}
273270

274271
collision_state_machine_.setStateWithMarginTime(
@@ -279,7 +276,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
279276

280277
if (is_prioritized) {
281278
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};
283281
}
284282

285283
// Safe
@@ -289,7 +287,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
289287
// Only collision
290288
if (!is_occlusion_state && has_collision_with_margin) {
291289
return intersection::NonOccludedCollisionStop{
292-
closest_idx, collision_stopline_idx, occlusion_stopline_idx};
290+
closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report};
293291
}
294292
// Occluded
295293
// utility functions
@@ -343,7 +341,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
343341
: false;
344342
if (!has_traffic_light_) {
345343
if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) {
346-
return intersection::Indecisive{
344+
return intersection::InternalError{
347345
"already passed maximum peeking line in the absence of traffic light"};
348346
}
349347
return intersection::OccludedAbsenceTrafficLight{
@@ -352,7 +350,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
352350
temporal_stop_before_attention_required,
353351
closest_idx,
354352
first_attention_stopline_idx,
355-
occlusion_wo_tl_pass_judge_line_idx};
353+
occlusion_wo_tl_pass_judge_line_idx,
354+
safety_report};
356355
}
357356

358357
// ==========================================================================================
@@ -395,7 +394,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
395394
collision_stopline_idx,
396395
first_attention_stopline_idx,
397396
occlusion_stopline_idx,
398-
static_occlusion_timeout};
397+
static_occlusion_timeout,
398+
safety_report};
399399
} else {
400400
return intersection::PeekingTowardOcclusion{
401401
is_occlusion_cleared_with_margin,
@@ -438,7 +438,17 @@ void prepareRTCByDecisionResult(
438438

439439
template <>
440440
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,
442452
[[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
443453
[[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance,
444454
[[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance)
@@ -646,7 +656,22 @@ template <>
646656
void reactRTCApprovalByDecisionResult(
647657
[[maybe_unused]] const bool rtc_default_approved,
648658
[[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,
650675
[[maybe_unused]] const IntersectionModule::PlannerParam & planner_param,
651676
[[maybe_unused]] const double baselink2front,
652677
[[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -336,7 +336,7 @@ class IntersectionModule : public SceneModuleInterface
336336
bool is_permanent_go_{false};
337337

338338
//! for checking if ego is over the pass judge lines because previously the situation was SAFE
339-
intersection::DecisionResult prev_decision_result_{intersection::Indecisive{""}};
339+
intersection::DecisionResult prev_decision_result_{intersection::InternalError{""}};
340340

341341
//! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line
342342
//! is treated as the same position as occlusion_peeking_stopline
@@ -461,13 +461,13 @@ class IntersectionModule : public SceneModuleInterface
461461

462462
/**
463463
* @brief prepare basic data structure
464-
* @return return IntersectionStopLines if all data is valid, otherwise Indecisive
464+
* @return return IntersectionStopLines if all data is valid, otherwise InternalError
465465
* @note if successful, it is ensure that intersection_lanelets_,
466466
* intersection_lanelets.first_conflicting_lane are not null
467467
*
468468
* To simplify modifyPathVelocityDetail(), this function is used at first
469469
*/
470-
intersection::Result<BasicData, intersection::Indecisive> prepareIntersectionData(
470+
intersection::Result<BasicData, intersection::InternalError> prepareIntersectionData(
471471
const bool is_prioritized, PathWithLaneId * path);
472472

473473
/**
@@ -625,7 +625,7 @@ class IntersectionModule : public SceneModuleInterface
625625
*/
626626
/**
627627
* @brief check if ego is already over the pass judge line
628-
* @return if ego is over both 1st/2nd pass judge lines, return Indecisive, else return
628+
* @return if ego is over both 1st/2nd pass judge lines, return InternalError, else return
629629
* (is_over_1st_pass_judge, is_over_2nd_pass_judge)
630630
* @attention this function has access to value() of intersection_stoplines.default_stopline,
631631
* intersection_stoplines.occlusion_stopline

planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -361,7 +361,7 @@ IntersectionModule::isGreenPseudoCollisionStatus(
361361
if (exist_close_vehicles) {
362362
const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value();
363363
return intersection::NonOccludedCollisionStop{
364-
closest_idx, collision_stopline_idx, occlusion_stopline_idx};
364+
closest_idx, collision_stopline_idx, occlusion_stopline_idx, std::string("")};
365365
}
366366
}
367367
return std::nullopt;

0 commit comments

Comments
 (0)