planning: remove useless functions in SpeedProfileGenerator

modules/planning/common/speed_profile_generator.cc

@@ -38,92 +38,6 @@ using common::SpeedPoint;
 using common::TrajectoryPoint;
 using common::math::Vec2d;
 
-std::vector<SpeedPoint> SpeedProfileGenerator::GenerateInitSpeedProfile(
-    const TrajectoryPoint& planning_init_point,
-    const ReferenceLineInfo* reference_line_info) {
-  std::vector<SpeedPoint> 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<SpeedPoint> SpeedProfileGenerator::GenerateSpeedHotStart(
-    const TrajectoryPoint& planning_init_point) {
-  std::vector<SpeedPoint> 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

modules/planning/common/speed_profile_generator.h

@@ -37,13 +37,6 @@ class SpeedProfileGenerator {
   SpeedProfileGenerator() = delete;
   ~SpeedProfileGenerator() = delete;
 
-  static std::vector<common::SpeedPoint> GenerateInitSpeedProfile(
-      const common::TrajectoryPoint& planning_init_point,
-      const ReferenceLineInfo* reference_line_info);
-
-  static std::vector<common::SpeedPoint> 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,

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<Scenario> 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_) {

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);