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.
   *****************************************************************************/
  
  /**
   * @file
   **/
  
  #include <string>
  #include <vector>
  
  #include "modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_provider.h"
  
  #include "cyber/task/task.h"
  #include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
  #include "modules/planning/common/planning_gflags.h"
  #include "modules/planning/common/trajectory/publishable_trajectory.h"
  #include "modules/planning/common/trajectory_stitcher.h"
  
  namespace apollo {
  namespace planning {
  
  using apollo::common::ErrorCode;
  using apollo::common::Status;
  using apollo::common::TrajectoryPoint;
  using apollo::common::time::Clock;
  
  OpenSpaceTrajectoryProvider::OpenSpaceTrajectoryProvider(
      const TaskConfig& config)
      : TrajectoryOptimizer(config) {
    open_space_trajectory_optimizer_.reset(new OpenSpaceTrajectoryOptimizer(
        config.open_space_trajectory_provider_config()
            .open_space_trajectory_optimizer_config()));
  }
  
  OpenSpaceTrajectoryProvider::~OpenSpaceTrajectoryProvider() {
    if (FLAGS_enable_open_space_planner_thread) {
      Stop();
    }
  }
  
  // TODO(Jinyun) Fix "no associate state" error.
  void OpenSpaceTrajectoryProvider::Stop() {
    if (FLAGS_enable_open_space_planner_thread) {
      is_stop_.store(true);
      task_future_.get();
      trajectory_updated_.store(false);
      trajectory_error_.store(false);
      trajectory_skipped_.store(false);
      optimizer_thread_counter = 0;
    }
  }
  
  // TODO(Jinyun) Fix "no associate state error". Driving out of parking spot not
  // supported right now
  void OpenSpaceTrajectoryProvider::Restart() {
    if (FLAGS_enable_open_space_planner_thread) {
      is_stop_.store(true);
      task_future_.get();
      is_stop_.store(false);
      thread_init_flag_ = false;
      trajectory_updated_.store(false);
      trajectory_error_.store(false);
      trajectory_skipped_.store(false);
      optimizer_thread_counter = 0;
    }
  }
  
  Status OpenSpaceTrajectoryProvider::Process() {
    auto trajectory_data =
        frame_->mutable_open_space_info()->mutable_stitched_trajectory_result();
    // Start thread when getting in Process() for the first time
    if (FLAGS_enable_open_space_planner_thread && !thread_init_flag_) {
      task_future_ = cyber::Async(
          &OpenSpaceTrajectoryProvider::GenerateTrajectoryThread, this);
      thread_init_flag_ = true;
    }
    // Get stitching trajectory from last frame
    const double start_timestamp = Clock::NowInSeconds();
    const common::VehicleState vehicle_state = frame_->vehicle_state();
    const double planning_cycle_time = FLAGS_open_space_planning_period;
    std::string replan_reason;
    auto previous_frame = FrameHistory::Instance()->Latest();
    // Use complete raw trajectory from last frame for stitching purpose
    const auto& previous_planning =
        previous_frame->open_space_info().stitched_trajectory_result();
    const auto& previous_planning_header =
        previous_frame->current_frame_planned_trajectory()
            .header()
            .timestamp_sec();
    PublishableTrajectory last_frame_complete_trajectory(previous_planning_header,
                                                         previous_planning);
    auto stitching_trajectory = TrajectoryStitcher::ComputeStitchingTrajectory(
        vehicle_state, start_timestamp, planning_cycle_time,
        &last_frame_complete_trajectory, &replan_reason);
  
    // Get open_space_info from current frame
    const auto& open_space_info = frame_->open_space_info();
  
    if (FLAGS_enable_open_space_planner_thread) {
      ADEBUG << "Open space plan in multi-threads mode";
  
      if (is_stop_) {
        GenerateStopTrajectory(trajectory_data);
        return Status(ErrorCode::OK, "Parking finished");
      }
  
      {
        std::lock_guard<std::mutex> lock(open_space_mutex_);
        thread_data_.stitching_trajectory = stitching_trajectory;
        thread_data_.end_pose = open_space_info.open_space_end_pose();
        thread_data_.rotate_angle = open_space_info.origin_heading();
        thread_data_.translate_origin = open_space_info.origin_point();
        thread_data_.obstacles_edges_num = open_space_info.obstacles_edges_num();
        thread_data_.obstacles_A = open_space_info.obstacles_A();
        thread_data_.obstacles_b = open_space_info.obstacles_b();
        thread_data_.obstacles_vertices_vec =
            open_space_info.obstacles_vertices_vec();
        thread_data_.XYbounds = open_space_info.ROI_xy_boundary();
      }
  
      // Check vehicle state
      if (IsVehicleNearDestination(
              vehicle_state, open_space_info.open_space_end_pose(),
              open_space_info.origin_heading(), open_space_info.origin_point())) {
        GenerateStopTrajectory(trajectory_data);
        is_stop_.store(true);
        return Status(ErrorCode::OK, "Vehicle is near to destination");
      }
  
      // Check if trajectory updated
      if (trajectory_updated_) {
        std::lock_guard<std::mutex> lock(open_space_mutex_);
        LoadResult(trajectory_data);
        trajectory_updated_.store(false);
        return Status::OK();
      }
  
      if (trajectory_error_) {
        ++optimizer_thread_counter;
        trajectory_error_.store(false);
        if (optimizer_thread_counter > 5) {
          return Status(ErrorCode::PLANNING_ERROR,
                        "open_space_optimizer failed too many times");
        }
      }
  
      if (previous_frame->open_space_info().open_space_provider_success()) {
        ReuseLastFrameResult(previous_frame, trajectory_data);
        return Status(ErrorCode::OK,
                      "Waiting for open_space_trajectory_optimizer in "
                      "open_space_trajectory_provider");
      } else {
        GenerateStopTrajectory(trajectory_data);
        return Status(ErrorCode::OK, "Stop due to computation not finished");
      }
    } else {
      const auto& end_pose = open_space_info.open_space_end_pose();
      const auto& rotate_angle = open_space_info.origin_heading();
      const auto& translate_origin = open_space_info.origin_point();
      const auto& obstacles_edges_num = open_space_info.obstacles_edges_num();
      const auto& obstacles_A = open_space_info.obstacles_A();
      const auto& obstacles_b = open_space_info.obstacles_b();
      const auto& obstacles_vertices_vec =
          open_space_info.obstacles_vertices_vec();
      const auto& XYbounds = open_space_info.ROI_xy_boundary();
  
      // Check vehicle state
      if (IsVehicleNearDestination(vehicle_state, end_pose, rotate_angle,
                                   translate_origin)) {
        GenerateStopTrajectory(trajectory_data);
        return Status(ErrorCode::OK, "Vehicle is near to destination");
      }
  
      // Generate Trajectory;
      Status status = open_space_trajectory_optimizer_->Plan(
          stitching_trajectory, end_pose, XYbounds, rotate_angle,
          translate_origin, obstacles_edges_num, obstacles_A, obstacles_b,
          obstacles_vertices_vec);
  
      // If status is OK, update vehicle trajectory;
      if (status == Status::OK()) {
        LoadResult(trajectory_data);
        return status;
      } else {
        return status;
      }
    }
    return Status(ErrorCode::PLANNING_ERROR);
  }
  
  void OpenSpaceTrajectoryProvider::GenerateTrajectoryThread() {
    while (!is_stop_) {
      OpenSpaceTrajectoryThreadData thread_data;
      {
        std::lock_guard<std::mutex> lock(open_space_mutex_);
        thread_data = thread_data_;
      }
      if (!trajectory_updated_) {
        Status status = open_space_trajectory_optimizer_->Plan(
            thread_data.stitching_trajectory, thread_data.end_pose,
            thread_data.XYbounds, thread_data.rotate_angle,
            thread_data.translate_origin, thread_data.obstacles_edges_num,
            thread_data.obstacles_A, thread_data.obstacles_b,
            thread_data_.obstacles_vertices_vec);
        if (status == Status::OK()) {
          trajectory_updated_.store(true);
        } else {
          AERROR_EVERY(200) << "open_space_trajectory_optimizer not returning "
                               "OK() with status: "
                            << status.ToString();
          if (status.ok()) {
            trajectory_skipped_.store(true);
          } else {
            trajectory_error_.store(true);
          }
        }
      }
    }
  }
  
  bool OpenSpaceTrajectoryProvider::IsVehicleNearDestination(
      const common::VehicleState& vehicle_state,
      const std::vector<double>& end_pose, double rotate_angle,
      const Vec2d& translate_origin) {
    CHECK_EQ(end_pose.size(), 4);
    Vec2d end_pose_to_world_frame = Vec2d(end_pose[0], end_pose[1]);
  
    end_pose_to_world_frame.SelfRotate(rotate_angle);
    end_pose_to_world_frame += translate_origin;
  
    double distance_to_vehicle =
        std::sqrt((vehicle_state.x() - end_pose_to_world_frame.x()) *
                      (vehicle_state.x() - end_pose_to_world_frame.x()) +
                  (vehicle_state.y() - end_pose_to_world_frame.y()) *
                      (vehicle_state.y() - end_pose_to_world_frame.y()));
  
    if (distance_to_vehicle < config_.open_space_trajectory_provider_config()
                                  .open_space_trajectory_optimizer_config()
                                  .is_near_destination_threshold()) {
      ADEBUG << "vehicle reach end_pose";
      frame_->mutable_open_space_info()->set_destination_reached(true);
      return true;
    }
    return false;
  }
  
  void OpenSpaceTrajectoryProvider::GenerateStopTrajectory(
      DiscretizedTrajectory* const trajectory_data) {
    double relative_time = 0.0;
    // TODO(Jinyun) Move to conf
    constexpr int stop_trajectory_length = 10;
    constexpr double relative_stop_time = 0.1;
    trajectory_data->clear();
    for (size_t i = 0; i < stop_trajectory_length; i++) {
      TrajectoryPoint point;
      point.mutable_path_point()->set_x(frame_->vehicle_state().x());
      point.mutable_path_point()->set_y(frame_->vehicle_state().y());
      point.mutable_path_point()->set_theta(frame_->vehicle_state().heading());
      point.mutable_path_point()->set_s(0.0);
      point.mutable_path_point()->set_kappa(0.0);
      point.set_relative_time(relative_time);
      point.set_v(0.0);
      point.set_a(0.0);
      trajectory_data->emplace_back(point);
      relative_time += relative_stop_time;
    }
  }
  
  void OpenSpaceTrajectoryProvider::LoadResult(
      DiscretizedTrajectory* const trajectory_data) {
    // Load unstitched two trajectories into frame for debug
    trajectory_data->clear();
    auto optimizer_trajectory_ptr =
        frame_->mutable_open_space_info()->mutable_optimizer_trajectory_data();
    auto stitching_trajectory_ptr =
        frame_->mutable_open_space_info()->mutable_stitching_trajectory_data();
    open_space_trajectory_optimizer_->GetOptimizedTrajectory(
        optimizer_trajectory_ptr);
    open_space_trajectory_optimizer_->GetStitchingTrajectory(
        stitching_trajectory_ptr);
    // Stitch two trajectories and load back to trajectory_data from frame
    size_t optimizer_trajectory_size = optimizer_trajectory_ptr->size();
    double stitching_point_relative_time =
        stitching_trajectory_ptr->back().relative_time();
    double stitching_point_relative_s =
        stitching_trajectory_ptr->back().path_point().s();
    for (size_t i = 0; i < optimizer_trajectory_size; ++i) {
      optimizer_trajectory_ptr->at(i).set_relative_time(
          optimizer_trajectory_ptr->at(i).relative_time() +
          stitching_point_relative_time);
      optimizer_trajectory_ptr->at(i).mutable_path_point()->set_s(
          optimizer_trajectory_ptr->at(i).path_point().s() +
          stitching_point_relative_s);
    }
    *(trajectory_data) = *(optimizer_trajectory_ptr);
  
    // Last point in stitching trajectory is already in optimized trajectory, so
    // it is deleted
    frame_->mutable_open_space_info()
        ->mutable_stitching_trajectory_data()
        ->pop_back();
    trajectory_data->PrependTrajectoryPoints(
        frame_->open_space_info().stitching_trajectory_data());
    frame_->mutable_open_space_info()->set_open_space_provider_success(true);
  }
  
  void OpenSpaceTrajectoryProvider::ReuseLastFrameResult(
      const Frame* last_frame, DiscretizedTrajectory* const trajectory_data) {
    *(trajectory_data) =
        last_frame->open_space_info().stitched_trajectory_result();
    frame_->mutable_open_space_info()->set_open_space_provider_success(true);
  }
  
  }  // namespace planning
  }  // namespace apollo
  