diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 7a049b65c0990..c47e6d89db02e 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -92,7 +92,18 @@ If a stopline is associated with the intersection lane on the map, that line is #### Pass Judge Line -To avoid sudden braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) is required to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. +To avoid sudden braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) is required to stop just in front of the attention area, namely the `first_attention_stop_line`, this module does not insert stopline after it passed the `default stop_line` position. + +The position of the pass judge line depends on the occlusion detection configuration and the existence of the associated traffic light of the intersection lane. + +- If `occlusion.enable` is false, the pass judge line before the `first_attention_stop_line` by the braking distance $v_{ego}^{2} / 2a_{max}$. +- If `occlusion.enable` is true and: + - if there are associated traffic lights, the pass judge line is at the `occlusion_peeking_stop_line` in order to continue peeking/collision detection while occlusion is detected. + - if there are no associated traffic lights and: + - if occlusion is detected, pass judge line is at the `occlusion_wo_tl_pass_judge_line` to continue peeking. + - if occlusion is not detected, pass judge line is at the same place at the case where `occlusion.enable` is false. + +![data structure](./docs/data-structure.drawio.svg) ### Occlusion detection @@ -162,8 +173,6 @@ entity IntersectionStopLines { @enduml ``` -![data structure](./docs/data-structure.drawio.svg) - ### Module Parameters | Parameter | Type | Description | diff --git a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg index fb512902ef856..ec5878a0d828e 100644 --- a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg @@ -5,31 +5,31 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="2159px" - height="1028px" - viewBox="-0.5 -0.5 2159 1028" - content="<mxfile host="Electron" modified="2023-10-03T07:12:57.327Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="t-GOTeomSwFlDSuYHtEs" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="2382px" + height="2070px" + viewBox="-0.5 -0.5 2382 2070" + content="<mxfile host="Electron" modified="2023-11-12T05:07:41.048Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="NhhSyCkZKf7kn4cJ36_Z" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > - + - - - - - + + + + + - - - - - - - - - - - - + + + + + + + + + + + + - + - + - + - + - + - - - - - + + + + + + - - - - + - - - + + + - - - - + + + + - - + + - - + + - - + + - - + + - - + + - - + + - - - - - - - - - - - + transform="translate(292.95,0)scale(-1,1)translate(-292.95,0)rotate(90,292.95,751.55)" + pointer-events="none" + /> + + + + + + + + + + +
@@ -294,18 +285,18 @@
- closest_idx + closest_idx - - - + + +
@@ -316,21 +307,21 @@
- stuck_stop_line + stuck_stop_line - - - - - - + + + + + +
@@ -341,18 +332,18 @@
- default_stop_line + default_stop_line - - - + + +
@@ -363,18 +354,18 @@
- first_attention_stop_line + first_attention_stop_line - - - + + +
@@ -389,25 +380,25 @@
- occlusion_peeking... + occlusion_peeking... - - - - - - - - - - - - - - - + + + + + + + + + + + + + + - + - + - + - + - + - - - - - + + + + + + + - - - + - - - + + + - - - + + + - - + + - - + + - - + + - - + + - - + + - - + + - +
@@ -649,18 +632,18 @@
- next + next - - - + + +
@@ -673,18 +656,18 @@
- prev + prev - - - + + +
@@ -697,18 +680,18 @@
- ego_or_entry2exit + ego_or_entry2exit - - - + + +
@@ -721,15 +704,15 @@
- entry2ego + entry2ego - - + + -
+
@@ -739,7 +722,7 @@
- IntersectionStopLines + IntersectionStopLines @@ -747,7 +730,7 @@
@@ -758,9 +741,325 @@
- PathLanelets + PathLanelets + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + occlusion_wo_tl +
+ _pass_judge_line +
+ + +
+
+
+ + occlusion_wo_tl... + + + + + + + + +
+
+
+ + closest_idx + +
+
+
+ + closest_idx + + + + + + + + + + + +
+
+
+ + stuck_stop_line + +
+
+
+ + stuck_stop_line + + + + + + + + +
+
+
+ + default_stop_line + +
+
+
+ + default_stop_line + + + + + + + + +
+
+
+ + first_attention_stop_line + +
+
+
+ + first_attention_stop_line + + + + + + + + +
+
+
+ + + occlusion_peeking +
+ _stop_line + + +
+
+
+ + occlusion_peeking... + + + + + + + + + + +
+
+
+ + + occlusion_wo_tl +
+ _pass_judge_line +
+ + +
+
+
+ + occlusion_wo_tl... + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 2ace5f7ef2cdf..c2761e2cefb86 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -147,6 +147,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); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 9c022329e7a2c..1be9bdf311ea6 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -825,6 +825,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( @@ -858,15 +859,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 @@ -875,21 +878,22 @@ 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, - 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_.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(); @@ -965,38 +969,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!default_stop_line_idx_opt) { return IntersectionModule::Indecisive{"default stop line is null"}; } - const auto default_stop_line_idx = default_stop_line_idx_opt.value(); - - // TODO(Mamoru Sobue): this part needs more formal handling - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - 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 = std::fabs(planner_data_->current_velocity->twist.linear.x); - const bool keep_detection = (vel < planner_param_.collision_detection.keep_detection_vel_thr); - const bool was_safe = std::holds_alternative(prev_decision_result_); - // if ego is over the pass judge line and not stopped - if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { - RCLCPP_DEBUG( - logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection"); - // do nothing - } else if ( - (was_safe && is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || - is_permanent_go_) { - // is_go_out_: previous RTC approval - // activated_: current RTC approval - is_permanent_go_ = true; - 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 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(); @@ -1009,30 +989,6 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( 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); - - // calculate dynamic collision around attention area - const double time_to_restart = (is_go_out_ || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.state_transit_margin_time - - collision_state_machine_.getDuration()); - - const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, time_to_restart, traffic_prioritized_level); - collision_state_machine_.setStateWithMarginTime( - has_collision ? StateMachine::State::STOP : StateMachine::State::GO, - logger_.get_child("collision state_machine"), *clock_); - const bool has_collision_with_margin = - collision_state_machine_.getState() == StateMachine::State::STOP; - - if (is_prioritized) { - return TrafficLightArrowSolidOn{ - 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( @@ -1043,6 +999,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } 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)); @@ -1070,6 +1031,68 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( prev_occlusion_status_ = occlusion_status; } + // TODO(Mamoru Sobue): this part needs more formal handling + 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 double vel_norm = std::hypot( + planner_data_->current_velocity->twist.linear.x, + planner_data_->current_velocity->twist.linear.y); + const bool keep_detection = + (vel_norm < planner_param_.collision_detection.keep_detection_vel_thr); + const bool was_safe = std::holds_alternative(prev_decision_result_); + // if ego is over the pass judge line and not stopped + if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { + RCLCPP_DEBUG( + logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection"); + // do nothing + } else if ( + (was_safe && is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || + is_permanent_go_) { + // is_go_out_: previous RTC approval + // activated_: current RTC approval + is_permanent_go_ = true; + return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; + } + + // calculate dynamic collision around attention area + const double time_to_restart = (is_go_out_ || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.state_transit_margin_time - + collision_state_machine_.getDuration()); + + const bool has_collision = checkCollision( + *path, &target_objects, path_lanelets, closest_idx, time_to_restart, traffic_prioritized_level); + collision_state_machine_.setStateWithMarginTime( + has_collision ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("collision state_machine"), *clock_); + const bool has_collision_with_margin = + collision_state_machine_.getState() == StateMachine::State::STOP; + + if (is_prioritized) { + return TrafficLightArrowSolidOn{ + has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // Safe if (!is_occlusion_state && !has_collision_with_margin) { return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; @@ -1094,14 +1117,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 diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 1553345dfc827..bbdd29d069817 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -95,7 +95,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; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 90e7e89d77cdf..8f2889726d365 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -219,13 +219,15 @@ static std::optional getStopLineIndexFromMap( static std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint) + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); - for (auto i = lane_start; i <= lane_end; ++i) { + for (auto i = start; i <= lane_end; ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; const auto path_footprint = tier4_autoware_utils::transformVector( footprint, tier4_autoware_utils::pose2transform(base_pose)); @@ -241,12 +243,15 @@ static std::optional & polygons, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint) + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - for (size_t i = lane_start; i <= lane_end; ++i) { + for (size_t i = start; i <= lane_end; ++i) { const auto & pose = path_ip.points.at(i).point.pose; const auto path_footprint = tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); @@ -263,7 +268,8 @@ getFirstPointInsidePolygonsByFootprint( std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_detection_area, + const lanelet::CompoundPolygon3d & first_attention_area, + const lanelet::ConstLineString2d & first_attention_lane_centerline, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, const double stop_line_margin, const double peeking_offset, @@ -272,31 +278,39 @@ std::optional generateIntersectionStopLines( const auto & path_ip = interpolated_path_info.path; const double ds = interpolated_path_info.ds; const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + const double baselink2front = planner_data->vehicle_info_.max_longitudinal_offset_m; const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / ds); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); - /* - // find the index of the first point that intersects with detection_areas - const auto first_inside_detection_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_detection_area); - // if path is not intersecting with detection_area, error - if (!first_inside_detection_idx_ip_opt) { - return std::nullopt; - } - const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); - */ // find the index of the first point whose vehicle footprint on it intersects with detection_area const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); std::optional first_footprint_inside_detection_ip_opt = getFirstPointInsidePolygonByFootprint( - first_detection_area, interpolated_path_info, local_footprint); + first_attention_area, interpolated_path_info, local_footprint, baselink2front); if (!first_footprint_inside_detection_ip_opt) { return std::nullopt; } const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); + std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; + for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { + // TODO(Mamoru Sobue): maybe consideration of braking dist is necessary + first_footprint_attention_centerline_ip_opt = i; + break; + } + } + if (!first_footprint_attention_centerline_ip_opt) { + return std::nullopt; + } + const size_t first_footprint_attention_centerline_ip = + first_footprint_attention_centerline_ip_opt.value(); + // (1) default stop line position on interpolated path bool default_stop_line_valid = true; int stop_idx_ip_int = -1; @@ -325,24 +339,25 @@ std::optional generateIntersectionStopLines( // (3) occlusion peeking stop line position on interpolated path int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); bool occlusion_peeking_line_valid = true; + // NOTE: if footprints[0] is already inside the detection area, invalid { - // NOTE: if footprints[0] is already inside the detection area, invalid const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; const auto path_footprint0 = tier4_autoware_utils::transformVector( local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); if (bg::intersects( - path_footprint0, lanelet::utils::to2D(first_detection_area).basicPolygon())) { + path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { occlusion_peeking_line_valid = false; } } if (occlusion_peeking_line_valid) { - occlusion_peeking_line_ip_int = first_footprint_inside_detection_ip; + occlusion_peeking_line_ip_int = + first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); } - const auto first_attention_stop_line_ip = static_cast(occlusion_peeking_line_ip_int); - const bool first_attention_stop_line_valid = occlusion_peeking_line_valid; - occlusion_peeking_line_ip_int += std::ceil(peeking_offset / ds); + const auto occlusion_peeking_line_ip = static_cast( std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto first_attention_stop_line_ip = first_footprint_inside_detection_ip; + const bool first_attention_stop_line_valid = true; // (4) pass judge line position on interpolated path const double velocity = planner_data->current_velocity->twist.linear.x; @@ -356,6 +371,9 @@ std::optional generateIntersectionStopLines( static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); const auto pass_judge_line_ip = static_cast( std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + // TODO(Mamoru Sobue): maybe braking dist should be considered + const auto occlusion_wo_tl_pass_judge_line_ip = + static_cast(first_footprint_attention_centerline_ip); // (5) stuck vehicle stop line int stuck_stop_line_ip_int = 0; @@ -364,7 +382,7 @@ std::optional generateIntersectionStopLines( // NOTE: when ego vehicle is approaching detection area and already passed // first_conflicting_area, this could be null. const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygonByFootprint( - first_conflicting_area, interpolated_path_info, local_footprint); + first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); if (!stuck_stop_line_idx_ip_opt) { stuck_stop_line_valid = false; stuck_stop_line_ip_int = 0; @@ -388,6 +406,7 @@ std::optional generateIntersectionStopLines( size_t first_attention_stop_line{0}; size_t occlusion_peeking_stop_line{0}; size_t pass_judge_line{0}; + size_t occlusion_wo_tl_pass_judge_line{0}; }; IntersectionStopLinesTemp intersection_stop_lines_temp; @@ -398,7 +417,8 @@ std::optional generateIntersectionStopLines( {&first_attention_stop_line_ip, &intersection_stop_lines_temp.first_attention_stop_line}, {&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line}, {&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line}, - }; + {&occlusion_wo_tl_pass_judge_line_ip, + &intersection_stop_lines_temp.occlusion_wo_tl_pass_judge_line}}; stop_lines.sort( [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); for (const auto & [stop_idx_ip, stop_idx] : stop_lines) { @@ -415,12 +435,6 @@ std::optional generateIntersectionStopLines( intersection_stop_lines_temp.occlusion_peeking_stop_line = intersection_stop_lines_temp.default_stop_line; } - if ( - intersection_stop_lines_temp.occlusion_peeking_stop_line > - intersection_stop_lines_temp.pass_judge_line) { - intersection_stop_lines_temp.pass_judge_line = - intersection_stop_lines_temp.occlusion_peeking_stop_line; - } IntersectionStopLines intersection_stop_lines; intersection_stop_lines.closest_idx = intersection_stop_lines_temp.closest_idx; @@ -439,6 +453,8 @@ std::optional generateIntersectionStopLines( intersection_stop_lines_temp.occlusion_peeking_stop_line; } intersection_stop_lines.pass_judge_line = intersection_stop_lines_temp.pass_judge_line; + intersection_stop_lines.occlusion_wo_tl_pass_judge_line = + intersection_stop_lines_temp.occlusion_wo_tl_pass_judge_line; return intersection_stop_lines; } @@ -1339,13 +1355,13 @@ double calcDistanceUntilIntersectionLanelet( void IntersectionLanelets::update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint) + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path if (!first_conflicting_area_) { - auto first = - getFirstPointInsidePolygonsByFootprint(conflicting_area_, interpolated_path_info, footprint); + auto first = getFirstPointInsidePolygonsByFootprint( + conflicting_area_, interpolated_path_info, footprint, vehicle_length); if (first) { first_conflicting_lane_ = conflicting_.at(first.value().second); first_conflicting_area_ = conflicting_area_.at(first.value().second); @@ -1353,7 +1369,7 @@ void IntersectionLanelets::update( } if (!first_attention_area_) { auto first = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_area_, interpolated_path_info, footprint); + attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); if (first) { first_attention_lane_ = attention_non_preceding_.at(first.value().second); first_attention_area_ = attention_non_preceding_area_.at(first.value().second); diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 674f6d8afa3c0..e7ba2b00c749a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -67,7 +67,8 @@ std::optional generateStuckStopLine( std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_detection_area, + const lanelet::CompoundPolygon3d & first_attention_area, + const lanelet::ConstLineString2d & first_attention_lane_centerline, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, const double stop_line_margin, const double peeking_offset, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index adca3a9a8ce41..cd54d90011076 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -72,7 +72,7 @@ struct IntersectionLanelets public: void update( const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint); + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); const lanelet::ConstLanelets & attention() const { return is_prioritized_ ? attention_non_preceding_ : attention_; @@ -158,6 +158,7 @@ struct IntersectionStopLines std::optional occlusion_peeking_stop_line{std::nullopt}; // if the value is calculated negative, its value is 0 size_t pass_judge_line{0}; + size_t occlusion_wo_tl_pass_judge_line{0}; }; struct PathLanelets