reference_line_info.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2017 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 <limits>
#include <list>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/common_msgs/basic_msgs/drive_state.pb.h"
#include "modules/common_msgs/basic_msgs/pnc_point.pb.h"
#include "modules/common_msgs/planning_msgs/planning.pb.h"
#include "modules/planning/planning_base/proto/lattice_structure.pb.h"
 
#include "modules/map/hdmap/hdmap_common.h"
#include "modules/map/pnc_map/route_segments.h"
#include "modules/planning/planning_base/common/path/path_data.h"
#include "modules/planning/planning_base/common/path_boundary.h"
#include "modules/planning/planning_base/common/path_decision.h"
#include "modules/planning/planning_base/common/planning_context.h"
#include "modules/planning/planning_base/common/speed/speed_data.h"
#include "modules/planning/planning_base/common/st_graph_data.h"
#include "modules/planning/planning_base/common/trajectory/discretized_trajectory.h"
#include "modules/planning/planning_base/gflags/planning_gflags.h"
#include "modules/planning/planning_base/common/sl_polygon.h"
 
namespace apollo {
namespace planning {
 
class ReferenceLineInfo {
 public:
  enum class LaneType { LeftForward, LeftReverse, RightForward, RightReverse };
  ReferenceLineInfo() = default;
 
  ReferenceLineInfo(const common::VehicleState& vehicle_state,
                    const common::TrajectoryPoint& adc_planning_point,
                    const ReferenceLine& reference_line,
                    const hdmap::RouteSegments& segments);
 
  bool Init(const std::vector<const Obstacle*>& obstacles, double target_speed);
 
  bool AddObstacles(const std::vector<const Obstacle*>& obstacles);
  Obstacle* AddObstacle(const Obstacle* obstacle);
 
  const common::VehicleState& vehicle_state() const { return vehicle_state_; }
 
  PathDecision* path_decision();
  const PathDecision& path_decision() const;
 
  const ReferenceLine& reference_line() const;
  ReferenceLine* mutable_reference_line();
 
  double SDistanceToDestination() const;
  double SDistanceToRefEnd() const;
 
  bool ReachedDestination() const;
 
  void SetTrajectory(const DiscretizedTrajectory& trajectory);
  const DiscretizedTrajectory& trajectory() const;
 
  double Cost() const { return cost_; }
  void AddCost(double cost) { cost_ += cost; }
  void SetCost(double cost) { cost_ = cost; }
  double PriorityCost() const { return priority_cost_; }
  void SetPriorityCost(double cost) { priority_cost_ = cost; }
  // For lattice planner'speed planning target
  void SetLatticeStopPoint(const StopPoint& stop_point);
  void SetLatticeCruiseSpeed(double speed);
  const PlanningTarget& planning_target() const { return planning_target_; }
 
  void SetCruiseSpeed(double speed) {
    cruise_speed_ = speed;
    base_cruise_speed_ = speed;
  }
  void LimitCruiseSpeed(double speed);
 
  double GetBaseCruiseSpeed() const;
 
  double GetCruiseSpeed() const;
 
  hdmap::LaneInfoConstPtr LocateLaneInfo(const double s) const;
 
  bool GetNeighborLaneInfo(const ReferenceLineInfo::LaneType lane_type,
                           const double s, hdmap::Id* ptr_lane_id,
                           double* ptr_lane_width) const;
 
  bool IsStartFrom(const ReferenceLineInfo& previous_reference_line_info) const;
 
  planning_internal::Debug* mutable_debug() { return &debug_; }
  const planning_internal::Debug& debug() const { return debug_; }
  LatencyStats* mutable_latency_stats() { return &latency_stats_; }
  const LatencyStats& latency_stats() const { return latency_stats_; }
 
  const PathData& path_data() const;
  const PathData& fallback_path_data() const;
  const SpeedData& speed_data() const;
  PathData* mutable_path_data();
  PathData* mutable_fallback_path_data();
  SpeedData* mutable_speed_data();
 
  const RSSInfo& rss_info() const;
  RSSInfo* mutable_rss_info();
  // aggregate final result together by some configuration
  bool CombinePathAndSpeedProfile(
      const double relative_time, const double start_s,
      DiscretizedTrajectory* discretized_trajectory);
 
  // adjust trajectory if it starts from cur_vehicle postion rather planning
  // init point from upstream
  bool AdjustTrajectoryWhichStartsFromCurrentPos(
      const common::TrajectoryPoint& planning_start_point,
      const std::vector<common::TrajectoryPoint>& trajectory,
      DiscretizedTrajectory* adjusted_trajectory);
 
  const SLBoundary& AdcSlBoundary() const;
  std::string PathSpeedDebugString() const;
  void PrintReferenceSegmentDebugString();
 
  bool IsChangeLanePath() const;
 
  bool IsNeighborLanePath() const;
 
  void SetDrivable(bool drivable);
  bool IsDrivable() const;
 
  void ExportEngageAdvice(common::EngageAdvice* engage_advice,
                          PlanningContext* planning_context) const;
 
  const hdmap::RouteSegments& Lanes() const;
  std::list<hdmap::Id> TargetLaneId() const;
 
  void ExportDecision(DecisionResult* decision_result,
                      PlanningContext* planning_context) const;
 
  void SetJunctionRightOfWay(const double junction_s,
                             const bool is_protected) const;
 
  ADCTrajectory::RightOfWayStatus GetRightOfWayStatus() const;
 
  hdmap::Lane::LaneTurn GetPathTurnType(const double s) const;
 
  bool GetIntersectionRightofWayStatus(
      const hdmap::PathOverlap& pnc_junction_overlap) const;
 
  double OffsetToOtherReferenceLine() const {
    return offset_to_other_reference_line_;
  }
  void SetOffsetToOtherReferenceLine(const double offset) {
    offset_to_other_reference_line_ = offset;
  }
 
  const std::vector<PathBoundary>& GetCandidatePathBoundaries() const;
 
  void SetCandidatePathBoundaries(
      std::vector<PathBoundary>&& candidate_path_boundaries);
 
  const std::vector<PathData>& GetCandidatePathData() const;
  std::vector<PathData>* MutableCandidatePathData();
  void SetCandidatePathData(std::vector<PathData>&& candidate_path_data);
 
  Obstacle* GetBlockingObstacle() const { return blocking_obstacle_; }
  void SetBlockingObstacle(const std::string& blocking_obstacle_id);
 
  bool is_path_lane_borrow() const { return is_path_lane_borrow_; }
  void set_is_path_lane_borrow(const bool is_path_lane_borrow) {
    is_path_lane_borrow_ = is_path_lane_borrow;
  }
 
  void set_is_on_reference_line() { is_on_reference_line_ = true; }
 
  uint32_t GetPriority() const { return reference_line_.GetPriority(); }
 
  void SetPriority(uint32_t priority) { reference_line_.SetPriority(priority); }
 
  void set_trajectory_type(
      const ADCTrajectory::TrajectoryType trajectory_type) {
    trajectory_type_ = trajectory_type;
  }
 
  ADCTrajectory::TrajectoryType trajectory_type() const {
    return trajectory_type_;
  }
 
  StGraphData* mutable_st_graph_data() { return &st_graph_data_; }
 
  const StGraphData& st_graph_data() { return st_graph_data_; }
 
  // different types of overlaps that can be handled by different scenarios.
  enum OverlapType {
    CLEAR_AREA = 1,
    CROSSWALK = 2,
    OBSTACLE = 3,
    PNC_JUNCTION = 4,
    SIGNAL = 5,
    STOP_SIGN = 6,
    YIELD_SIGN = 7,
    JUNCTION = 8,
    AREA = 9,
  };
 
  const std::vector<std::pair<OverlapType, hdmap::PathOverlap>>&
  FirstEncounteredOverlaps() const {
    return first_encounter_overlaps_;
  }
 
  int GetPnCJunction(const double s,
                     hdmap::PathOverlap* pnc_junction_overlap) const;
  int GetJunction(const double s, hdmap::PathOverlap* junction_overlap) const;
  int GetArea(const double s, hdmap::PathOverlap* area_overlap) const;
  std::vector<common::SLPoint> GetAllStopDecisionSLPoint() const;
 
  void SetTurnSignal(const common::VehicleSignal::TurnSignal& turn_signal);
  void SetEmergencyLight();
 
  void set_path_reusable(const bool path_reusable) {
    path_reusable_ = path_reusable;
  }
 
  bool path_reusable() const { return path_reusable_; }
  hdmap::PathOverlap* GetOverlapOnReferenceLine(
      const std::string& overlap_id, const OverlapType& overlap_type) const;
 
  void set_key(std::size_t key) { key_ = key; }
  std::size_t key() const { return key_; }
 
  void set_index(std::size_t index) { index_ = index; }
  std::size_t index() const { return index_; }
 
  void set_id(std::string id) { id_ = id; }
  std::string id() const { return id_; }
 
  void GetRangeOverlaps(std::vector<hdmap::PathOverlap>* path_overlaps,
                        double start_s, double end_s);
 
  const std::vector<double>& reference_line_towing_l() const;
  std::vector<double>* mutable_reference_line_towing_l();
  const PathBoundary& reference_line_towing_path_boundary() const;
  PathBoundary* mutable_reference_line_towing_path_boundary();
 
  const std::vector<SLPolygon>& obs_sl_polygons() const;
  std::vector<SLPolygon>* mutable_obs_sl_polygons();
 
 private:
  void InitFirstOverlaps();
 
  bool CheckChangeLane() const;
 
  void SetTurnSignalBasedOnLaneTurnType(
      common::VehicleSignal* vehicle_signal) const;
 
  void ExportVehicleSignal(common::VehicleSignal* vehicle_signal) const;
 
  bool IsIrrelevantObstacle(const Obstacle& obstacle);
 
  void MakeDecision(DecisionResult* decision_result,
                    PlanningContext* planning_context) const;
 
  int MakeMainStopDecision(DecisionResult* decision_result) const;
 
  void MakeMainMissionCompleteDecision(DecisionResult* decision_result,
                                       PlanningContext* planning_context) const;
 
  void MakeEStopDecision(DecisionResult* decision_result) const;
 
  void SetObjectDecisions(ObjectDecisions* object_decisions) const;
 
  bool AddObstacleHelper(const std::shared_ptr<Obstacle>& obstacle);
 
  bool GetFirstOverlap(const std::vector<hdmap::PathOverlap>& path_overlaps,
                       hdmap::PathOverlap* path_overlap);
 
  bool IsEgoOnRoutingLane() const;
 
 private:
  static std::unordered_map<std::string, bool> junction_right_of_way_map_;
  const common::VehicleState vehicle_state_;
  const common::TrajectoryPoint adc_planning_point_;
  ReferenceLine reference_line_;
 
  double cost_ = 0.0;
 
  bool is_drivable_ = true;
 
  PathDecision path_decision_;
 
  Obstacle* blocking_obstacle_ = nullptr;
 
  std::vector<PathBoundary> candidate_path_boundaries_;
  std::vector<PathData> candidate_path_data_;
 
  std::vector<double> reference_line_towing_l_;
  PathBoundary reference_line_towing_path_boundary_;
 
  PathData path_data_;
  PathData fallback_path_data_;
  SpeedData speed_data_;
 
  DiscretizedTrajectory discretized_trajectory_;
 
  RSSInfo rss_info_;
 
  SLBoundary adc_sl_boundary_;
 
  planning_internal::Debug debug_;
  LatencyStats latency_stats_;
 
  hdmap::RouteSegments lanes_;
 
  bool is_on_reference_line_ = false;
 
  bool is_path_lane_borrow_ = false;
 
  ADCTrajectory::RightOfWayStatus status_ = ADCTrajectory::UNPROTECTED;
 
  double offset_to_other_reference_line_ = 0.0;
 
  double priority_cost_ = 0.0;
 
  PlanningTarget planning_target_;
 
  ADCTrajectory::TrajectoryType trajectory_type_ = ADCTrajectory::UNKNOWN;
 
  std::vector<std::pair<OverlapType, hdmap::PathOverlap>>
      first_encounter_overlaps_;
 
  StGraphData st_graph_data_;
 
  common::VehicleSignal vehicle_signal_;
 
  double cruise_speed_ = 0.0;
  double base_cruise_speed_ = 0.0;
 
  bool path_reusable_ = false;
  std::string id_ = "";
  std::size_t key_ = 0;
  std::size_t index_ = 0;
  std::vector<SLPolygon> obs_sl_polygons_;
 
  DISALLOW_COPY_AND_ASSIGN(ReferenceLineInfo);
};
 
}  // namespace planning
}  // namespace apollo