path_bounds_decider.h

Go to the documentation of this file.

/******************************************************************************
 * 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.
 *****************************************************************************/

#pragma once

#include <memory>
#include <string>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>

#include "gtest/gtest.h"

#include "modules/planning/proto/planning_config.pb.h"

#include "modules/planning/tasks/deciders/decider.h"

namespace apollo {
namespace planning {

constexpr double kPathBoundsDeciderHorizon = 100.0;
constexpr double kPathBoundsDeciderResolution = 0.5;
constexpr double kDefaultLaneWidth = 5.0;
constexpr double kDefaultRoadWidth = 20.0;
// TODO(all): Update extra tail point base on vehicle speed.
constexpr int kNumExtraTailBoundPoint = 20;
constexpr double kPulloverLonSearchCoeff = 1.5;
constexpr double kPulloverLatSearchCoeff = 1.25;

class PathBoundsDecider : public Decider {
 public:
  enum class LaneBorrowInfo {
    LEFT_BORROW,
    NO_BORROW,
    RIGHT_BORROW,
  };
  PathBoundsDecider(const TaskConfig& config,
                    const std::shared_ptr<DependencyInjector>& injector);

 private:
  common::Status Process(Frame* frame,
                         ReferenceLineInfo* reference_line_info) override;

  // Below are functions called every frame when executing PathBoundsDecider.

  void InitPathBoundsDecider(const Frame& frame,
                             const ReferenceLineInfo& reference_line_info);

  common::TrajectoryPoint InferFrontAxeCenterFromRearAxeCenter(
      const common::TrajectoryPoint& traj_point);

  common::Status GenerateRegularPathBound(
      const ReferenceLineInfo& reference_line_info,
      const LaneBorrowInfo& lane_borrow_info,
      std::vector<std::tuple<double, double, double>>* const path_bound,
      std::string* const blocking_obstacle_id,
      std::string* const borrow_lane_type);

  common::Status GenerateFallbackPathBound(
      const ReferenceLineInfo& reference_line_info,
      std::vector<std::tuple<double, double, double>>* const path_bound);

  common::Status GenerateLaneChangePathBound(
      const ReferenceLineInfo& reference_line_info,
      std::vector<std::tuple<double, double, double>>* const path_bound);

  common::Status GeneratePullOverPathBound(
      const Frame& frame, const ReferenceLineInfo& reference_line_info,
      std::vector<std::tuple<double, double, double>>* const path_bound);

  int IsPointWithinPathBound(
      const ReferenceLineInfo& reference_line_info, const double x,
      const double y,
      const std::vector<std::tuple<double, double, double>>& path_bound);

  bool FindDestinationPullOverS(
      const Frame& frame, const ReferenceLineInfo& reference_line_info,
      const std::vector<std::tuple<double, double, double>>& path_bound,
      double* pull_over_s);
  bool FindEmergencyPullOverS(const ReferenceLineInfo& reference_line_info,
                              double* pull_over_s);

  bool SearchPullOverPosition(
      const Frame& frame, const ReferenceLineInfo& reference_line_info,
      const std::vector<std::tuple<double, double, double>>& path_bound,
      std::tuple<double, double, double, int>* const pull_over_configuration);

  void RemoveRedundantPathBoundaries(
      std::vector<PathBoundary>* const candidate_path_boundaries);

  bool IsContained(const std::vector<std::pair<double, double>>& lhs,
                   const std::vector<std::pair<double, double>>& rhs);

  // Below are functions called when generating path bounds.

  bool InitPathBoundary(
      const ReferenceLineInfo& reference_line_info,
      std::vector<std::tuple<double, double, double>>* const path_bound);

  bool GetBoundaryFromRoads(
      const ReferenceLineInfo& reference_line_info,
      std::vector<std::tuple<double, double, double>>* const path_bound);

  bool GetBoundaryFromLanes(
      const ReferenceLineInfo& reference_line_info,
      const LaneBorrowInfo& lane_borrow_info,
      std::vector<std::tuple<double, double, double>>* const path_bound,
      std::string* const borrow_lane_type);

  bool GetBoundaryFromADC(
      const ReferenceLineInfo& reference_line_info, double ADC_extra_buffer,
      std::vector<std::tuple<double, double, double>>* const path_bound);

  bool GetBoundaryFromLanesAndADC(
      const ReferenceLineInfo& reference_line_info,
      const LaneBorrowInfo& lane_borrow_info, double ADC_buffer,
      std::vector<std::tuple<double, double, double>>* const path_bound,
      std::string* const borrow_lane_type, bool is_fallback_lanechange = false);

  void UpdatePullOverBoundaryByLaneBoundary(
      const ReferenceLineInfo& reference_line_info,
      std::vector<std::tuple<double, double, double>>* const path_bound);

  void ConvertBoundarySAxisFromLaneCenterToRefLine(
      const ReferenceLineInfo& reference_line_info,
      std::vector<std::tuple<double, double, double>>* const path_bound);

  void GetBoundaryFromLaneChangeForbiddenZone(
      const ReferenceLineInfo& reference_line_info,
      std::vector<std::tuple<double, double, double>>* const path_bound);

  bool GetBoundaryFromStaticObstacles(
      const PathDecision& path_decision,
      std::vector<std::tuple<double, double, double>>* const path_boundaries,
      std::string* const blocking_obstacle_id);

  std::vector<std::tuple<int, double, double, double, std::string>>
  SortObstaclesForSweepLine(
      const IndexedList<std::string, Obstacle>& indexed_obstacles);

  std::vector<std::vector<std::tuple<double, double, double>>>
  ConstructSubsequentPathBounds(
      const std::vector<std::tuple<int, double, double, double, std::string>>&
          sorted_obstacles,
      size_t path_idx, size_t obs_idx,
      std::unordered_map<std::string, std::tuple<bool, double>>* const
          obs_id_to_details,
      std::vector<std::tuple<double, double, double>>* const curr_path_bounds);

  std::vector<std::vector<bool>> DecidePassDirections(
      double l_min, double l_max,
      const std::vector<std::tuple<int, double, double, double, std::string>>&
          new_entering_obstacles);

  // Below are several helper functions:

  double GetBufferBetweenADCCenterAndEdge();

  bool UpdatePathBoundaryWithBuffer(
      size_t idx, double left_bound, double right_bound,
      std::vector<std::tuple<double, double, double>>* const path_boundaries,
      bool is_left_lane_bound = false, bool is_right_lane_bound = false);

  bool UpdatePathBoundaryAndCenterLineWithBuffer(
      size_t idx, double left_bound, double right_bound,
      std::vector<std::tuple<double, double, double>>* const path_boundaries,
      double* const center_line);

  bool UpdatePathBoundary(
      size_t idx, double left_bound, double right_bound,
      std::vector<std::tuple<double, double, double>>* const path_boundaries);

  void TrimPathBounds(
      const int path_blocked_idx,
      std::vector<std::tuple<double, double, double>>* const path_boundaries);

  void PathBoundsDebugString(
      const std::vector<std::tuple<double, double, double>>& path_boundaries);

  bool CheckLaneBoundaryType(const ReferenceLineInfo& reference_line_info,
                             const double check_s,
                             const LaneBorrowInfo& lane_borrow_info);

  void RecordDebugInfo(
      const std::vector<std::tuple<double, double, double>>& path_boundaries,
      const std::string& debug_name,
      ReferenceLineInfo* const reference_line_info);

 private:
  double adc_frenet_s_ = 0.0;
  double adc_frenet_sd_ = 0.0;
  double adc_frenet_l_ = 0.0;
  double adc_frenet_ld_ = 0.0;
  double adc_l_to_lane_center_ = 0.0;
  double adc_lane_width_ = 0.0;

  FRIEND_TEST(PathBoundsDeciderTest, InitPathBoundary);
  FRIEND_TEST(PathBoundsDeciderTest, GetBoundaryFromLanesAndADC);
};

}  // namespace planning
}  // namespace apollo