Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(intersection): disable peeking while collision is detected #3765

Merged
merged 9 commits into from
May 23, 2023
Merged

feat(intersection): disable peeking while collision is detected #3765

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
04da127
feat(intersection): disable peeking while collision is detected
soblin May 18, 2023
1affab9
preempt PR3711 (#11)
soblin May 19, 2023
53f4819
fixed !has_collision
soblin May 19, 2023
4220df6
Preempt PR3711(2) (#13)
soblin May 22, 2023
fd227aa
added COLLISION_DETECTED state
soblin May 22, 2023
83a03a9
po
soblin May 22, 2023
7fea28c
Revert "Preempt PR3711(2) (#13)"
soblin May 22, 2023
d0d88c6
Revert "preempt PR3711 (#11)"
soblin May 22, 2023
9f6dfce
call setSafe() regardless of occlusion_stop_required
soblin May 22, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 25 additions & 16 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -303,24 +303,34 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
is_actually_occluded_ = !is_occlusion_cleared;
is_forcefully_occluded_ = ext_occlusion_requested;
if (!is_occlusion_cleared || ext_occlusion_requested) {
const bool approached_stop_line =
(std::fabs(dist_1st_stopline) < planner_param_.common.stop_overshoot_margin);
const bool is_stopped = planner_data_->isVehicleStopped();
if (!default_stop_line_idx_opt) {
RCLCPP_DEBUG(logger_, "occlusion is detected but default stop line is not set or generated");
RCLCPP_DEBUG(logger_, "===== plan end =====");
return true;
}
if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
occlusion_stop_required = true;
occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt;

// clear first stop line
// insert creep velocity [closest_idx, occlusion_stop_line)
insert_creep_during_occlusion =
std::make_pair(closest_idx, occlusion_peeking_line_idx_opt.value());
occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE;
} else if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
if (!has_collision) {
occlusion_stop_required = true;
occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt;
// clear first stop line
// insert creep velocity [closest_idx, occlusion_stop_line)
insert_creep_during_occlusion =
std::make_pair(closest_idx, occlusion_peeking_line_idx_opt.value());
occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE;
} else {
collision_stop_required = true;
stop_line_idx = default_stop_line_idx_opt;
occlusion_stop_required = true;
occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt;
// clear first stop line
// insert creep velocity [closest_idx, occlusion_stop_line)
insert_creep_during_occlusion =
std::make_pair(closest_idx, occlusion_peeking_line_idx_opt.value());
occlusion_state_ = OcclusionState::COLLISION_DETECTED;
}
} else {
const bool approached_stop_line =
(std::fabs(dist_1st_stopline) < planner_param_.common.stop_overshoot_margin);
const bool is_stopped = planner_data_->isVehicleStopped();
if (is_stopped && approached_stop_line) {
// start waiting at the first stop line
before_creep_state_machine_.setStateWithMarginTime(
Expand Down Expand Up @@ -403,10 +413,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
if (occlusion_stop_required) {
occlusion_first_stop_required_ = first_phase_stop_required;
occlusion_safety_ = is_occlusion_cleared;
} else {
/* collision */
setSafe(collision_state_machine_.getState() == StateMachine::State::GO);
}
/* collision */
setSafe(collision_state_machine_.getState() == StateMachine::State::GO);

/* make decision */
const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,7 @@ class IntersectionModule : public SceneModuleInterface
WAIT_FIRST_STOP_LINE,
CREEP_SECOND_STOP_LINE,
CLEARED,
COLLISION_DETECTED,
};

IntersectionModule(
Expand Down