From 30ece1b655e100f056590627ad01e53a0fc6ba52 Mon Sep 17 00:00:00 2001 From: Yajia Zhang Date: Thu, 9 May 2019 17:15:13 -0700 Subject: [PATCH] planning: remove useless functions in SpeedProfileGenerator --- .../common/speed_profile_generator.cc | 100 ------------------ .../planning/common/speed_profile_generator.h | 10 -- .../planning/scenarios/scenario_manager.cc | 24 ----- modules/planning/scenarios/scenario_manager.h | 2 - 4 files changed, 136 deletions(-) diff --git a/modules/planning/common/speed_profile_generator.cc b/modules/planning/common/speed_profile_generator.cc index 81abb26f65..811c4ec994 100644 --- a/modules/planning/common/speed_profile_generator.cc +++ b/modules/planning/common/speed_profile_generator.cc @@ -38,92 +38,6 @@ using common::SpeedPoint; using common::TrajectoryPoint; using common::math::Vec2d; -std::vector SpeedProfileGenerator::GenerateInitSpeedProfile( - const TrajectoryPoint& planning_init_point, - const ReferenceLineInfo* reference_line_info) { - std::vector speed_profile; - const auto* last_frame = FrameHistory::Instance()->Latest(); - if (!last_frame) { - AWARN << "last frame is empty"; - return speed_profile; - } - const ReferenceLineInfo* last_reference_line_info = - last_frame->DriveReferenceLineInfo(); - if (!last_reference_line_info) { - ADEBUG << "last reference line info is empty"; - return speed_profile; - } - if (!reference_line_info->IsStartFrom(*last_reference_line_info)) { - ADEBUG << "Current reference line is not started previous drived line"; - return speed_profile; - } - const auto& last_speed_data = last_reference_line_info->speed_data(); - - if (!last_speed_data.empty()) { - const auto& last_init_point = last_frame->PlanningStartPoint().path_point(); - Vec2d last_xy_point(last_init_point.x(), last_init_point.y()); - SLPoint last_sl_point; - if (!last_reference_line_info->reference_line().XYToSL(last_xy_point, - &last_sl_point)) { - AERROR << "Fail to transfer xy to sl when init speed profile"; - } - - Vec2d xy_point(planning_init_point.path_point().x(), - planning_init_point.path_point().y()); - SLPoint sl_point; - if (!last_reference_line_info->reference_line().XYToSL(xy_point, - &sl_point)) { - AERROR << "Fail to transfer xy to sl when init speed profile"; - } - - double s_diff = sl_point.s() - last_sl_point.s(); - double start_time = 0.0; - double start_s = 0.0; - bool is_updated_start = false; - for (const auto& speed_point : last_speed_data) { - if (speed_point.s() < s_diff) { - continue; - } - if (!is_updated_start) { - start_time = speed_point.t(); - start_s = speed_point.s(); - is_updated_start = true; - } - SpeedPoint refined_speed_point; - refined_speed_point.set_s(speed_point.s() - start_s); - refined_speed_point.set_t(speed_point.t() - start_time); - refined_speed_point.set_v(speed_point.v()); - refined_speed_point.set_a(speed_point.a()); - refined_speed_point.set_da(speed_point.da()); - speed_profile.push_back(std::move(refined_speed_point)); - } - } - return speed_profile; -} - -// a dummy simple hot start -// TODO(All): refine the hotstart speed profile -std::vector SpeedProfileGenerator::GenerateSpeedHotStart( - const TrajectoryPoint& planning_init_point) { - std::vector hot_start_speed_profile; - double s = 0.0; - double t = 0.0; - double v = common::math::Clamp(planning_init_point.v(), 5.0, - FLAGS_planning_upper_speed_limit); - while (t < FLAGS_trajectory_time_length) { - SpeedPoint speed_point; - speed_point.set_s(s); - speed_point.set_t(t); - speed_point.set_v(v); - - hot_start_speed_profile.push_back(std::move(speed_point)); - - t += FLAGS_trajectory_time_min_interval; - s += v * FLAGS_trajectory_time_min_interval; - } - return hot_start_speed_profile; -} - SpeedData SpeedProfileGenerator::GenerateFallbackSpeed( const double stop_distance) { AERROR << "Fallback using piecewise jerk speed!"; @@ -398,19 +312,5 @@ SpeedData SpeedProfileGenerator::GenerateFixedDistanceCreepProfile( return speed_data; } -SpeedData SpeedProfileGenerator::GenerateFixedSpeedCreepProfile( - const double distance, const double max_speed) { - constexpr double kProceedingSpeed = 2.23; // (5mph proceeding speed) - const double proceeding_speed = std::min(max_speed, kProceedingSpeed); - - constexpr double kDeltaS = 0.1; - SpeedData speed_data; - for (double s = 0.0; s < distance; s += kDeltaS) { - speed_data.AppendSpeedPoint(s, s / proceeding_speed, proceeding_speed, 0.0, - 0.0); - } - return speed_data; -} - } // namespace planning } // namespace apollo diff --git a/modules/planning/common/speed_profile_generator.h b/modules/planning/common/speed_profile_generator.h index 0b58d7083d..92e248f34a 100644 --- a/modules/planning/common/speed_profile_generator.h +++ b/modules/planning/common/speed_profile_generator.h @@ -37,13 +37,6 @@ class SpeedProfileGenerator { SpeedProfileGenerator() = delete; ~SpeedProfileGenerator() = delete; - static std::vector GenerateInitSpeedProfile( - const common::TrajectoryPoint& planning_init_point, - const ReferenceLineInfo* reference_line_info); - - static std::vector GenerateSpeedHotStart( - const common::TrajectoryPoint& planning_init_point); - static SpeedData GenerateFallbackSpeed(const double stop_distance = 0.0); static void FillEnoughSpeedPoints(SpeedData* const speed_data); @@ -56,9 +49,6 @@ class SpeedProfileGenerator { static SpeedData GenerateFixedDistanceCreepProfile(const double distance, const double max_speed); - static SpeedData GenerateFixedSpeedCreepProfile(const double distance, - const double max_speed); - private: static SpeedData GenerateStopProfile(const double init_speed, const double init_acc, diff --git a/modules/planning/scenarios/scenario_manager.cc b/modules/planning/scenarios/scenario_manager.cc index 719c3d93d7..e3381e928d 100644 --- a/modules/planning/scenarios/scenario_manager.cc +++ b/modules/planning/scenarios/scenario_manager.cc @@ -29,7 +29,6 @@ #include "modules/planning/scenarios/lane_follow/lane_follow_scenario.h" #include "modules/planning/scenarios/park/pull_over/pull_over_scenario.h" #include "modules/planning/scenarios/park/valet_parking/valet_parking_scenario.h" -//#include "modules/planning/scenarios/side_pass/side_pass_scenario.h" #include "modules/planning/scenarios/stop_sign/unprotected/stop_sign_unprotected_scenario.h" #include "modules/planning/scenarios/traffic_light/protected/traffic_light_protected_scenario.h" #include "modules/planning/scenarios/traffic_light/unprotected_left_turn/traffic_light_unprotected_left_turn_scenario.h" @@ -60,10 +59,6 @@ std::unique_ptr ScenarioManager::CreateScenario( ptr.reset(new lane_follow::LaneFollowScenario(config_map_[scenario_type], &scenario_context_)); break; -// case ScenarioConfig::SIDE_PASS: -// ptr.reset(new scenario::side_pass::SidePassScenario( -// config_map_[scenario_type], &scenario_context_)); -// break; case ScenarioConfig::BARE_INTERSECTION_UNPROTECTED: ptr.reset( new scenario::bare_intersection::BareIntersectionUnprotectedScenario( @@ -427,18 +422,6 @@ ScenarioConfig::ScenarioType ScenarioManager::SelectBareIntersectionScenario( return default_scenario_type_; } -//ScenarioConfig::ScenarioType ScenarioManager::SelectSidePassScenario( -// const Frame& frame) { -// // TODO(all): to be updated when SIDE_PASS obstacle decisions -// // from ReferenceLine is ready -// if (scenario::side_pass::SidePassScenario::IsTransferable( -// frame, config_map_[ScenarioConfig::SIDE_PASS], *current_scenario_)) { -// return ScenarioConfig::SIDE_PASS; -// } -// -// return default_scenario_type_; -//} - ScenarioConfig::ScenarioType ScenarioManager::SelectValetParkingScenario( const Frame& frame) { const auto& scenario_config = @@ -530,7 +513,6 @@ void ScenarioManager::ScenarioDispatch(const common::TrajectoryPoint& ego_point, case ScenarioConfig::CHANGE_LANE: case ScenarioConfig::PULL_OVER: break; - case ScenarioConfig::SIDE_PASS: case ScenarioConfig::BARE_INTERSECTION_UNPROTECTED: case ScenarioConfig::STOP_SIGN_PROTECTED: case ScenarioConfig::STOP_SIGN_UNPROTECTED: @@ -608,12 +590,6 @@ void ScenarioManager::ScenarioDispatch(const common::TrajectoryPoint& ego_point, scenario_type = SelectChangeLaneScenario(frame); } -// //////////////////////////////////////// -// // SIDE_PASS scenario -// if (scenario_type == default_scenario_type_) { -// scenario_type = SelectSidePassScenario(frame); -// } - //////////////////////////////////////// // pull-over scenario if (scenario_type == default_scenario_type_) { diff --git a/modules/planning/scenarios/scenario_manager.h b/modules/planning/scenarios/scenario_manager.h index 924487969b..069b96a2f2 100644 --- a/modules/planning/scenarios/scenario_manager.h +++ b/modules/planning/scenarios/scenario_manager.h @@ -54,8 +54,6 @@ class ScenarioManager final { ScenarioConfig::ScenarioType SelectPullOverScenario(const Frame& frame); -// ScenarioConfig::ScenarioType SelectSidePassScenario(const Frame& frame); - ScenarioConfig::ScenarioType SelectStopSignScenario( const Frame& frame, const hdmap::PathOverlap& stop_sign_overlap); -- GitLab