From 05add5151b248383bfc5cea64f8da8af8623fa1b Mon Sep 17 00:00:00 2001 From: YajiaZhang Date: Wed, 21 Feb 2018 16:22:23 -0800 Subject: [PATCH] planning: add lon. stop trajectory generation; removed virtual obstacle construction for traffic light --- .../behavior_decider/signal_light_scenario.cc | 65 +------------------ .../behavior_decider/signal_light_scenario.h | 7 -- .../end_condition_sampler.cc | 31 +++++++++ .../end_condition_sampler.h | 3 + .../trajectory1d_generator.cc | 26 +++++++- .../trajectory1d_generator.h | 4 ++ 6 files changed, 65 insertions(+), 71 deletions(-) diff --git a/modules/planning/lattice/behavior_decider/signal_light_scenario.cc b/modules/planning/lattice/behavior_decider/signal_light_scenario.cc index 00fed654eb..f246edfded 100644 --- a/modules/planning/lattice/behavior_decider/signal_light_scenario.cc +++ b/modules/planning/lattice/behavior_decider/signal_light_scenario.cc @@ -62,17 +62,8 @@ int SignalLightScenario::ComputeScenarioDecision( continue; } - double stop_deceleration = - EstimateStopDeceleration(reference_line_info, signal_from_map); - - if ((it_signal->second->color() == TrafficLight::RED && - stop_deceleration < FLAGS_max_stop_deceleration) || - // (signal.color() == TrafficLight::UNKNOWN && - // stop_deceleration < FLAGS_max_stop_deceleration) || - (it_signal->second->color() == TrafficLight::YELLOW && - stop_deceleration < FLAGS_max_stop_deacceleration_for_yellow_light)) { - CreateStopObstacle(frame, reference_line_info, signal_from_map); - + if ((it_signal->second->color() == TrafficLight::RED) || + (it_signal->second->color() == TrafficLight::YELLOW)) { stop_s = std::min(stop_s, signal_from_map->start_s); } } @@ -125,57 +116,5 @@ SignalLightScenario::GetPerceptionDetectedSignals() { return detected_signals; } -double SignalLightScenario::EstimateStopDeceleration( - ReferenceLineInfo* const reference_line_info, - const hdmap::PathOverlap* signal_light) { - double ego_speed = - common::VehicleStateProvider::instance()->linear_velocity(); - - double ego_front_s = reference_line_info->AdcSlBoundary().end_s(); - double stop_s = signal_light->start_s; - - if (stop_s >= ego_front_s) { - double stop_distance = stop_s - ego_front_s; - return (ego_speed * ego_speed) / stop_distance * 0.5; - } else { - // longitudinal_acceleration_lower_bound is a negative value. - return -FLAGS_longitudinal_acceleration_lower_bound; - } -} - -void SignalLightScenario::CreateStopObstacle( - Frame* frame, ReferenceLineInfo* const reference_line_info, - const hdmap::PathOverlap* signal_light) { - // check - const auto& reference_line = reference_line_info->reference_line(); - const double stop_s = - signal_light->start_s - FLAGS_traffic_light_stop_distance; - const double box_center_s = - signal_light->start_s + FLAGS_virtual_stop_wall_length / 2.0; - if (!WithinBound(0.0, reference_line.Length(), stop_s) || - !WithinBound(0.0, reference_line.Length(), box_center_s)) { - ADEBUG << "signal " << signal_light->object_id - << " is not on reference line"; - return; - } - - // create virtual stop wall - std::string virtual_obstacle_id = - FLAGS_signal_light_virtual_obstacle_id_prefix + signal_light->object_id; - auto* obstacle = frame->CreateVirtualStopObstacle( - reference_line_info, virtual_obstacle_id, signal_light->start_s); - if (!obstacle) { - AERROR << "Failed to create obstacle [" << virtual_obstacle_id << "]"; - return; - } - PathObstacle* stop_wall = reference_line_info->AddObstacle(obstacle); - if (!stop_wall) { - AERROR << "Failed to create path_obstacle for: " << virtual_obstacle_id; - return; - } - - return; -} - } // namespace planning } // namespace apollo diff --git a/modules/planning/lattice/behavior_decider/signal_light_scenario.h b/modules/planning/lattice/behavior_decider/signal_light_scenario.h index 8c11ea8083..fc7e6dd566 100644 --- a/modules/planning/lattice/behavior_decider/signal_light_scenario.h +++ b/modules/planning/lattice/behavior_decider/signal_light_scenario.h @@ -50,13 +50,6 @@ class SignalLightScenario : public Scenario { std::vector FindValidSignalLightFromMap( ReferenceLineInfo* const reference_line_info); - double EstimateStopDeceleration(ReferenceLineInfo* const reference_line_info, - const hdmap::PathOverlap* signal_light); - - void CreateStopObstacle(Frame* frame, - ReferenceLineInfo* const reference_line_info, - const hdmap::PathOverlap* signal_light); - DECLARE_SCENARIO(SignalLightScenario); }; diff --git a/modules/planning/lattice/trajectory_generator/end_condition_sampler.cc b/modules/planning/lattice/trajectory_generator/end_condition_sampler.cc index ed57a34671..8539395474 100644 --- a/modules/planning/lattice/trajectory_generator/end_condition_sampler.cc +++ b/modules/planning/lattice/trajectory_generator/end_condition_sampler.cc @@ -88,6 +88,37 @@ EndConditionSampler::SampleLonEndConditionsForCruising( return end_s_conditions; } + +std::vector, double>> +EndConditionSampler::SampleLonEndConditionsForStopping( + const double ref_stop_point) const { + // time interval is one second plus the last one 0.01 + constexpr std::size_t num_time_section = 9; + std::array time_sections; + for (std::size_t i = 0; i + 1 < num_time_section; ++i) { + time_sections[i] = FLAGS_trajectory_time_length - i; + } + time_sections[num_time_section - 1] = 0.01; + + constexpr std::size_t num_stop_section = 3; + std::array s_offsets; + for (std::size_t i = 0; i < num_stop_section; ++i) { + s_offsets[i] = -1 * i; + } + + std::vector, double>> end_s_conditions; + for (const auto& time : time_sections) { + for (const auto& s_offset : s_offsets) { + std::array end_s; + end_s[0] = std::max(init_s_[0], ref_stop_point + s_offset); + end_s[1] = 0.0; + end_s[2] = 0.0; + end_s_conditions.emplace_back(end_s, time); + } + } + return end_s_conditions; +} + std::vector, double>> EndConditionSampler::SampleLonEndConditionsForPathTimeBounds( const PlanningTarget& planning_target) const { diff --git a/modules/planning/lattice/trajectory_generator/end_condition_sampler.h b/modules/planning/lattice/trajectory_generator/end_condition_sampler.h index 71b3e7ff78..729d89eee9 100644 --- a/modules/planning/lattice/trajectory_generator/end_condition_sampler.h +++ b/modules/planning/lattice/trajectory_generator/end_condition_sampler.h @@ -47,6 +47,9 @@ class EndConditionSampler { std::vector, double>> SampleLonEndConditionsForCruising(const double ref_cruise_speed) const; + std::vector, double>> + SampleLonEndConditionsForStopping(const double ref_stop_point) const; + std::vector, double>> SampleLonEndConditionsForPathTimeBounds( const PlanningTarget& planning_target) const; diff --git a/modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc b/modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc index a05bc7122f..516dfcf8a6 100644 --- a/modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc +++ b/modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc @@ -63,7 +63,7 @@ void Trajectory1dGenerator::GenerateSpeedProfilesForCruising( const double target_speed, std::vector>* ptr_lon_trajectory_bundle) const { ADEBUG << "cruise speed is " << target_speed; - std::vector, double>> end_conditions = + auto end_conditions = end_condition_sampler_->SampleLonEndConditionsForCruising(target_speed); for (const auto& end_condition : end_conditions) { @@ -77,6 +77,25 @@ void Trajectory1dGenerator::GenerateSpeedProfilesForCruising( std::shared_ptr(new QuarticPolynomialCurve1d( init_lon_state_, end_state, end_condition.second)))); + lattice_traj_ptr->set_target_velocity(end_condition.first[1]); + lattice_traj_ptr->set_target_time(end_condition.second); + ptr_lon_trajectory_bundle->push_back(lattice_traj_ptr); + } +} + +void Trajectory1dGenerator::GenerateSpeedProfileForStopping( + const double stop_point, + std::vector>* ptr_lon_trajectory_bundle) const { + ADEBUG << "stop point is " << stop_point; + auto end_conditions = + end_condition_sampler_->SampleLonEndConditionsForStopping(stop_point); + + for (const auto& end_condition : end_conditions) { + std::shared_ptr lattice_traj_ptr( + new LatticeTrajectory1d( + std::shared_ptr(new QuinticPolynomialCurve1d( + init_lon_state_, end_condition.first, end_condition.second)))); + lattice_traj_ptr->set_target_position(end_condition.first[0]); lattice_traj_ptr->set_target_velocity(end_condition.first[1]); lattice_traj_ptr->set_target_time(end_condition.second); @@ -114,6 +133,11 @@ void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle( // GenerateSpeedProfilesForPathTimeBound(planning_target, ptr_lon_trajectory_bundle); + + if (planning_target.has_stop_point()) { + GenerateSpeedProfileForStopping(planning_target.stop_point(), + ptr_lon_trajectory_bundle); + } } void Trajectory1dGenerator::GenerateLateralTrajectoryBundle( diff --git a/modules/planning/lattice/trajectory_generator/trajectory1d_generator.h b/modules/planning/lattice/trajectory_generator/trajectory1d_generator.h index 9b3429cdc5..a84173cca1 100644 --- a/modules/planning/lattice/trajectory_generator/trajectory1d_generator.h +++ b/modules/planning/lattice/trajectory_generator/trajectory1d_generator.h @@ -51,6 +51,10 @@ class Trajectory1dGenerator { const double target_speed, std::vector>* ptr_lon_trajectory_bundle) const; + void GenerateSpeedProfileForStopping( + const double stop_point, + std::vector>* ptr_lon_trajectory_bundle) const; + void GenerateSpeedProfilesForPathTimeBound( const PlanningTarget& planning_target, std::vector>* ptr_lon_trajectory_bundle) const; -- GitLab