diff --git a/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_intersection_cruise.cc b/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_intersection_cruise.cc index 7b5854c535a7326b06811ca88a49ca34fa19b08c..670125812cad1c0194eb88036126b3bf70805c85 100644 --- a/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_intersection_cruise.cc +++ b/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_intersection_cruise.cc @@ -38,6 +38,7 @@ namespace planning { namespace scenario { namespace stop_sign { +using apollo::hdmap::PathOverlap; using common::TrajectoryPoint; Stage::StageStatus StopSignUnprotectedIntersectionCruise::Process( @@ -45,8 +46,7 @@ Stage::StageStatus StopSignUnprotectedIntersectionCruise::Process( ADEBUG << "stage: IntersectionCruise"; CHECK_NOTNULL(frame); - if (GetContext()->stop_sign_id != - PlanningContext::GetScenarioInfo()->next_stop_sign_overlap.object_id) { + if (CheckPassIntersection(frame)) { next_stage_ = ScenarioConfig::NO_STAGE; return Stage::FINISHED; } @@ -58,6 +58,33 @@ Stage::StageStatus StopSignUnprotectedIntersectionCruise::Process( return Stage::RUNNING; } +bool StopSignUnprotectedIntersectionCruise::CheckPassIntersection( + Frame* frame) { + // TODO(all): update when pnc-junction is ready + constexpr double kIntersectionLength = 10.0; // unit: m + + auto& reference_line_info = frame->mutable_reference_line_info()->front(); + double stop_sign_oberlap_end_s = 0; + const std::vector& stop_sign_overlaps = + reference_line_info.reference_line().map_path().stop_sign_overlaps(); + for (const PathOverlap& stop_sign_overlap : stop_sign_overlaps) { + if (GetContext()->stop_sign_id == stop_sign_overlap.object_id) { + stop_sign_oberlap_end_s = stop_sign_overlap.end_s; + break; + } + } + + const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s(); + ADEBUG << "adc_back_edge_s[" << adc_back_edge_s + << "] stop_sign_overlap_end_s[" << stop_sign_oberlap_end_s << "]"; + if (adc_back_edge_s - stop_sign_oberlap_end_s > + kIntersectionLength) { + return true; + } + return false; +} + + } // namespace stop_sign } // namespace scenario } // namespace planning diff --git a/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_intersection_cruise.h b/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_intersection_cruise.h index 8fd62e5811b58ef2bf0cd763dbce92cbdabdfd87..e766d039dfc7bc25280cdf337237dc829aa3ab50 100644 --- a/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_intersection_cruise.h +++ b/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_intersection_cruise.h @@ -36,8 +36,24 @@ namespace stop_sign { struct StopSignUnprotectedContext; -DECLARE_STAGE(StopSignUnprotectedIntersectionCruise, - StopSignUnprotectedContext); +class StopSignUnprotectedIntersectionCruise : public Stage { + public: + explicit StopSignUnprotectedIntersectionCruise( + const ScenarioConfig::StageConfig& config) : Stage(config) {} + + private: + Stage::StageStatus Process(const common::TrajectoryPoint& planning_init_point, + Frame* frame) override; + + StopSignUnprotectedContext* GetContext() { + return GetContextAs(); + } + + bool CheckPassIntersection(Frame* frame); + + private: + ScenarioStopSignUnprotectedConfig scenario_config_; +}; } // namespace stop_sign } // namespace scenario diff --git a/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_scenario.cc b/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_scenario.cc index 500381a1b4cdd0e87a7141559ece34b2e6bd9f5d..344550f5f4125cf363c6d95af00835193bbbaef9 100644 --- a/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_scenario.cc +++ b/modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_scenario.cc @@ -131,11 +131,11 @@ bool StopSignUnprotectedScenario::IsTransferable( const Scenario& current_scenario, const common::TrajectoryPoint& ego_point, const Frame& frame) { - const std::string stop_sign_overlap_id = - PlanningContext::GetScenarioInfo()->next_stop_sign_overlap.object_id; - if (stop_sign_overlap_id.empty()) { - return false; - } + // const std::string stop_sign_overlap_id = + // PlanningContext::GetScenarioInfo()->next_stop_sign_overlap.object_id; + // if (stop_sign_overlap_id.empty()) { + // return false; + // } const auto& reference_line_info = frame.reference_line_info().front(); const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s(); @@ -149,23 +149,28 @@ bool StopSignUnprotectedScenario::IsTransferable( static_cast(ceil( adc_distance_to_stop_sign / adc_speed)) : 0; ADEBUG << "adc_distance_to_stop_sign[" << adc_distance_to_stop_sign + << "] stop_sign_overlap_start_s[" << stop_sign_overlap_start_s << "] adc_speed[" << adc_speed << "] time_distance[" << time_distance << "]"; + ADEBUG << "IsTransferable: current: " << current_scenario.Name() + << "; status: " << current_scenario.GetStatus(); switch (current_scenario.scenario_type()) { case ScenarioConfig::LANE_FOLLOW: case ScenarioConfig::CHANGE_LANE: case ScenarioConfig::SIDE_PASS: case ScenarioConfig::APPROACH: - return (adc_distance_to_stop_sign <= - config_.stop_sign_unprotected_config(). - start_stop_sign_scenario_distance() || - time_distance <= config_.stop_sign_unprotected_config(). - start_stop_sign_scenario_timer()); + return (adc_distance_to_stop_sign > 0 && + (adc_distance_to_stop_sign <= + config_.stop_sign_unprotected_config(). + start_stop_sign_scenario_distance() || + time_distance <= config_.stop_sign_unprotected_config(). + start_stop_sign_scenario_timer())); case ScenarioConfig::STOP_SIGN_PROTECTED: return false; case ScenarioConfig::STOP_SIGN_UNPROTECTED: - return true; + return (current_scenario.GetStatus() != + Scenario::ScenarioStatus::STATUS_DONE); case ScenarioConfig::TRAFFIC_LIGHT_LEFT_TURN_PROTECTED: case ScenarioConfig::TRAFFIC_LIGHT_LEFT_TURN_UNPROTECTED: case ScenarioConfig::TRAFFIC_LIGHT_RIGHT_TURN_PROTECTED: