Planning: Added fallback and fixed naming issue.

modules/planning/scenarios/lane_follow/lane_follow_stage.cc

@@ -198,7 +198,12 @@ Status LaneFollowStage::PlanOnReferenceLine(
     reference_line_info->set_trajectory_type(ADCTrajectory::SPEED_FALLBACK);
   }
 
-  reference_line_info->set_trajectory_type(ADCTrajectory::NORMAL);
+  if (!(reference_line_info->trajectory_type() ==
+            ADCTrajectory::PATH_FALLBACK ||
+        reference_line_info->trajectory_type() ==
+            ADCTrajectory::SPEED_FALLBACK)) {
+    reference_line_info->set_trajectory_type(ADCTrajectory::NORMAL);
+  }
   DiscretizedTrajectory trajectory;
   if (!reference_line_info->CombinePathAndSpeedProfile(
           planning_start_point.relative_time(),

modules/planning/scenarios/side_pass/side_pass_stage.cc

@@ -96,7 +96,7 @@ Stage::StageStatus SidePassBackup::Process(
   }
 
   // do path planning
-  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
+  bool plan_ok = ExecuteTaskOnReferenceLine(planning_start_point, frame);
   if (!plan_ok) {
     AERROR << "Stage " << Name() << " error: "
            << "planning on reference line failed.";
@@ -160,7 +160,7 @@ Stage::StageStatus SidePassApproachObstacle::Process(
     return Stage::FINISHED;
   }
   // do path planning
-  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
+  bool plan_ok = ExecuteTaskOnReferenceLine(planning_start_point, frame);
   if (!plan_ok) {
     AERROR << "Stage " << Name() << " error: "
            << "planning on reference line failed.";
@@ -221,7 +221,7 @@ Stage::StageStatus SidePassApproachObstacle::Process(
  */
 Stage::StageStatus SidePassGeneratePath::Process(
     const TrajectoryPoint& planning_start_point, Frame* frame) {
-  if (!PlanningOnReferenceLine(planning_start_point, frame)) {
+  if (!ExecuteTaskOnReferenceLine(planning_start_point, frame)) {
     AERROR << "Fail to plan on reference_line.";
     next_stage_ = ScenarioConfig::SIDE_PASS_BACKUP;
     return Stage::FINISHED;
@@ -273,7 +273,7 @@ Stage::StageStatus SidePassDetectSafety::Process(
   debug_path->mutable_path_point()->CopyFrom(
       {path_points.begin(), path_points.end()});
 
-  if (!PlanningOnReferenceLine(planning_start_point, frame)) {
+  if (!ExecuteTaskOnReferenceLine(planning_start_point, frame)) {
     return Stage::ERROR;
   }
   bool is_safe = true;
@@ -338,7 +338,7 @@ Stage::StageStatus SidePassPassObstacle::Process(
   debug_path->mutable_path_point()->CopyFrom(
       {path_points.begin(), path_points.end()});
 
-  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
+  bool plan_ok = ExecuteTaskOnReferenceLine(planning_start_point, frame);
   if (!plan_ok) {
     AERROR << "Fail to plan on reference line.";
     return Stage::ERROR;

modules/planning/scenarios/stage.cc

@@ -23,12 +23,19 @@
 #include <unordered_map>
 #include <utility>
 
+#include "modules/planning/common/speed_profile_generator.h"
 #include "modules/planning/toolkits/task_factory.h"
 
 namespace apollo {
 namespace planning {
 namespace scenario {
 
+namespace {
+constexpr double kPathOptimizationFallbackCost = 2e4;
+constexpr double kSpeedOptimizationFallbackCost = 2e4;
+constexpr double kStraightForwardLineCost = 10.0;
+}  // namespace
+
 Stage::Stage(const ScenarioConfig::StageConfig& config) : config_(config) {
   name_ = ScenarioConfig::StageType_Name(config_.stage_type());
   next_stage_ = config_.stage_type();
@@ -59,7 +66,7 @@ Task* Stage::FindTask(TaskConfig::TaskType task_type) const {
   }
 }
 
-bool Stage::PlanningOnReferenceLine(
+bool Stage::ExecuteTaskOnReferenceLine(
     const common::TrajectoryPoint& planning_start_point, Frame* frame) {
   for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
     if (!reference_line_info.IsDrivable()) {
@@ -77,7 +84,14 @@ bool Stage::PlanningOnReferenceLine(
       }
     }
 
-    reference_line_info.set_trajectory_type(ADCTrajectory::NORMAL);
+    if (reference_line_info.speed_data().Empty()) {
+      *reference_line_info.mutable_speed_data() =
+          SpeedProfileGenerator::GenerateFallbackSpeedProfile();
+      reference_line_info.AddCost(kSpeedOptimizationFallbackCost);
+      reference_line_info.set_trajectory_type(ADCTrajectory::SPEED_FALLBACK);
+    } else {
+      reference_line_info.set_trajectory_type(ADCTrajectory::NORMAL);
+    }
     DiscretizedTrajectory trajectory;
     if (!reference_line_info.CombinePathAndSpeedProfile(
             planning_start_point.relative_time(),

modules/planning/scenarios/stage.h

@@ -79,7 +79,7 @@ class Stage {
   ScenarioConfig::StageType NextStage() const { return next_stage_; }
 
  protected:
-  bool PlanningOnReferenceLine(
+  bool ExecuteTaskOnReferenceLine(
       const common::TrajectoryPoint& planning_start_point, Frame* frame);
 
   std::map<TaskConfig::TaskType, std::unique_ptr<Task>> tasks_;

modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_creep.cc

@@ -56,7 +56,7 @@ Stage::StageStatus StopSignUnprotectedCreep::Process(
   if (dynamic_cast<DeciderCreep*>(FindTask(TaskConfig::DECIDER_CREEP))
           ->CheckCreepDone(*frame, reference_line_info,
                            stop_sign_overlap_end_s)) {
-    bool plan_ok = PlanningOnReferenceLine(planning_init_point, frame);
+    bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
     if (!plan_ok) {
       AERROR << "StopSignUnprotectedCreep planning error";
     }
@@ -70,7 +70,7 @@ Stage::StageStatus StopSignUnprotectedCreep::Process(
       ->SetProceedWithCautionSpeedParam(*frame, reference_line_info,
                                         stop_sign_overlap_end_s);
 
-  bool plan_ok = PlanningOnReferenceLine(planning_init_point, frame);
+  bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
   if (!plan_ok) {
     AERROR << "StopSignUnprotectedCreep planning error";
   }

modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_intersection_cruise.cc

@@ -46,7 +46,7 @@ Stage::StageStatus StopSignUnprotectedIntersectionCruise::Process(
   ADEBUG << "stage: IntersectionCruise";
   CHECK_NOTNULL(frame);
 
-  bool plan_ok = PlanningOnReferenceLine(planning_init_point, frame);
+  bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
   if (!plan_ok) {
     AERROR << "StopSignUnprotectedIntersectionCruise plan error";
   }
@@ -82,15 +82,13 @@ bool StopSignUnprotectedIntersectionCruise::CheckPassIntersection(
 
   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_overlap_end_s << "]";
-  if (adc_back_edge_s - stop_sign_overlap_end_s >
-      kIntersectionLength) {
+         << "] stop_sign_overlap_end_s[" << stop_sign_overlap_end_s << "]";
+  if (adc_back_edge_s - stop_sign_overlap_end_s > kIntersectionLength) {
     return true;
   }
   return false;
 }
 
-
 }  // namespace stop_sign
 }  // namespace scenario
 }  // namespace planning

modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_pre_stop.cc

@@ -56,7 +56,7 @@ Stage::StageStatus StopSignUnprotectedPreStop::Process(
 
   scenario_config_.CopyFrom(GetContext()->scenario_config);
 
-  bool plan_ok = PlanningOnReferenceLine(planning_init_point, frame);
+  bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
   if (!plan_ok) {
     AERROR << "StopSignUnprotectedPreStop planning error";
   }
@@ -80,8 +80,8 @@ Stage::StageStatus StopSignUnprotectedPreStop::Process(
       std::string vehicle = (it->second)[i];
       s = s.empty() ? vehicle : s + "," + vehicle;
     }
-    ADEBUG << "watch_vehicles: lane_id[" << associated_lane_id
-         << "] vehicle[" << s << "]";
+    ADEBUG << "watch_vehicles: lane_id[" << associated_lane_id << "] vehicle["
+           << s << "]";
   }
 
   for (const auto* obstacle : path_decision.obstacles().Items()) {
@@ -99,7 +99,6 @@ int StopSignUnprotectedPreStop::AddWatchVehicle(
     const Obstacle& obstacle, StopSignLaneVehicles* watch_vehicles) {
   CHECK_NOTNULL(watch_vehicles);
 
-
   const PerceptionObstacle& perception_obstacle = obstacle.Perception();
   const std::string& obstacle_id = std::to_string(perception_obstacle.id());
   PerceptionObstacle::Type obstacle_type = perception_obstacle.type();

modules/planning/scenarios/stop_sign/stop_sign_unprotected/stop_sign_unprotected_stop.cc

@@ -56,18 +56,17 @@ Stage::StageStatus StopSignUnprotectedStop::Process(
 
   scenario_config_.CopyFrom(GetContext()->scenario_config);
 
-  bool plan_ok = PlanningOnReferenceLine(planning_init_point, frame);
+  bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
   if (!plan_ok) {
     AERROR << "StopSignUnprotectedPreStop planning error";
   }
 
   auto start_time = GetContext()->stop_start_time;
   const double wait_time = Clock::NowInSeconds() - start_time;
-  ADEBUG << "stop_start_time[" << start_time
-      << "] wait_time[" << wait_time << "]";
+  ADEBUG << "stop_start_time[" << start_time << "] wait_time[" << wait_time
+         << "]";
   auto& watch_vehicles = GetContext()->watch_vehicles;
-  if (wait_time >= scenario_config_.stop_duration() &&
-      watch_vehicles.empty()) {
+  if (wait_time >= scenario_config_.stop_duration() && watch_vehicles.empty()) {
     next_stage_ = ScenarioConfig::STOP_SIGN_UNPROTECTED_CREEP;
     PlanningContext::GetScenarioInfo()->stop_done_overlap_id =
         GetContext()->stop_sign_id;
@@ -87,8 +86,8 @@ Stage::StageStatus StopSignUnprotectedStop::Process(
       std::string vehicle = watch_vehicle_ids[i];
       s = s.empty() ? vehicle : s + "," + vehicle;
     }
-    ADEBUG << "watch_vehicles: lane_id[" << associated_lane_id
-         << "] vehicle[" << s << "]";
+    ADEBUG << "watch_vehicles: lane_id[" << associated_lane_id << "] vehicle["
+           << s << "]";
   }
 
   if (watch_vehicle_ids.size() == 0) {
@@ -123,8 +122,7 @@ Stage::StageStatus StopSignUnprotectedStop::Process(
  * @brief: remove a watch vehicle which not stopping at stop sign any more
  */
 int StopSignUnprotectedStop::RemoveWatchVehicle(
-    const Obstacle& obstacle,
-    const std::vector<std::string>& watch_vehicle_ids,
+    const Obstacle& obstacle, const std::vector<std::string>& watch_vehicle_ids,
     StopSignLaneVehicles* watch_vehicles) {
   CHECK_NOTNULL(watch_vehicles);
 
@@ -152,8 +150,8 @@ int StopSignUnprotectedStop::RemoveWatchVehicle(
   }
 
   for (auto it = watch_vehicles->begin(); it != watch_vehicles->end(); ++it) {
-    if (std::find(it->second.begin(), it->second.end(),
-                  obstacle_id) == it->second.end()) {
+    if (std::find(it->second.begin(), it->second.end(), obstacle_id) ==
+        it->second.end()) {
       continue;
     }
 
@@ -174,37 +172,34 @@ int StopSignUnprotectedStop::RemoveWatchVehicle(
             hdmap::MakeMapId(associated_lane_id));
     if (stop_sign_over_lap_info == nullptr) {
       AERROR << "can't find stop_sign_over_lap_info for id: "
-          << associated_lane_id;
+             << associated_lane_id;
       continue;
     }
     const double stop_line_end_s =
         stop_sign_over_lap_info->lane_overlap_info().end_s();
 
-    const auto lane = HDMapUtil::BaseMap().GetLaneById(
-        hdmap::MakeMapId(associated_lane_id));
+    const auto lane =
+        HDMapUtil::BaseMap().GetLaneById(hdmap::MakeMapId(associated_lane_id));
     if (lane == nullptr) {
       continue;
     }
     auto stop_sign_point = lane.get()->GetSmoothPoint(stop_line_end_s);
     auto obstacle_point = common::util::MakePointENU(
-        perception_obstacle.position().x(),
-        perception_obstacle.position().y(),
+        perception_obstacle.position().x(), perception_obstacle.position().y(),
         perception_obstacle.position().z());
 
-    double distance = common::util::DistanceXY(
-        stop_sign_point, obstacle_point);
-    AERROR << "obstacle_id[" << obstacle_id
-        << "] distance[" << distance << "]";
+    double distance = common::util::DistanceXY(stop_sign_point, obstacle_point);
+    AERROR << "obstacle_id[" << obstacle_id << "] distance[" << distance << "]";
 
     // TODO(all): move 10.0 to conf
     if (distance > 10.0) {
       ADEBUG << "ERASE obstacle_id[" << obstacle_id << "]";
       for (StopSignLaneVehicles::iterator it = watch_vehicles->begin();
            it != watch_vehicles->end(); ++it) {
         std::vector<std::string>& vehicles = it->second;
-        vehicles.erase(std::remove(vehicles.begin(), vehicles.end(),
-                                   obstacle_id),
-                                   vehicles.end());
+        vehicles.erase(
+            std::remove(vehicles.begin(), vehicles.end(), obstacle_id),
+            vehicles.end());
       }
       return 0;
     }

modules/planning/scenarios/traffic_light/right_turn_unprotected/traffic_light_right_turn_unprotected_creep.cc

@@ -66,7 +66,7 @@ Stage::StageStatus TrafficLightRightTurnUnprotectedCreep::Process(
       ->SetProceedWithCautionSpeedParam(*frame, reference_line_info,
                                         traffic_light_overlap_end_s);
 
-  bool plan_ok = PlanningOnReferenceLine(planning_init_point, frame);
+  bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
   if (!plan_ok) {
     AERROR << "TrafficLightRightTurnUnprotectedCreep planning error";
   }

modules/planning/scenarios/traffic_light/right_turn_unprotected/traffic_light_right_turn_unprotected_intersection_cruise.cc

@@ -46,13 +46,13 @@ Stage::StageStatus TrafficLightRightTurnUnprotectedIntersectionCruise::Process(
   CHECK_NOTNULL(frame);
 
   if (GetContext()->traffic_light_id !=
-      PlanningContext::GetScenarioInfo()->
-        next_traffic_light_overlap.object_id) {
+      PlanningContext::GetScenarioInfo()
+          ->next_traffic_light_overlap.object_id) {
     next_stage_ = ScenarioConfig::NO_STAGE;
     return Stage::FINISHED;
   }
 
-  bool plan_ok = PlanningOnReferenceLine(planning_init_point, frame);
+  bool plan_ok = ExecuteTaskOnReferenceLine(planning_init_point, frame);
   if (!plan_ok) {
     AERROR << "TrafficLightRightTurnUnprotectedIntersectionCruise plan error";
   }