diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 63ba6ac0b6016..80a209e7213f4 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -121,6 +121,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node, ns + ".collision_detection.pass_judge.judge_before_default_stop_line"); ip.collision_detection.pass_judge.keep_detection_vel_thr = getOrDeclareParameter( node, ns + ".collision_detection.pass_judge.keep_detection_vel_thr"); + ip.collision_detection.pass_judge.allow_overshoot_to_unprotected_area = + getOrDeclareParameter( + node, ns + ".collision_detection.pass_judge.allow_overshoot_to_unprotected_area"); + ip.collision_detection.pass_judge.tolerable_overshoot_to_unprotected_area = + getOrDeclareParameter( + node, ns + ".collision_detection.pass_judge.tolerable_overshoot_to_unprotected_area"); ip.collision_detection.use_upstream_velocity = getOrDeclareParameter(node, ns + ".collision_detection.use_upstream_velocity"); ip.collision_detection.minimum_upstream_velocity = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 7b1689ace7489..781b837c91270 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -99,7 +99,6 @@ IntersectionModule::IntersectionModule( const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), - node_(node), lane_id_(lane_id), associative_ids_(associative_ids), turn_direction_(turn_direction), @@ -131,7 +130,7 @@ IntersectionModule::IntersectionModule( } decision_state_pub_ = - node_.create_publisher("~/debug/intersection/decision_state", 1); + node.create_publisher("~/debug/intersection/decision_state", 1); } void IntersectionModule::initializeRTCStatus() @@ -877,9 +876,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason, &velocity_factor_, &debug_data_); if (!activated_ || !occlusion_activated_) { - is_go_out_ = false; + is_trying_stop_previously_ = true; } else { - is_go_out_ = true; + is_trying_stop_previously_ = false; } RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; @@ -978,15 +977,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( 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, planner_param_.occlusion.enable, path); + 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, pass_judge_line_idx, + sudden_stop_line_idx] = intersection_stop_lines; // see the doc for struct PathLanelets const auto & conflicting_area = intersection_lanelets.conflicting_area(); @@ -1060,48 +1059,49 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // if attention area is not null but default stop line is not available, ego/backward-path has // already passed the stop line - if (!default_stop_line_idx_opt) { - return IntersectionModule::Indecisive{"default stop line is null"}; + if (!default_stop_line_idx_opt || !first_attention_stop_line_idx_opt) { + return IntersectionModule::Indecisive{"default_stop_line/first_attention_stop_line is null"}; } const auto default_stop_line_idx = default_stop_line_idx_opt.value(); + const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); + // 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 (!occlusion_peeking_stop_line_idx_opt) { + return IntersectionModule::Indecisive{"occlusion stop line is null"}; + } + const auto occlusion_stop_line_idx = occlusion_peeking_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 can_stop_before_unprotected_area = + planner_param_.occlusion.enable + ? !util::isOverTargetIndex(*path, closest_idx, current_pose, occlusion_stop_line_idx) + : !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); - const bool keep_detection = - (vel_norm < planner_param_.collision_detection.pass_judge.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"}; + if ( + !can_stop_before_unprotected_area && + planner_param_.collision_detection.pass_judge.judge_before_default_stop_line) { + return Indecisive{"cannot stop before unprotected area without overshoot"}; } - - // 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"}; + if ( + !can_stop_before_unprotected_area && + !planner_param_.collision_detection.pass_judge.allow_overshoot_to_unprotected_area) { + return Indecisive{"cannot stop before unprotected area without overshoot"}; + } + const double expected_overshoot_to_unprotected_area = motion_utils::calcSignedArcLength( + path->points, first_attention_stop_line_idx, sudden_stop_line_idx); + if ( + !can_stop_before_unprotected_area && + planner_param_.collision_detection.pass_judge.allow_overshoot_to_unprotected_area && + expected_overshoot_to_unprotected_area > + planner_param_.collision_detection.pass_judge.tolerable_overshoot_to_unprotected_area) { + return Indecisive{"cannot stop before unprotected area within specified margin"}; } + 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(); @@ -1161,7 +1161,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } // calculate dynamic collision around attention area - const double time_to_restart = (is_go_out_ || is_prioritized) + const double time_to_restart = (!is_trying_stop_previously_ || is_prioritized) ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - collision_state_machine_.getDuration()); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 0946ba0897274..856b33845594e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -107,6 +107,8 @@ class IntersectionModule : public SceneModuleInterface bool judge_before_default_stop_line; double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr + bool allow_overshoot_to_unprotected_area; + double tolerable_overshoot_to_unprotected_area; } pass_judge; bool use_upstream_velocity; double minimum_upstream_velocity; @@ -153,6 +155,12 @@ class IntersectionModule : public SceneModuleInterface { std::string error; }; + struct OverPassJudge + { + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; + }; struct StuckStop { size_t closest_idx{0}; @@ -221,7 +229,8 @@ class IntersectionModule : public SceneModuleInterface size_t occlusion_stop_line_idx{0}; }; using DecisionResult = std::variant< - Indecisive, // internal process error, or over the pass judge line + Indecisive, // internal process error + // OverPassJudge cannot avoid overshoot to unprotected area if stopped suddenly StuckStop, // detected stuck vehicle YieldStuckStop, // detected yield stuck vehicle NonOccludedCollisionStop, // detected collision while FOV is clear @@ -258,14 +267,13 @@ class IntersectionModule : public SceneModuleInterface bool isOcclusionFirstStopRequired() { return occlusion_first_stop_required_; } private: - rclcpp::Node & node_; const int64_t lane_id_; const std::set associative_ids_; const std::string turn_direction_; const bool has_traffic_light_; - bool is_go_out_{false}; - bool is_permanent_go_{false}; + bool is_trying_stop_previously_{false}; + bool is_going_permanently_{false}; DecisionResult prev_decision_result_{Indecisive{""}}; // Parameter diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index d6bc0adfc8a2e..077a90270d374 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -269,7 +269,7 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, 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, const double enable_occlusion, + const double stop_line_margin, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { const auto & path_ip = interpolated_path_info.path; @@ -360,6 +360,13 @@ std::optional generateIntersectionStopLines( const auto pass_judge_line_ip = static_cast( std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + // (4.1) expected stop position on sudden braking + constexpr double emergency_response_time = 0.0; + const double emergency_braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( + velocity, acceleration, max_stop_acceleration, max_stop_jerk, emergency_response_time); + const auto sudden_stop_position_line_ip = std::min( + closest_idx_ip + std::ceil(emergency_braking_dist / ds), path_ip.points.size() - 1); + // (5) stuck vehicle stop line int stuck_stop_line_ip_int = 0; bool stuck_stop_line_valid = true; @@ -391,6 +398,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 sudden_stop_position_line{0}; }; IntersectionStopLinesTemp intersection_stop_lines_temp; @@ -401,7 +409,7 @@ 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}, - }; + {&sudden_stop_position_line_ip, &intersection_stop_lines_temp.sudden_stop_position_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) { @@ -412,19 +420,6 @@ std::optional generateIntersectionStopLines( } *stop_idx = insert_idx.value(); } - if ( - intersection_stop_lines_temp.occlusion_peeking_stop_line < - intersection_stop_lines_temp.default_stop_line) { - intersection_stop_lines_temp.occlusion_peeking_stop_line = - intersection_stop_lines_temp.default_stop_line; - } - if ( - enable_occlusion && 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; if (stuck_stop_line_valid) { @@ -442,6 +437,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.sudden_stop_position_line = + intersection_stop_lines_temp.sudden_stop_position_line; return intersection_stop_lines; } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index bb7be61678deb..ef658a25fce55 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -70,7 +70,7 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, 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, const double enable_occlusion, + const double stop_line_margin, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); std::optional getFirstPointInsidePolygon( diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 3f09b54f88be4..8ae60a1484e9c 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -159,6 +159,8 @@ 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}; + // NOTE: this position is calculated with delay_response_time = 0.0 + size_t sudden_stop_position_line{0}; }; struct PathLanelets