text: /******************************************************************************
   * Copyright 2019 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.
   *****************************************************************************/
  
  #include "modules/planning/tasks/deciders/speed_bounds_decider/speed_bounds_decider.h"
  
  #include <algorithm>
  #include <limits>
  #include <string>
  #include <tuple>
  #include <vector>
  
  #include "modules/common/vehicle_state/vehicle_state_provider.h"
  #include "modules/planning/common/path/path_data.h"
  #include "modules/planning/common/planning_context.h"
  #include "modules/planning/common/planning_gflags.h"
  #include "modules/planning/common/st_graph_data.h"
  #include "modules/planning/common/util/common.h"
  #include "modules/planning/tasks/deciders/speed_bounds_decider/speed_limit_decider.h"
  #include "modules/planning/tasks/deciders/speed_bounds_decider/st_boundary_mapper.h"
  
  namespace apollo {
  namespace planning {
  
  using apollo::common::ErrorCode;
  using apollo::common::SLPoint;
  using apollo::common::Status;
  using apollo::common::TrajectoryPoint;
  using apollo::common::math::Vec2d;
  using apollo::planning_internal::StGraphBoundaryDebug;
  using apollo::planning_internal::STGraphDebug;
  
  SpeedBoundsDecider::SpeedBoundsDecider(const TaskConfig &config)
      : Decider(config) {
    CHECK(config.has_speed_bounds_decider_config());
    speed_bounds_config_ = config.speed_bounds_decider_config();
    SetName("SpeedBoundsDecider");
  }
  
  Status SpeedBoundsDecider::Process(
      Frame *const frame, ReferenceLineInfo *const reference_line_info) {
    // retrieve data from frame and reference_line_info
    const SLBoundary &adc_sl_boundary = reference_line_info->AdcSlBoundary();
    const PathData &path_data = reference_line_info->path_data();
    const TrajectoryPoint &init_point = frame->PlanningStartPoint();
    const ReferenceLine &reference_line = reference_line_info->reference_line();
    PathDecision *const path_decision = reference_line_info->path_decision();
  
    // 1. Map obstacles into st graph
    STBoundaryMapper boundary_mapper(adc_sl_boundary, speed_bounds_config_,
                                     reference_line, path_data,
                                     speed_bounds_config_.total_path_length(),
                                     speed_bounds_config_.total_time());
  
    path_decision->EraseStBoundaries();
    if (boundary_mapper.CreateStBoundary(path_decision).code() ==
        ErrorCode::PLANNING_ERROR) {
      const std::string msg = "Mapping obstacle failed.";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
  
    std::vector<const STBoundary *> boundaries;
    for (auto *obstacle : path_decision->obstacles().Items()) {
      const auto &id = obstacle->Id();
      const auto &st_boundary = obstacle->st_boundary();
      if (!st_boundary.IsEmpty()) {
        if (st_boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
          path_decision->Find(id)->SetBlockingObstacle(false);
        } else {
          path_decision->Find(id)->SetBlockingObstacle(true);
        }
        boundaries.push_back(&st_boundary);
      }
    }
  
    const double min_s_on_st_boundaries = SetSpeedFallbackDistance(path_decision);
  
    // 2. Create speed limit along path
    SpeedLimitDecider speed_limit_decider(adc_sl_boundary, speed_bounds_config_,
                                          reference_line, path_data);
  
    SpeedLimit speed_limit;
    if (!speed_limit_decider
             .GetSpeedLimits(path_decision->obstacles(), &speed_limit)
             .ok()) {
      std::string msg("Getting speed limits failed!");
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
  
    // 3. Get path_length as s axis search bound in st graph
    const double path_data_length = path_data.discretized_path().Length();
    const double path_length_by_conf = speed_bounds_config_.total_path_length();
  
    // 4. Get time duration as t axis search bound in st graph
    const double total_time_by_conf = speed_bounds_config_.total_time();
  
    // Load generated st graph data back to frame
    StGraphData *st_graph_data = reference_line_info_->mutable_st_graph_data();
  
    // Add a st_graph debug info and save the pointer to st_graph_data for
    // optimizer logging
    auto *debug = reference_line_info_->mutable_debug();
    STGraphDebug *st_graph_debug = debug->mutable_planning_data()->add_st_graph();
  
    st_graph_data->LoadData(boundaries, min_s_on_st_boundaries, init_point,
                            speed_limit, path_data_length, path_length_by_conf,
                            total_time_by_conf, st_graph_debug);
  
    // Create and record st_graph debug info
    RecordSTGraphDebug(*st_graph_data, st_graph_debug);
  
    return Status::OK();
  }
  
  double SpeedBoundsDecider::SetSpeedFallbackDistance(
      PathDecision *const path_decision) {
    // Set min_s_on_st_boundaries to guide speed fallback. Different stop distance
    // is taken when there is an obstacle moving in opposite direction of ADV
    constexpr double kEpsilon = 1.0e-6;
    double min_s_non_reverse = std::numeric_limits<double>::infinity();
    double min_s_reverse = std::numeric_limits<double>::infinity();
    // TODO(Jinyun): tmp workaround for side pass capability because of doomed
    // speed planning failure when side pass creeping
    double side_pass_stop_s = std::numeric_limits<double>::infinity();
  
    for (auto *obstacle : path_decision->obstacles().Items()) {
      const auto &st_boundary = obstacle->st_boundary();
  
      if (st_boundary.IsEmpty()) {
        continue;
      }
  
      const auto left_bottom_point_s = st_boundary.bottom_left_point().s();
      const auto right_bottom_point_s = st_boundary.bottom_right_point().s();
      const auto lowest_s = std::min(left_bottom_point_s, right_bottom_point_s);
  
      if (left_bottom_point_s - right_bottom_point_s > kEpsilon) {
        if (min_s_reverse > lowest_s) {
          min_s_reverse = lowest_s;
        }
      } else if (min_s_non_reverse > lowest_s) {
        min_s_non_reverse = lowest_s;
      }
  
      if (obstacle->LongitudinalDecision().stop().reason_code() ==
              StopReasonCode::STOP_REASON_SIDEPASS_SAFETY &&
          side_pass_stop_s > lowest_s) {
        side_pass_stop_s = lowest_s;
      }
    }
  
    min_s_reverse = std::max(min_s_reverse, 0.0);
    min_s_non_reverse = std::max(min_s_non_reverse, 0.0);
    side_pass_stop_s = std::max(side_pass_stop_s, 0.0);
  
    if (!std::isinf(side_pass_stop_s)) {
      return side_pass_stop_s;
    }
    return min_s_non_reverse > min_s_reverse ? 0.0 : min_s_non_reverse;
  }
  
  void SpeedBoundsDecider::RecordSTGraphDebug(
      const StGraphData &st_graph_data, STGraphDebug *st_graph_debug) const {
    if (!FLAGS_enable_record_debug || !st_graph_debug) {
      ADEBUG << "Skip record debug info";
      return;
    }
  
    for (const auto &boundary : st_graph_data.st_boundaries()) {
      auto boundary_debug = st_graph_debug->add_boundary();
      boundary_debug->set_name(boundary->id());
      switch (boundary->boundary_type()) {
        case STBoundary::BoundaryType::FOLLOW:
          boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_FOLLOW);
          break;
        case STBoundary::BoundaryType::OVERTAKE:
          boundary_debug->set_type(
              StGraphBoundaryDebug::ST_BOUNDARY_TYPE_OVERTAKE);
          break;
        case STBoundary::BoundaryType::STOP:
          boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_STOP);
          break;
        case STBoundary::BoundaryType::UNKNOWN:
          boundary_debug->set_type(
              StGraphBoundaryDebug::ST_BOUNDARY_TYPE_UNKNOWN);
          break;
        case STBoundary::BoundaryType::YIELD:
          boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_YIELD);
          break;
        case STBoundary::BoundaryType::KEEP_CLEAR:
          boundary_debug->set_type(
              StGraphBoundaryDebug::ST_BOUNDARY_TYPE_KEEP_CLEAR);
          break;
      }
  
      for (const auto &point : boundary->points()) {
        auto point_debug = boundary_debug->add_point();
        point_debug->set_t(point.x());
        point_debug->set_s(point.y());
      }
    }
  
    for (const auto &point : st_graph_data.speed_limit().speed_limit_points()) {
      common::SpeedPoint speed_point;
      speed_point.set_s(point.first);
      speed_point.set_v(point.second);
      st_graph_debug->add_speed_limit()->CopyFrom(speed_point);
    }
  }
  
  }  // namespace planning
  }  // namespace apollo
  