text: /******************************************************************************
   * Copyright 2018 The Apollo Authors. All Rights Reserved.
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   * http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   *****************************************************************************/
  
  /**
   * @file
   **/
  
  #include "modules/planning/scenarios/lane_follow/lane_follow_stage.h"
  
  #include <utility>
  
  #include "cyber/common/log.h"
  #include "modules/common/math/math_utils.h"
  #include "modules/common/time/time.h"
  #include "modules/common/util/point_factory.h"
  #include "modules/common/util/string_util.h"
  #include "modules/common/vehicle_state/vehicle_state_provider.h"
  #include "modules/map/hdmap/hdmap.h"
  #include "modules/map/hdmap/hdmap_common.h"
  #include "modules/planning/common/ego_info.h"
  #include "modules/planning/common/frame.h"
  #include "modules/planning/common/planning_gflags.h"
  #include "modules/planning/constraint_checker/constraint_checker.h"
  #include "modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.h"
  #include "modules/planning/tasks/deciders/path_decider/path_decider.h"
  #include "modules/planning/tasks/deciders/speed_decider/speed_decider.h"
  #include "modules/planning/tasks/optimizers/path_time_heuristic/path_time_heuristic_optimizer.h"
  
  namespace apollo {
  namespace planning {
  namespace scenario {
  namespace lane_follow {
  
  using apollo::common::ErrorCode;
  using apollo::common::SLPoint;
  using apollo::common::Status;
  using apollo::common::TrajectoryPoint;
  using apollo::common::time::Clock;
  using apollo::common::util::PointFactory;
  
  namespace {
  constexpr double kPathOptimizationFallbackCost = 2e4;
  constexpr double kSpeedOptimizationFallbackCost = 2e4;
  constexpr double kStraightForwardLineCost = 10.0;
  }  // namespace
  
  LaneFollowStage::LaneFollowStage(
      const ScenarioConfig::StageConfig& config,
      const std::shared_ptr<DependencyInjector>& injector)
      : Stage(config, injector) {}
  
  void LaneFollowStage::RecordObstacleDebugInfo(
      ReferenceLineInfo* reference_line_info) {
    if (!FLAGS_enable_record_debug) {
      ADEBUG << "Skip record debug info";
      return;
    }
    auto ptr_debug = reference_line_info->mutable_debug();
  
    const auto path_decision = reference_line_info->path_decision();
    for (const auto obstacle : path_decision->obstacles().Items()) {
      auto obstacle_debug = ptr_debug->mutable_planning_data()->add_obstacle();
      obstacle_debug->set_id(obstacle->Id());
      obstacle_debug->mutable_sl_boundary()->CopyFrom(
          obstacle->PerceptionSLBoundary());
      const auto& decider_tags = obstacle->decider_tags();
      const auto& decisions = obstacle->decisions();
      if (decider_tags.size() != decisions.size()) {
        AERROR << "decider_tags size: " << decider_tags.size()
               << " different from decisions size:" << decisions.size();
      }
      for (size_t i = 0; i < decider_tags.size(); ++i) {
        auto decision_tag = obstacle_debug->add_decision_tag();
        decision_tag->set_decider_tag(decider_tags[i]);
        decision_tag->mutable_decision()->CopyFrom(decisions[i]);
      }
    }
  }
  
  Stage::StageStatus LaneFollowStage::Process(
      const TrajectoryPoint& planning_start_point, Frame* frame) {
    bool has_drivable_reference_line = false;
  
    ADEBUG << "Number of reference lines:\t"
           << frame->mutable_reference_line_info()->size();
  
    unsigned int count = 0;
  
    for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
      // TODO(SHU): need refactor
      if (count++ == frame->mutable_reference_line_info()->size()) {
        break;
      }
      ADEBUG << "No: [" << count << "] Reference Line.";
      ADEBUG << "IsChangeLanePath: " << reference_line_info.IsChangeLanePath();
  
      if (has_drivable_reference_line) {
        reference_line_info.SetDrivable(false);
        break;
      }
  
      auto cur_status =
          PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);
  
      if (cur_status.ok()) {
        if (reference_line_info.IsChangeLanePath()) {
          ADEBUG << "reference line is lane change ref.";
          ADEBUG << "FLAGS_enable_smarter_lane_change: "
                 << FLAGS_enable_smarter_lane_change;
          if (reference_line_info.Cost() < kStraightForwardLineCost &&
              (LaneChangeDecider::IsClearToChangeLane(&reference_line_info) ||
               FLAGS_enable_smarter_lane_change)) {
            // If the path and speed optimization succeed on target lane while
            // under smart lane-change or IsClearToChangeLane under older version
            has_drivable_reference_line = true;
            reference_line_info.SetDrivable(true);
            LaneChangeDecider::UpdatePreparationDistance(
                true, frame, &reference_line_info, injector_->planning_context());
            ADEBUG << "\tclear for lane change";
          } else {
            LaneChangeDecider::UpdatePreparationDistance(
                false, frame, &reference_line_info,
                injector_->planning_context());
            reference_line_info.SetDrivable(false);
            ADEBUG << "\tlane change failed";
          }
        } else {
          ADEBUG << "reference line is NOT lane change ref.";
          has_drivable_reference_line = true;
        }
      } else {
        reference_line_info.SetDrivable(false);
      }
    }
  
    return has_drivable_reference_line ? StageStatus::RUNNING
                                       : StageStatus::ERROR;
  }
  
  Status LaneFollowStage::PlanOnReferenceLine(
      const TrajectoryPoint& planning_start_point, Frame* frame,
      ReferenceLineInfo* reference_line_info) {
    if (!reference_line_info->IsChangeLanePath()) {
      reference_line_info->AddCost(kStraightForwardLineCost);
    }
    ADEBUG << "planning start point:" << planning_start_point.DebugString();
    ADEBUG << "Current reference_line_info is IsChangeLanePath: "
           << reference_line_info->IsChangeLanePath();
  
    auto ret = Status::OK();
    for (auto* task : task_list_) {
      const double start_timestamp = Clock::NowInSeconds();
  
      ret = task->Execute(frame, reference_line_info);
  
      const double end_timestamp = Clock::NowInSeconds();
      const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
      ADEBUG << "after task[" << task->Name()
             << "]:" << reference_line_info->PathSpeedDebugString();
      ADEBUG << task->Name() << " time spend: " << time_diff_ms << " ms.";
      RecordDebugInfo(reference_line_info, task->Name(), time_diff_ms);
  
      if (!ret.ok()) {
        AERROR << "Failed to run tasks[" << task->Name()
               << "], Error message: " << ret.error_message();
        break;
      }
  
      // TODO(SHU): disable reference line order changes for now
      // updated reference_line_info, because it is changed in
      // lane_change_decider by PrioritizeChangeLane().
      // reference_line_info = &frame->mutable_reference_line_info()->front();
      // ADEBUG << "Current reference_line_info is IsChangeLanePath: "
      //        << reference_line_info->IsChangeLanePath();
    }
  
    RecordObstacleDebugInfo(reference_line_info);
  
    // check path and speed results for path or speed fallback
    reference_line_info->set_trajectory_type(ADCTrajectory::NORMAL);
    if (!ret.ok()) {
      PlanFallbackTrajectory(planning_start_point, frame, reference_line_info);
    }
  
    DiscretizedTrajectory trajectory;
    if (!reference_line_info->CombinePathAndSpeedProfile(
            planning_start_point.relative_time(),
            planning_start_point.path_point().s(), &trajectory)) {
      std::string msg("Fail to aggregate planning trajectory.");
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
  
    // determine if there is a destination on reference line.
    double dest_stop_s = -1.0;
    for (const auto* obstacle :
         reference_line_info->path_decision()->obstacles().Items()) {
      if (obstacle->LongitudinalDecision().has_stop() &&
          obstacle->LongitudinalDecision().stop().reason_code() ==
              STOP_REASON_DESTINATION) {
        SLPoint dest_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
                                    reference_line_info->reference_line());
        dest_stop_s = dest_sl.s();
      }
    }
  
    for (const auto* obstacle :
         reference_line_info->path_decision()->obstacles().Items()) {
      if (obstacle->IsVirtual()) {
        continue;
      }
      if (!obstacle->IsStatic()) {
        continue;
      }
      if (obstacle->LongitudinalDecision().has_stop()) {
        bool add_stop_obstacle_cost = false;
        if (dest_stop_s < 0.0) {
          add_stop_obstacle_cost = true;
        } else {
          SLPoint stop_sl = GetStopSL(obstacle->LongitudinalDecision().stop(),
                                      reference_line_info->reference_line());
          if (stop_sl.s() < dest_stop_s) {
            add_stop_obstacle_cost = true;
          }
        }
        if (add_stop_obstacle_cost) {
          static constexpr double kReferenceLineStaticObsCost = 1e3;
          reference_line_info->AddCost(kReferenceLineStaticObsCost);
        }
      }
    }
  
    if (FLAGS_enable_trajectory_check) {
      if (ConstraintChecker::ValidTrajectory(trajectory) !=
          ConstraintChecker::Result::VALID) {
        std::string msg("Current planning trajectory is not valid.");
        AERROR << msg;
        return Status(ErrorCode::PLANNING_ERROR, msg);
      }
    }
  
    reference_line_info->SetTrajectory(trajectory);
    reference_line_info->SetDrivable(true);
    return Status::OK();
  }
  
  void LaneFollowStage::PlanFallbackTrajectory(
      const TrajectoryPoint& planning_start_point, Frame* frame,
      ReferenceLineInfo* reference_line_info) {
    // path and speed fall back
    if (reference_line_info->path_data().Empty()) {
      AERROR << "Path fallback due to algorithm failure";
      GenerateFallbackPathProfile(reference_line_info,
                                  reference_line_info->mutable_path_data());
      reference_line_info->AddCost(kPathOptimizationFallbackCost);
      reference_line_info->set_trajectory_type(ADCTrajectory::PATH_FALLBACK);
    }
  
    if (reference_line_info->trajectory_type() != ADCTrajectory::PATH_FALLBACK) {
      if (!RetrieveLastFramePathProfile(
              reference_line_info, frame,
              reference_line_info->mutable_path_data())) {
        const auto& candidate_path_data =
            reference_line_info->GetCandidatePathData();
        for (const auto& path_data : candidate_path_data) {
          if (path_data.path_label().find("self") != std::string::npos) {
            *reference_line_info->mutable_path_data() = path_data;
            AERROR << "Use current frame self lane path as fallback ";
            break;
          }
        }
      }
    }
  
    AERROR << "Speed fallback due to algorithm failure";
    *reference_line_info->mutable_speed_data() =
        SpeedProfileGenerator::GenerateFallbackSpeed(injector_->ego_info());
  
    if (reference_line_info->trajectory_type() != ADCTrajectory::PATH_FALLBACK) {
      reference_line_info->AddCost(kSpeedOptimizationFallbackCost);
      reference_line_info->set_trajectory_type(ADCTrajectory::SPEED_FALLBACK);
    }
  }
  
  void LaneFollowStage::GenerateFallbackPathProfile(
      const ReferenceLineInfo* reference_line_info, PathData* path_data) {
    const double unit_s = 1.0;
    const auto& reference_line = reference_line_info->reference_line();
  
    auto adc_point = injector_->ego_info()->start_point();
    DCHECK(adc_point.has_path_point());
    const auto adc_point_x = adc_point.path_point().x();
    const auto adc_point_y = adc_point.path_point().y();
  
    common::SLPoint adc_point_s_l;
    if (!reference_line.XYToSL(adc_point.path_point(), &adc_point_s_l)) {
      AERROR << "Fail to project ADC to reference line when calculating path "
                "fallback. Straight forward path is generated";
      const auto adc_point_heading = adc_point.path_point().theta();
      const auto adc_point_kappa = adc_point.path_point().kappa();
      const auto adc_point_dkappa = adc_point.path_point().dkappa();
      std::vector<common::PathPoint> path_points;
      double adc_traversed_x = adc_point_x;
      double adc_traversed_y = adc_point_y;
  
      const double max_s = 100.0;
      for (double s = 0; s < max_s; s += unit_s) {
        path_points.push_back(PointFactory::ToPathPoint(
            adc_traversed_x, adc_traversed_y, 0.0, s, adc_point_heading,
            adc_point_kappa, adc_point_dkappa));
        adc_traversed_x += unit_s * std::cos(adc_point_heading);
        adc_traversed_y += unit_s * std::sin(adc_point_heading);
      }
      path_data->SetDiscretizedPath(DiscretizedPath(std::move(path_points)));
      return;
    }
  
    // Generate a fallback path along the reference line direction
    const auto adc_s = adc_point_s_l.s();
    const auto& adc_ref_point =
        reference_line.GetReferencePoint(adc_point_x, adc_point_y);
    const double dx = adc_point_x - adc_ref_point.x();
    const double dy = adc_point_y - adc_ref_point.y();
  
    std::vector<common::PathPoint> path_points;
    const double max_s = reference_line.Length();
    for (double s = adc_s; s < max_s; s += unit_s) {
      const auto& ref_point = reference_line.GetReferencePoint(s);
      path_points.push_back(PointFactory::ToPathPoint(
          ref_point.x() + dx, ref_point.y() + dy, 0.0, s - adc_s,
          ref_point.heading(), ref_point.kappa(), ref_point.dkappa()));
    }
    path_data->SetDiscretizedPath(DiscretizedPath(std::move(path_points)));
  }
  
  bool LaneFollowStage::RetrieveLastFramePathProfile(
      const ReferenceLineInfo* reference_line_info, const Frame* frame,
      PathData* path_data) {
    const auto* ptr_last_frame = injector_->frame_history()->Latest();
    if (ptr_last_frame == nullptr) {
      AERROR
          << "Last frame doesn't succeed, fail to retrieve last frame path data";
      return false;
    }
  
    const auto& last_frame_discretized_path =
        ptr_last_frame->current_frame_planned_path();
  
    path_data->SetDiscretizedPath(last_frame_discretized_path);
    const auto adc_frenet_frame_point_ =
        reference_line_info->reference_line().GetFrenetPoint(
            frame->PlanningStartPoint().path_point());
  
    bool trim_success = path_data->LeftTrimWithRefS(adc_frenet_frame_point_);
    if (!trim_success) {
      AERROR << "Fail to trim path_data. adc_frenet_frame_point: "
             << adc_frenet_frame_point_.ShortDebugString();
      return false;
    }
    AERROR << "Use last frame good path to do speed fallback";
    return true;
  }
  
  SLPoint LaneFollowStage::GetStopSL(const ObjectStop& stop_decision,
                                     const ReferenceLine& reference_line) const {
    SLPoint sl_point;
    reference_line.XYToSL(stop_decision.stop_point(), &sl_point);
    return sl_point;
  }
  
  }  // namespace lane_follow
  }  // namespace scenario
  }  // namespace planning
  }  // namespace apollo
  