diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 5b626ec19d917..a932254140878 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -51,6 +51,7 @@ possible_object_bbox: [1.5, 2.5] # [m x m] ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h stop_release_margin_time: 1.5 # [s] + temporal_stop_before_attention_area: false enable_rtc: intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 633ab3115b94c..28cc7a1ad2881 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -130,6 +130,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); ip.occlusion.stop_release_margin_time = getOrDeclareParameter(node, ns + ".occlusion.stop_release_margin_time"); + ip.occlusion.temporal_stop_before_attention_area = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index f795a113672e0..fec7589f3c18f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -98,6 +98,11 @@ IntersectionModule::IntersectionModule( stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); stuck_private_area_timeout_.setState(StateMachine::State::STOP); } + { + temporal_stop_before_attention_state_machine_.setMarginTime( + planner_param_.occlusion.before_creep_stop_time); + temporal_stop_before_attention_state_machine_.setState(StateMachine::State::STOP); + } decision_state_pub_ = node_.create_publisher("~/debug/intersection/decision_state", 1); @@ -480,7 +485,10 @@ void reactRTCApprovalByDecisionResult( // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; + const size_t occlusion_peeking_stop_line = + decision_result.temporal_stop_before_attention_required + ? decision_result.first_attention_stop_line_idx + : decision_result.occlusion_stop_line_idx; if (planner_param.occlusion.enable_creeping) { const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { @@ -544,7 +552,9 @@ void reactRTCApprovalByDecisionResult( } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.occlusion_stop_line_idx; + const auto stop_line_idx = decision_result.temporal_stop_before_attention_required + ? decision_result.first_attention_stop_line_idx + : decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -784,7 +794,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); const auto [closest_idx, stuck_stop_line_idx_opt, default_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] = + intersection_stop_lines; const auto & conflicting_area = intersection_lanelets_.value().conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( @@ -866,12 +877,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( return IntersectionModule::Indecisive{}; } - if (!occlusion_peeking_stop_line_idx_opt) { + if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) { RCLCPP_DEBUG(logger_, "occlusion stop line is null"); return IntersectionModule::Indecisive{}; } 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 & attention_lanelets = intersection_lanelets_.value().attention(); @@ -949,35 +961,62 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if ( occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || ext_occlusion_requested) { - const double dist_stopline = motion_utils::calcSignedArcLength( + const double dist_default_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(closest_idx).point.pose.position, path->points.at(default_stop_line_idx).point.pose.position); - const bool approached_stop_line = - (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); - const bool over_stop_line = (dist_stopline < 0.0); - const bool is_stopped = + const bool approached_default_stop_line = + (std::fabs(dist_default_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_default_stop_line = (dist_default_stopline < 0.0); + const bool is_stopped_at_default = planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); - if (over_stop_line) { + if (over_default_stop_line) { before_creep_state_machine_.setState(StateMachine::State::GO); } if (before_creep_state_machine_.getState() == StateMachine::State::GO) { + const double dist_first_attention_stopline = motion_utils::calcSignedArcLength( + path->points, path->points.at(closest_idx).point.pose.position, + path->points.at(first_attention_stop_line_idx).point.pose.position); + const bool approached_first_attention_stop_line = + (std::fabs(dist_first_attention_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_first_attention_stop_line = (dist_first_attention_stopline < 0.0); + const bool is_stopped_at_first_attention = + planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); + if (planner_param_.occlusion.temporal_stop_before_attention_area) { + if (over_first_attention_stop_line) { + temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO); + } + if (is_stopped_at_first_attention && approached_first_attention_stop_line) { + temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO); + } + } + const bool temporal_stop_before_attention_required = + planner_param_.occlusion.temporal_stop_before_attention_area && + temporal_stop_before_attention_state_machine_.getState() == StateMachine::State::STOP; if (has_collision_with_margin) { - return IntersectionModule::OccludedCollisionStop{ - is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, - occlusion_stop_line_idx}; + return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx}; } else { - return IntersectionModule::PeekingTowardOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, - occlusion_stop_line_idx}; + return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx}; } } else { - if (is_stopped && approached_stop_line) { + if (is_stopped_at_default && approached_default_stop_line) { // start waiting at the first stop line before_creep_state_machine_.setState(StateMachine::State::GO); } + const auto occlusion_stop_line = planner_param_.occlusion.temporal_stop_before_attention_area + ? first_attention_stop_line_idx + : occlusion_stop_line_idx; return IntersectionModule::FirstWaitBeforeOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, - occlusion_stop_line_idx}; + is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line}; } } else if (has_collision_with_margin) { return IntersectionModule::NonOccludedCollisionStop{ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 268bdb5fe1f72..89444bb71c947 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -113,6 +113,7 @@ class IntersectionModule : public SceneModuleInterface std::vector possible_object_bbox; double ignore_parked_vehicle_speed_threshold; double stop_release_margin_time; + bool temporal_stop_before_attention_area; } occlusion; }; @@ -141,15 +142,19 @@ class IntersectionModule : public SceneModuleInterface // NOTE: if intersection_occlusion is disapproved externally through RTC, // it indicates "is_forcefully_occluded" bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; size_t closest_idx{0}; size_t collision_stop_line_idx{0}; + size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; }; struct OccludedCollisionStop { bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; size_t closest_idx{0}; size_t collision_stop_line_idx{0}; + size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; }; struct Safe @@ -222,6 +227,7 @@ class IntersectionModule : public SceneModuleInterface StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; + StateMachine temporal_stop_before_attention_state_machine_; // NOTE: uuid_ is base member // for stuck vehicle detection diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index a59c11deb55d1..a3bebecbd2bca 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -279,6 +279,8 @@ std::optional generateIntersectionStopLines( } } } + 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)); @@ -324,6 +326,7 @@ std::optional generateIntersectionStopLines( size_t closest_idx{0}; size_t stuck_stop_line{0}; size_t default_stop_line{0}; + size_t first_attention_stop_line{0}; size_t occlusion_peeking_stop_line{0}; size_t pass_judge_line{0}; }; @@ -333,6 +336,7 @@ std::optional generateIntersectionStopLines( {&closest_idx_ip, &intersection_stop_lines_temp.closest_idx}, {&stuck_stop_line_ip, &intersection_stop_lines_temp.stuck_stop_line}, {&default_stop_line_ip, &intersection_stop_lines_temp.default_stop_line}, + {&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}, }; @@ -367,6 +371,10 @@ std::optional generateIntersectionStopLines( if (default_stop_line_valid) { intersection_stop_lines.default_stop_line = intersection_stop_lines_temp.default_stop_line; } + if (first_attention_stop_line_valid) { + intersection_stop_lines.first_attention_stop_line = + intersection_stop_lines_temp.first_attention_stop_line; + } if (occlusion_peeking_line_valid) { intersection_stop_lines.occlusion_peeking_stop_line = intersection_stop_lines_temp.occlusion_peeking_stop_line; diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 212081af43111..4135884f03c74 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -128,8 +128,10 @@ struct IntersectionStopLines size_t closest_idx{0}; // NOTE: null if path does not conflict with first_conflicting_area std::optional stuck_stop_line{std::nullopt}; - // NOTE: null if path is over map stop_line OR its value is calculated negative area + // NOTE: null if path is over map stop_line OR its value is calculated negative std::optional default_stop_line{std::nullopt}; + // NOTE: null if the index is calculated negative + std::optional first_attention_stop_line{std::nullopt}; // NOTE: null if footprints do not change from outside to inside of detection area std::optional occlusion_peeking_stop_line{std::nullopt}; // if the value is calculated negative, its value is 0