path_bounds_decider_util.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2023 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 <string>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include "modules/common/util/point_factory.h"
#include "modules/planning/planning_base/common/path_boundary.h"
#include "modules/planning/planning_base/common/reference_line_info.h"
#include "modules/planning/planning_base/common/sl_polygon.h"
 
namespace apollo {
namespace planning {
// ObstacleEdge contains: (is_start_s, s, l_min, l_max, obstacle_id).
using ObstacleEdge = std::tuple<int, double, double, double, std::string>;
// SLSstate contains: (s ,s' ,s''), (l, l', l'')
using SLState = std::pair<std::array<double, 3>, std::array<double, 3>>;
 
enum class LaneBorrowInfo {
  LEFT_BORROW,
  NO_BORROW,
  RIGHT_BORROW,
};
 
class PathBoundsDeciderUtil {
 public:
  static bool InitPathBoundary(const ReferenceLineInfo& reference_line_info,
                               PathBoundary* const path_bound,
                               SLState init_sl_state);
  static void GetStartPoint(common::TrajectoryPoint planning_start_point,
                            const ReferenceLine& reference_line,
                            SLState* init_sl_state);
  static double GetADCLaneWidth(const ReferenceLine& reference_line,
                                const double adc_frenet_s);
  static bool GetBoundaryFromLanesAndADC(
      const ReferenceLineInfo& reference_line_info,
      const LaneBorrowInfo& lane_borrow_info, bool is_extend_adc,
      double ADC_buffer, PathBoundary* const path_bound,
      std::string* const borrow_lane_type, bool is_fallback_lanechange,
      const SLState& init_sl_state);
  static bool UpdatePathBoundaryWithBuffer(
      double left_bound, double right_bound, BoundType left_type,
      BoundType right_type, std::string left_id, std::string right_id,
      PathBoundPoint* const bound_point);
  static bool UpdateRightPathBoundaryWithBuffer(
      double right_bound, BoundType right_type, std::string right_id,
      PathBoundPoint* const bound_point);
  static bool UpdateLeftPathBoundaryWithBuffer(
      double left_bound, BoundType left_type, std::string left_id,
      PathBoundPoint* const bound_point);
  static void TrimPathBounds(const int path_blocked_idx,
                             PathBoundary* const path_boundaries);
  static std::string FindFarthestBlockObstaclesId(
      const std::unordered_map<std::string, double>& obs_id_to_start_s);
  static bool GetBoundaryFromStaticObstacles(
      const ReferenceLineInfo& reference_line_info,
      std::vector<SLPolygon>* const sl_polygons, const SLState& init_sl_state,
      PathBoundary* const path_boundaries,
      std::string* const blocking_obstacle_id, double* const narrowest_width);
 
  static std::vector<ObstacleEdge> SortObstaclesForSweepLine(
      const IndexedList<std::string, Obstacle>& indexed_obstacles,
      const SLState& init_sl_state);
  static bool UpdatePathBoundaryAndCenterLineWithBuffer(
      size_t idx, double left_bound, double right_bound,
      PathBoundary* const path_boundaries, double* const center_line);
  static double GetBufferBetweenADCCenterAndEdge();
  static bool IsWithinPathDeciderScopeObstacle(const Obstacle& obstacle);
  static bool ComputeSLBoundaryIntersection(const SLBoundary& sl_boundary,
                                            const double s, double* ptr_l_min,
                                            double* ptr_l_max);
  static common::TrajectoryPoint InferFrontAxeCenterFromRearAxeCenter(
      const common::TrajectoryPoint& traj_point);
  static bool GetBoundaryFromSelfLane(
      const ReferenceLineInfo& reference_line_info,
      const SLState& init_sl_state, PathBoundary* const path_bound);
 
  static bool GetBoundaryFromRoad(const ReferenceLineInfo& reference_line_info,
                                  const SLState& init_sl_state,
                                  PathBoundary* const path_bound);
 
  static bool ExtendBoundaryByADC(const ReferenceLineInfo& reference_line_info,
                                  const SLState& init_sl_state,
                                  const double extend_buffer,
                                  PathBoundary* const path_bound);
  static void ConvertBoundarySAxisFromLaneCenterToRefLine(
      const ReferenceLineInfo& reference_line_info,
      PathBoundary* const path_bound);
  static int IsPointWithinPathBound(
      const ReferenceLineInfo& reference_line_info, const double x,
      const double y, const PathBound& path_bound);
  static void GetSLPolygons(const ReferenceLineInfo& reference_line_info,
                            std::vector<SLPolygon>* polygons,
                            const SLState& init_sl_state);
  static bool UpdatePathBoundaryBySLPolygon(
      const ReferenceLineInfo& reference_line_info,
      std::vector<SLPolygon>* const sl_polygon, const SLState& init_sl_state,
      PathBoundary* const path_boundary, std::string* const blocked_id,
      double* const narrowest_width);
  static bool AddCornerPoint(double s, double l_lower, double l_upper,
                             const PathBoundary& path_boundary,
                             ObsCornerConstraints* extra_constraints);
  static bool AddCornerPoint(SLPoint sl_pt, const PathBoundary& path_boundary,
                             ObsCornerConstraints* extra_constraints,
                             bool is_left, std::string obs_id,
                             bool is_front_pt);
  static void AddCornerBounds(const std::vector<SLPolygon>& sl_polygons,
                              PathBoundary* const path_boundary);
  static void AddAdcVertexBounds(PathBoundary* const path_boundary);
  static bool RelaxBoundaryPoint(PathBoundPoint* const path_bound_point,
                                 bool is_left, double init_l, double heading,
                                 double delta_s, double init_frenet_kappa,
                                 double min_radius);
  static bool RelaxEgoPathBoundary(PathBoundary* const path_boundary,
                                   const SLState& init_sl_state);
  static bool RelaxObsCornerBoundary(PathBoundary* const path_boundary,
                                     const SLState& init_sl_state);
 
  static bool AddExtraPathBound(const std::vector<SLPolygon>& sl_polygons,
                                PathBoundary* const path_boundary,
                                const SLState& init_sl_state,
                                std::string* const blocked_id);
  static bool UpdateBlockInfoWithObsCornerBoundary(
      PathBoundary* const path_boundary, std::string* const blocked_id);
};
 
}  // namespace planning
}  // namespace apollo