Skip to content

Commit

Permalink
PR5547
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin authored Nov 10, 2023
1 parent ce0f11a commit 2355ca2
Show file tree
Hide file tree
Showing 6 changed files with 156 additions and 114 deletions.
1 change: 1 addition & 0 deletions planning/behavior_velocity_intersection_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -193,6 +193,7 @@ void IntersectionModuleManager::launchNewModules(
const auto lanelets =
planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose);
// run occlusion detection only in the first intersection
// TODO(Mamoru Sobue): remove `enable_occlusion_detection` variable
const bool enable_occlusion_detection = intersection_param_.occlusion.enable;
for (size_t i = 0; i < lanelets.size(); i++) {
const auto ll = lanelets.at(i);
Expand Down
173 changes: 97 additions & 76 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -936,6 +936,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_);
const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else");
const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;

// spline interpolation
const auto interpolated_path_info_opt = util::generateInterpolatedPath(
Expand Down Expand Up @@ -969,15 +970,17 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
const bool is_prioritized =
traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED;
const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0);
intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint);
intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front);

// this is abnormal
const auto & conflicting_lanelets = intersection_lanelets.conflicting();
const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area();
if (conflicting_lanelets.empty() || !first_conflicting_area_opt) {
const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane();
if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) {
return IntersectionModule::Indecisive{"conflicting area is empty"};
}
const auto first_conflicting_area = first_conflicting_area_opt.value();
const auto & first_conflicting_lane = first_conflicting_lane_opt.value();
const auto & first_conflicting_area = first_conflicting_area_opt.value();

// generate all stop line candidates
// see the doc for struct IntersectionStopLines
Expand All @@ -986,22 +989,24 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
/// conflicting lanes
const auto & dummy_first_attention_area =
first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area;
const double peeking_offset =
has_traffic_light_ ? planner_param_.occlusion.peeking_offset
: planner_param_.occlusion.absence_traffic_light.maximum_peeking_distance;
const auto & dummy_first_attention_lane_centerline =
intersection_lanelets.first_attention_lane()
? intersection_lanelets.first_attention_lane().value().centerline2d()
: first_conflicting_lane.centerline2d();
const auto intersection_stop_lines_opt = util::generateIntersectionStopLines(
first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info,
planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin,
planner_param_.common.max_accel, planner_param_.common.max_jerk,
planner_param_.common.delay_response_time, peeking_offset, path);
first_conflicting_area, dummy_first_attention_area, dummy_first_attention_lane_centerline,
planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline,
planner_param_.common.stop_line_margin, planner_param_.common.max_accel,
planner_param_.common.max_jerk, planner_param_.common.delay_response_time,
planner_param_.occlusion.peeking_offset, path);
if (!intersection_stop_lines_opt) {
return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"};
}
const auto & intersection_stop_lines = intersection_stop_lines_opt.value();
const auto
[closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt,
first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] =
intersection_stop_lines;
first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt,
default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stop_lines;

// see the doc for struct PathLanelets
const auto & conflicting_area = intersection_lanelets.conflicting_area();
Expand Down Expand Up @@ -1083,16 +1088,91 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
if (!default_stop_line_idx_opt) {
return IntersectionModule::Indecisive{"default stop line is null"};
}
// occlusion stop line is generated from the intersection of ego footprint along the path with the
// attention area, so if this is null, eog has already passed the intersection
if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) {
return IntersectionModule::Indecisive{"occlusion stop line is null"};
}
const auto default_stop_line_idx = default_stop_line_idx_opt.value();
const bool is_over_default_stop_line =
util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx);
const auto collision_stop_line_idx =
is_over_default_stop_line ? closest_idx : default_stop_line_idx;
const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value();
const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value();

const auto & adjacent_lanelets = intersection_lanelets.adjacent();
const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention();
const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area();
debug_data_.attention_area = intersection_lanelets.attention_area();
debug_data_.occlusion_attention_area = occlusion_attention_area;
debug_data_.adjacent_area = intersection_lanelets.adjacent_area();

// check occlusion on detection lane
if (!occlusion_attention_divisions_) {
occlusion_attention_divisions_ = util::generateDetectionLaneDivisions(
occlusion_attention_lanelets, routing_graph_ptr,
planner_data_->occupancy_grid->info.resolution,
planner_param_.occlusion.attention_lane_crop_curvature_threshold,
planner_param_.occlusion.attention_lane_curvature_calculation_ds);
}
const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value();

// get intersection area
const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr);
// filter objects
auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area);

const double occlusion_dist_thr = std::fabs(
std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) /
(2 * planner_param_.occlusion.min_vehicle_brake_for_rss));
auto occlusion_status =
(enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized)
? getOcclusionStatus(
*planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets,
first_attention_area, interpolated_path_info, occlusion_attention_divisions,
target_objects, current_pose, occlusion_dist_thr)
: OcclusionType::NOT_OCCLUDED;
occlusion_stop_state_machine_.setStateWithMarginTime(
occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP,
logger_.get_child("occlusion_stop"), *clock_);
const bool is_occlusion_cleared_with_margin =
(occlusion_stop_state_machine_.getState() == StateMachine::State::GO);
// distinguish if ego detected occlusion or RTC detects occlusion
const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_);
if (ext_occlusion_requested) {
occlusion_status = OcclusionType::RTC_OCCLUDED;
}
const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested);
if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) {
occlusion_status = prev_occlusion_status_;
} else {
prev_occlusion_status_ = occlusion_status;
}

// TODO(Mamoru Sobue): this part needs more formal handling
const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
const size_t pass_judge_line_idx = [=]() {
if (enable_occlusion_detection_) {
// if occlusion detection is enabled, pass_judge position is beyond the boundary of first
// attention area
if (has_traffic_light_) {
return occlusion_stop_line_idx;
} else if (is_occlusion_state) {
// if there is no traffic light and occlusion is detected, pass_judge position is beyond
// the boundary of first attention area
return occlusion_wo_tl_pass_judge_line_idx;
} else {
// if there is no traffic light and occlusion is not detected, pass_judge position is
// default
return default_pass_judge_line_idx;
}
}
return default_pass_judge_line_idx;
}();
debug_data_.pass_judge_wall_pose =
planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path);
const bool is_over_pass_judge_line =
util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx);
const bool is_over_default_stop_line =
util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx);
const double vel_norm = std::hypot(
planner_data_->current_velocity->twist.linear.x,
planner_data_->current_velocity->twist.linear.y);
Expand All @@ -1113,28 +1193,6 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"};
}

// occlusion stop line is generated from the intersection of ego footprint along the path with the
// attention area, so if this is null, eog has already passed the intersection
if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) {
return IntersectionModule::Indecisive{"occlusion stop line is null"};
}
const auto collision_stop_line_idx =
is_over_default_stop_line ? closest_idx : default_stop_line_idx;
const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value();
const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value();

const auto & adjacent_lanelets = intersection_lanelets.adjacent();
const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention();
const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area();
debug_data_.attention_area = intersection_lanelets.attention_area();
debug_data_.occlusion_attention_area = occlusion_attention_area;
debug_data_.adjacent_area = intersection_lanelets.adjacent_area();

// get intersection area
const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr);

auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area);

// If there are any vehicles on the attention area when ego entered the intersection on green
// light, do pseudo collision detection because the vehicles are very slow and no collisions may
// be detected. check if ego vehicle entered assigned lanelet
Expand Down Expand Up @@ -1196,43 +1254,6 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx};
}

// check occlusion on detection lane
if (!occlusion_attention_divisions_) {
occlusion_attention_divisions_ = util::generateDetectionLaneDivisions(
occlusion_attention_lanelets, routing_graph_ptr,
planner_data_->occupancy_grid->info.resolution,
planner_param_.occlusion.attention_lane_crop_curvature_threshold,
planner_param_.occlusion.attention_lane_curvature_calculation_ds);
}
const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value();

const double occlusion_dist_thr = std::fabs(
std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) /
(2 * planner_param_.occlusion.min_vehicle_brake_for_rss));
auto occlusion_status =
(enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized)
? getOcclusionStatus(
*planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets,
first_attention_area, interpolated_path_info, occlusion_attention_divisions,
target_objects, current_pose, occlusion_dist_thr)
: OcclusionType::NOT_OCCLUDED;
occlusion_stop_state_machine_.setStateWithMarginTime(
occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP,
logger_.get_child("occlusion_stop"), *clock_);
const bool is_occlusion_cleared_with_margin =
(occlusion_stop_state_machine_.getState() == StateMachine::State::GO);
// distinguish if ego detected occlusion or RTC detects occlusion
const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_);
if (ext_occlusion_requested) {
occlusion_status = OcclusionType::RTC_OCCLUDED;
}
const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested);
if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) {
occlusion_status = prev_occlusion_status_;
} else {
prev_occlusion_status_ = occlusion_status;
}

// Safe
if (!is_occlusion_state && !has_collision_with_margin) {
return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx};
Expand All @@ -1257,14 +1278,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
temporal_stop_before_attention_state_machine_)
: false;
if (!has_traffic_light_) {
if (fromEgoDist(first_attention_stop_line_idx) <= -peeking_offset) {
if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) {
return IntersectionModule::Indecisive{
"already passed maximum peeking line in the absence of traffic light"};
}
return IntersectionModule::OccludedAbsenceTrafficLight{
is_occlusion_cleared_with_margin, has_collision_with_margin,
temporal_stop_before_attention_required, closest_idx,
first_attention_stop_line_idx, occlusion_stop_line_idx};
first_attention_stop_line_idx, occlusion_wo_tl_pass_judge_line_idx};
}
// following remaining block is "has_traffic_light_"
// if ego is stuck by static occlusion in the presence of traffic light, start timeout count
Expand Down
4 changes: 3 additions & 1 deletion planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,9 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
}
auto & intersection_lanelets = intersection_lanelets_.value();
const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0);
intersection_lanelets.update(false, interpolated_path_info, local_footprint);
intersection_lanelets.update(
false, interpolated_path_info, local_footprint,
planner_data_->vehicle_info_.max_longitudinal_offset_m);
const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area();
if (!first_conflicting_area) {
return false;
Expand Down
Loading

0 comments on commit 2355ca2

Please sign in to comment.