Apollo 10.0
自动驾驶开放平台
apollo::planning 命名空间参考

apollo::planning 更多...

命名空间

namespace  autotuning
 
namespace  util
 

struct  ADCTrajectory
 
struct  ADCTrajectoryPoint
 
class  ADCVertexConstraints
 
class  AffineConstraint
 
struct  AnchorPoint
 
class  AutotuningBaseModel
 
class  AutotuningFeatureBuilder
 : build model related input feature from raw feature generator 更多...
 
class  AutotuningMLPModel
 
class  AutotuningRawFeatureGenerator
 
class  AutotuningSpeedFeatureBuilder
 : build the mlp cost functional for speed profile based on trajectory raw feature 更多...
 
class  AutotuningSpeedMLPModel
 
class  BacksideVehicle
 
struct  BacksideVehicleConfig
 
class  BackupTrajectoryGenerator
 
struct  BareIntersectionUnprotectedContext
 
class  BareIntersectionUnprotectedScenario
 
class  BareIntersectionUnprotectedStageApproach
 
class  BareIntersectionUnprotectedStageIntersectionCruise
 
class  BaseStageCreep
 
class  BaseStageCruise
 
class  BaseStageTrafficLightCreep
 
class  BaseStageTrafficLightCruise
 
class  BirdviewImgFeatureRenderer
 
struct  BoundEdge
 
struct  ChangeLaneStatus
 
struct  ChassisFeature
 
class  CollisionChecker
 
struct  CommonPathPointFeature
 
struct  CommonTrajectoryPointFeature
 
class  ComparableCost
 
class  ConfigUtil
 
class  ConstantDecelerationTrajectory1d
 
class  ConstantJerkTrajectory1d
 
class  ConstraintChecker
 
class  ConstraintChecker1d
 
class  CosThetaIpoptInterface
 
class  CosThetaSmoother
 
struct  CosThetaSmootherConfig
 
struct  CreepDeciderStatus
 
struct  CreepStageConfig
 
class  Crosswalk
 
struct  CrosswalkConfig
 
struct  CrosswalkStatus
 
class  CubicPolynomialCurve1d
 
class  Curve1d
 
class  CurveMath
 
class  Decider
 
class  DecisionData
 
class  DependencyInjector
 
class  Destination
 This class decides whether we should stop for destination. 更多...
 
struct  DestinationConfig
 
struct  DestinationStatus
 
class  DiscretePointsMath
 
class  DiscretePointsReferenceLineSmoother
 
struct  DiscretePointsSmootherConfig
 
class  DiscretizedPath
 
class  DiscretizedTrajectory
 
struct  DistanceApproachConfig
 
class  DistanceApproachInterface
 
class  DistanceApproachIPOPTCUDAInterface
 
class  DistanceApproachIPOPTFixedDualInterface
 
class  DistanceApproachIPOPTFixedTsInterface
 
class  DistanceApproachIPOPTInterface
 
class  DistanceApproachIPOPTRelaxEndInterface
 
class  DistanceApproachIPOPTRelaxEndSlackInterface
 
class  DistanceApproachProblem
 
class  DpRoadGraph
 
class  DpStCost
 
struct  DpStSpeedOptimizerConfig
 
struct  DualVariableConfig
 
struct  DualVariableWarmStartConfig
 
class  DualVariableWarmStartIPOPTInterface
 
class  DualVariableWarmStartIPOPTQPInterface
 
class  DualVariableWarmStartOSQPInterface
 
class  DualVariableWarmStartProblem
 
class  DualVariableWarmStartSlackOSQPInterface
 
class  EgoInfo
 
struct  EmergencyPullOverContext
 
class  EmergencyPullOverScenario
 
class  EmergencyPullOverStageApproach
 
class  EmergencyPullOverStageSlowDown
 
class  EmergencyPullOverStageStandby
 
struct  EmergencyStopContext
 
class  EmergencyStopScenario
 
class  EmergencyStopStageApproach
 
class  EmergencyStopStageStandby
 
struct  EmergencyStopStatus
 
class  EndConditionSampler
 
struct  EnumClassHash
 
struct  EStop
 
class  Evaluator
 
class  EvaluatorLogger
 
class  FallbackPath
 
struct  FallbackPathConfig
 
class  FastStopTrajectoryFallback
 
class  FeasibleRegion
 
class  FeatureOutput
 
class  FemPosDeviationIpoptInterface
 
class  FemPosDeviationOsqpInterface
 
class  FemPosDeviationSmoother
 
struct  FemPosDeviationSmootherConfig
 
class  FemPosDeviationSqpOsqpInterface
 
struct  FollowDistance
 
struct  FollowDistanceSchedulerInfo
 
class  Frame
 Frame holds all data for one planning cycle. 更多...
 
class  FrameHistory
 
class  FrenetFramePath
 
struct  GearSwitchStates
 
struct  GridAStartResult
 
class  GriddedPathTimeGraph
 
class  GridSearch
 
class  History
 
class  HistoryFrame
 
class  HistoryObjectDecision
 
class  HistoryObjectStatus
 
class  HistoryStatus
 
class  HybridAObstacleContainer
 
class  HybridAResultContainer
 
class  HybridASoftBoundaryContainer
 
class  HybridAStar
 
struct  HybridAStartResult
 
class  IndexedList
 
class  IndexedQueue
 
struct  InterPolatedPoint
 
struct  IpoptConfig
 
struct  IpoptSolverConfig
 
struct  IterativeAnchoringConfig
 
class  IterativeAnchoringSmoother
 
class  KeepClear
 This class creates a virtual obstacle for each clear area region. 更多...
 
struct  KeepClearConfig
 
class  LaneBorrowPath
 
struct  LaneBorrowPathConfig
 
struct  LaneBorrowStatus
 
class  LaneChangePath
 
struct  LaneChangePathConfig
 
class  LaneFollowMap
 
class  LaneFollowPath
 
struct  LaneFollowPathConfig
 
class  LaneFollowScenario
 
class  LaneFollowStage
 
struct  LaneFollowStatus
 
struct  LatencyStats
 
class  LateralOSQPOptimizer
 
class  LateralQPOptimizer
 
class  LatticePlanner
 
class  LatticeTrajectory1d
 
class  LearningBasedData
 
struct  LearningData
 
struct  LearningDataFrame
 
struct  LearningModelInferenceTaskConfig
 
struct  LearningOutput
 
struct  LocalizationFeature
 
struct  LocalView
 
struct  LocationPose
 
class  MessageProcess
 
class  ModelInference
 
struct  MoveDestLaneConfigTable
 
class  NaviObstacleDecider
 NaviObstacleDecider is used to make appropriate decisions for obstacles around the vehicle in navigation mode. 更多...
 
struct  NaviObstacleDeciderConfig
 
class  NaviPathDecider
 NaviPathDecider is used to generate the local driving path of the 更多...
 
struct  NaviPathDeciderConfig
 
class  NaviPlanner
 NaviPlanner is a planner based on real-time relative maps. 更多...
 
class  NaviPlanning
 
class  NaviSpeedDecider
 NaviSpeedDecider is used to generate an appropriate speed curve of the vehicle in navigation mode. 更多...
 
struct  NaviSpeedDeciderConfig
 
struct  NaviSpeedTsConstraints
 NaviSpeedTsConstraints is used to describe constraints of a t-s point. 更多...
 
class  NaviSpeedTsGraph
 NaviSpeedTsGraph is used to generate a t-s graph with some limits and preferred. 更多...
 
struct  NaviSpeedTsPoint
 NaviSpeedTsPoint is used to describe a t-s point. 更多...
 
class  NaviTask
 
class  Node2d
 
class  Node3d
 
class  ObsCornerConstraints
 
class  Obstacle
 This is the class that associates an Obstacle with its path properties. 更多...
 
class  ObstacleContainer
 
struct  ObstacleFeature
 
struct  ObstacleTrajectoryFeature
 
class  OnLanePlanning
 
class  OpenSpaceFallbackDecider
 
struct  OpenSpaceFallBackDeciderConfig
 
class  OpenSpaceInfo
 
class  OpenSpacePreStopDecider
 
struct  OpenSpacePreStopDeciderConfig
 
class  OpenSpaceRoiDecider
 
struct  OpenSpaceRoiDeciderConfig
 
class  OpenSpaceROITest
 
class  OpenSpaceRoiUtil
 
struct  OpenSpaceStatus
 
class  OpenSpaceTrajectoryOptimizer
 
struct  OpenSpaceTrajectoryOptimizerConfig
 
class  OpenSpaceTrajectoryOptimizerUtil
 
class  OpenSpaceTrajectoryPartition
 
struct  OpenSpaceTrajectoryPartitionConfig
 
class  OpenSpaceTrajectoryProvider
 
struct  OpenSpaceTrajectoryProviderConfig
 
struct  OpenSpaceTrajectoryThreadData
 
struct  OSQPConfig
 
class  OsqpSpline1dSolver
 
class  OsqpSpline2dSolver
 
struct  OverlapFeature
 
struct  PadMessage
 
struct  ParkAndGoContext
 
class  ParkAndGoScenario
 
class  ParkAndGoStageAdjust
 
class  ParkAndGoStageCheck
 
class  ParkAndGoStageCruise
 
class  ParkAndGoStagePreCruise
 
struct  ParkAndGoStatus
 
struct  ParkingCommand
 
struct  ParkingInfo
 
class  PathAssessmentDeciderUtil
 
class  PathBoundary
 
struct  PathBoundPoint
 
class  PathBoundsDeciderUtil
 
class  PathData
 
class  PathDecider
 
struct  PathDeciderConfig
 
struct  PathDeciderStatus
 
class  PathDecision
 PathDecision represents all obstacle decisions on one path. 更多...
 
class  PathGeneration
 
class  PathOptimizerUtil
 
class  PathReferenceDecider
 
struct  PathReferenceDeciderConfig
 
class  PathTimeGraph
 
class  PathTimeHeuristicOptimizer
 PathTimeHeuristicOptimizer does ST graph speed planning with dynamic programming algorithm. 更多...
 
struct  PerceptionObstacleFeature
 
class  PiecewiseAccelerationTrajectory1d
 
class  PiecewiseBrakingTrajectoryGenerator
 
struct  PiecewiseJerkNonlinearSpeedOptimizerConfig
 
struct  PiecewiseJerkPathConfig
 
class  PiecewiseJerkPathProblem
 
class  PiecewiseJerkProblem
 
class  PiecewiseJerkSpeedNonlinearIpoptInterface
 
class  PiecewiseJerkSpeedNonlinearOptimizer
 
class  PiecewiseJerkSpeedOptimizer
 
struct  PiecewiseJerkSpeedOptimizerConfig
 
class  PiecewiseJerkSpeedProblem
 
class  PiecewiseJerkTrajectory1d
 
class  PiecewiseQuinticSpiralPath
 
class  PiecewiseTrajectory1d
 
class  Planner
 Planner is a base class for specific planners. 更多...
 
struct  PlannerNaviConfig
 
struct  PlannerOpenSpaceConfig
 
struct  PlannerPublicRoadConfig
 
class  PlannerWithReferenceLine
 
class  PlanningBase
 
struct  PlanningCommand
 
class  PlanningComponent
 
struct  PlanningConfig
 
class  PlanningContext
 
struct  PlanningLearningData
 
struct  PlanningSemanticMapConfig
 
struct  PlanningStatus
 
struct  PlanningTag
 
struct  PlanningTarget
 
class  PlanningTestBase
 
struct  PluginDeclareInfo
 
class  PncMapBase
 
struct  Point
 
class  PolynomialCurve1d
 
class  PolynomialXd
 
struct  PredictionObstacleFeature
 
class  PredictionQuerier
 
struct  PredictionTrajectoryFeature
 
class  PrintBox
 
class  PrintCurves
 
class  PrintPoints
 
class  PublicRoadPlanner
 PublicRoadPlanner is an expectation maximization planner. 更多...
 
class  PublishableTrajectory
 
struct  PullOverContext
 
class  PullOverPath
 
struct  PullOverPathConfig
 
class  PullOverScenario
 
class  PullOverStageApproach
 
class  PullOverStageRetryApproachParking
 
class  PullOverStageRetryParking
 
struct  PullOverStatus
 
struct  QPMatrix
 
class  QpSplineReferenceLineSmoother
 
struct  QpSplineSmootherConfig
 
struct  QuadraticProgrammingProblem
 
struct  QuadraticProgrammingProblemSet
 
class  QuarticPolynomialCurve1d
 
class  QuinticPolynomialCurve1d
 
class  QuinticSpiralPath
 Describe a quintic spiral path Map (theta0, kappa0, dkappa0) --— delta_s --—> (theta1, kappa1, dkappa1) 更多...
 
class  QuinticSpiralPathWithDerivation
 
class  ReedShepp
 
struct  ReedSheppPath
 
class  ReferenceLine
 
struct  ReferenceLineConfig
 
class  ReferenceLineEnd
 This class decides whether we should send rerouting request based on traffic situation. 更多...
 
struct  ReferenceLineEndConfig
 
class  ReferenceLineInfo
 ReferenceLineInfo holds all data for one reference line. 更多...
 
class  ReferenceLineProvider
 The class of ReferenceLineProvider. 更多...
 
class  ReferenceLineSmoother
 
struct  ReferenceLineSmootherConfig
 
class  ReferencePoint
 
class  Rerouting
 This class decides whether we should send rerouting request based on traffic situation. 更多...
 
struct  ReroutingConfig
 
struct  ReroutingStatus
 
class  ResultContainer
 
class  ReusePath
 
struct  ReusePathConfig
 
struct  ROIConfig
 
struct  RoutingFeature
 
struct  RoutingResponseFeature
 
struct  RSPParam
 
struct  rss_world_model_struct
 
class  RssDecider
 
struct  RssDeciderConfig
 
struct  RSSInfo
 
class  RTKReplayPlanner
 RTKReplayPlanner is a derived class of Planner. 更多...
 
class  RuleBasedStopDecider
 
struct  RuleBasedStopDeciderConfig
 
struct  SamplePoint
 
class  Scenario
 
struct  ScenarioBareIntersectionUnprotectedConfig
 
struct  ScenarioContext
 
struct  ScenarioEmergencyPullOverConfig
 
struct  ScenarioEmergencyStopConfig
 
class  ScenarioManager
 
struct  ScenarioParkAndGoConfig
 
struct  ScenarioPipeline
 
struct  ScenarioPullOverConfig
 
class  ScenarioResult
 
struct  ScenarioStatus
 
struct  ScenarioStopSignUnprotectedConfig
 
struct  ScenarioTrafficLightProtectedConfig
 
struct  ScenarioTrafficLightUnprotectedLeftTurnConfig
 
struct  ScenarioTrafficLightUnprotectedRightTurnConfig
 
struct  ScenarioValetParkingConfig
 
struct  ScenarioYieldSignConfig
 
struct  ShiftConfig
 
struct  SLBoundary
 
class  SLPolygon
 
class  Smoother
 
class  SmootherUtil
 
class  SpeedBoundsDecider
 
struct  SpeedBoundsDeciderConfig
 
class  SpeedData
 
class  SpeedDecider
 
struct  SpeedDeciderConfig
 
struct  SpeedDeciderStatus
 
struct  SpeedHeuristicOptimizerConfig
 
class  SpeedLimit
 
class  SpeedLimitDecider
 
class  SpeedOptimizer
 
struct  SpeedOptimizerConfig
 
class  SpeedProfileGenerator
 
class  SpiralProblemInterface
 
class  SpiralReferenceLineSmoother
 
struct  SpiralSmootherConfig
 
class  SpiralSmootherUtil
 
class  Spline1d
 
class  Spline1dConstraint
 
class  Spline1dKernel
 
class  Spline1dSeg
 
class  Spline1dSolver
 
class  Spline2d
 
class  Spline2dConstraint
 
class  Spline2dKernel
 
class  Spline2dSeg
 
class  Spline2dSolver
 
class  SplineSegKernel
 
class  Stage
 
class  StageApproachingParkingSpot
 
class  StageParking
 
struct  StagePipeline
 
class  StageResult
 
class  StandingStillTrajectory1d
 
class  STBoundary
 
class  STBoundaryMapper
 
class  STBoundsDecider
 
struct  STBoundsDeciderConfig
 
struct  STDrivableBoundary
 
struct  STDrivableBoundaryInstance
 
class  STDrivingLimits
 
class  StGapEstimator
 
class  StGraphData
 
class  StGraphPoint
 
class  STGuideLine
 
class  STObstaclesProcessor
 
struct  StopPoint
 
class  StopSign
 
struct  StopSignConfig
 
struct  StopSignStatus
 
struct  StopSignUnprotectedContext
 
class  StopSignUnprotectedScenario
 
class  StopSignUnprotectedStageCreep
 
class  StopSignUnprotectedStageIntersectionCruise
 
class  StopSignUnprotectedStagePreStop
 
class  StopSignUnprotectedStageStop
 
struct  StopTime
 
class  STPoint
 
class  Task
 
struct  TaskStats
 
class  ThreadSafeIndexedList
 
struct  TopicConfig
 
class  TrafficDecider
 Create traffic related decision in this class. 更多...
 
class  TrafficLight
 
struct  TrafficLightConfig
 
struct  TrafficLightDetectionFeature
 
struct  TrafficLightFeature
 
struct  TrafficLightProtectedContext
 
class  TrafficLightProtectedScenario
 
class  TrafficLightProtectedStageApproach
 
class  TrafficLightProtectedStageIntersectionCruise
 
struct  TrafficLightStatus
 
struct  TrafficLightUnprotectedLeftTurnContext
 
class  TrafficLightUnprotectedLeftTurnScenario
 
class  TrafficLightUnprotectedLeftTurnStageApproach
 
class  TrafficLightUnprotectedLeftTurnStageCreep
 
class  TrafficLightUnprotectedLeftTurnStageIntersectionCruise
 
struct  TrafficLightUnprotectedRightTurnContext
 
class  TrafficLightUnprotectedRightTurnScenario
 
class  TrafficLightUnprotectedRightTurnStageCreep
 
class  TrafficLightUnprotectedRightTurnStageIntersectionCruise
 
class  TrafficLightUnprotectedRightTurnStageStop
 
class  TrafficRule
 
struct  TrafficRulesPipeline
 
class  Trajectory1dGenerator
 
class  TrajectoryCombiner
 
class  TrajectoryCost
 
class  TrajectoryEvaluator
 
class  TrajectoryFallbackTask
 
class  TrajectoryImitationLibtorchInference
 
class  TrajectoryOptimizer
 
struct  TrajectoryPointFeature
 
class  TrajectoryStitcher
 
struct  ValetParkingContext
 
class  ValetParkingScenario
 
struct  WarmStartConfig
 
class  WaypointSampler
 
class  YieldSign
 
struct  YieldSignConfig
 
struct  YieldSignContext
 
class  YieldSignScenario
 
class  YieldSignStageApproach
 
class  YieldSignStageCreep
 
struct  YieldSignStatus
 

类型定义

using State = std::array< double, 3 >
 
using Condition = std::pair< State, double >
 
typedef std::vector< std::shared_ptr< Curve1d > > Trajectory1DBundle
 
using Trajectory1d = Curve1d
 
using PtrTrajectory1d = std::shared_ptr< Trajectory1d >
 
using Trajectory1dPair = std::pair< std::shared_ptr< Curve1d >, std::shared_ptr< Curve1d > >
 
typedef IndexedList< std::string, ObstacleIndexedObstacles
 
typedef ThreadSafeIndexedList< std::string, ObstacleThreadSafeIndexedObstacles
 
typedef std::pair< DiscretizedTrajectory, canbus::Chassis::GearPositionTrajGearPair
 
using PathBound = std::vector< PathBoundPoint >
 
using MapPath = hdmap::Path
 
using PathPointDecision = std::tuple< double, PathData::PathPointType, double >
 
using ObstacleEdge = std::tuple< int, double, double, double, std::string >
 
using SLState = std::pair< std::array< double, 3 >, std::array< double, 3 > >
 
using StopSignLaneVehicles = std::unordered_map< std::string, std::vector< std::string > >
 
using CrosswalkToStop = std::vector< std::pair< const hdmap::PathOverlap *, std::vector< std::string > > >
 
using CrosswalkStopTimer = std::unordered_map< std::string, std::unordered_map< std::string, double > >
 

枚举

enum  JucType {
  UNKNOWN = 0 , IN_ROAD = 1 , CROSS_ROAD = 2 , FORK_ROAD = 3 ,
  MAIN_SIDE = 4 , DEAD_END = 5
}
 
enum  NaviTaskType { NAVI_OBSTACLE_DECIDER = 3 , NAVI_PATH_DECIDER = 4 , NAVI_SPEED_DECIDER = 5 }
 
enum class  VirtualObjectType {
  DESTINATION = 0 , CROSSWALK = 1 , TRAFFIC_LIGHT = 2 , CLEAR_ZONE = 3 ,
  REROUTE = 4 , DECISION_JUMP = 5 , PRIORITY = 6
}
 
enum class  ParkingType { NO_PARKING , PARALLEL_PARKING , VERTICAL_PARKING }
 
enum  BoundType { ROAD = 0 , LANE = 1 , OBSTACLE = 2 , ADC = 3 }
 
enum class  StageStatusType { ERROR = 1 , READY = 2 , RUNNING = 3 , FINISHED = 4 }
 
enum class  ScenarioStatusType { STATUS_UNKNOWN = 0 , STATUS_PROCESSING = 1 , STATUS_DONE = 2 }
 
enum class  LaneBorrowInfo { LEFT_BORROW , NO_BORROW , RIGHT_BORROW }
 
enum  DualWarmUpMode {
  IPOPT = 0 , IPOPTQP = 1 , OSQP = 2 , DEBUG = 3 ,
  SLACKQP = 4
}
 
enum  DistanceApproachMode {
  DISTANCE_APPROACH_IPOPT = 0 , DISTANCE_APPROACH_IPOPT_CUDA = 1 , DISTANCE_APPROACH_IPOPT_FIXED_TS = 2 , DISTANCE_APPROACH_IPOPT_FIXED_DUAL = 3 ,
  DISTANCE_APPROACH_IPOPT_RELAX_END = 4 , DISTANCE_APPROACH_IPOPT_RELAX_END_SLACK = 5
}
 
enum  DualVariableMode { DUAL_VARIABLE_IPOPT = 0 , DUAL_VARIABLE_IPOPTQP = 1 , DUAL_VARIABLE_OSQP = 2 , DUAL_VARIABLE_DEBUG = 3 }
 
enum class  PullOverState {
  UNKNOWN = 0 , APPROACHING = 1 , PARK_COMPLETE = 2 , PARK_FAIL = 3 ,
  PASS_DESTINATION = 4
}
 
enum  SidePassDirection { LEFT_BORROW = 1 , RIGHT_BORROW = 2 , LEFT_BORROW = 1 , RIGHT_BORROW = 2 }
 
enum  SidePassDirection { LEFT_BORROW = 1 , RIGHT_BORROW = 2 , LEFT_BORROW = 1 , RIGHT_BORROW = 2 }
 

函数

 DEFINE_double (look_backward_distance, 50, "look backward this distance when creating reference line from routing")
 
 DEFINE_double (look_forward_short_distance, 180, "short look forward this distance when creating reference line " "from routing when ADC is slow")
 
 DEFINE_double (look_forward_long_distance, 250, "look forward this distance when creating reference line from routing")
 
 DECLARE_double (look_backward_distance)
 
 DECLARE_double (look_forward_short_distance)
 
 DECLARE_double (look_forward_long_distance)
 
Status AddTrafficDecisionConstraints ()
 
bool IsNonmovableObstacle (const ReferenceLineInfo &reference_line_info, const Obstacle &obstacle)
 
bool IsBlockingObstacleToSidePass (const Frame &frame, const Obstacle *obstacle, double block_obstacle_min_speed, double min_front_sidepass_distance, bool enable_obstacle_blocked_check)
 Decide whether an obstacle is a blocking one that needs to be side-passed.
 
double GetDistanceBetweenADCAndObstacle (const Frame &frame, const Obstacle *obstacle)
 
bool IsBlockingDrivingPathObstacle (const ReferenceLine &reference_line, const Obstacle *obstacle)
 
bool IsParkedVehicle (const ReferenceLine &reference_line, const Obstacle *obstacle)
 
bool IsBlockingObstacleFarFromIntersection (const ReferenceLineInfo &reference_line_info, const std::string &blocking_obstacle_id)
 
double DistanceBlockingObstacleToIntersection (const ReferenceLineInfo &reference_line_info, const std::string &blocking_obstacle_id)
 
double DistanceBlockingObstacleToJunction (const ReferenceLineInfo &reference_line_info, const std::string &blocking_obstacle_id)
 
bool IsBlockingObstacleWithinDestination (const ReferenceLineInfo &reference_line_info, const std::string &blocking_obstacle_id, const double threshold)
 
void CopyTrajectory (const DiscretizedTrajectory trajectory_src, apollo::common::Trajectory *trajectory_tgt_ptr)
 
bool WithinOverlap (const hdmap::PathOverlap &overlap, double s)
 
void EvaluateTrajectory ()
 
void GenerateLearningData ()
 
 DEFINE_string (test_data_dir, "", "the test data folder")
 
 DEFINE_bool (test_update_golden_log, false, "true to update decision golden log file.")
 
 DEFINE_string (test_routing_response_file, "", "The routing file used in test")
 
 DEFINE_string (test_localization_file, "", "The localization test file")
 
 DEFINE_string (test_chassis_file, "", "The chassis test file")
 
 DEFINE_string (test_planning_config_file, "", "planning config file for test")
 
 DEFINE_string (test_prediction_file, "", "The prediction module test file")
 
 DEFINE_string (test_traffic_light_file, "", "The traffic light test file")
 
 DEFINE_string (test_relative_map_file, "", "The relative map test file")
 
 DEFINE_string (test_previous_planning_file, "", "The previous planning test file")
 
 DECLARE_string (test_routing_response_file)
 
 DECLARE_string (test_localization_file)
 
 DECLARE_string (test_chassis_file)
 
 DECLARE_string (test_data_dir)
 
 DECLARE_string (test_prediction_file)
 
 DECLARE_string (test_traffic_light_file)
 
 DECLARE_string (test_relative_map_file)
 
 DECLARE_string (test_previous_planning_file)
 
void SetChartminmax (apollo::dreamview::Chart *chart, std::string label_name_x, std::string label_name_y)
 
void PopulateChartOptions (double x_min, double x_max, std::string x_label, double y_min, double y_max, std::string y_label, bool display, Chart *chart)
 
void AddSTGraph (const STGraphDebug &st_graph, Chart *chart)
 
void AddSLFrame (const SLFrameDebug &sl_frame, Chart *chart)
 
void AddSpeedPlan (const ::google::protobuf::RepeatedPtrField< SpeedPlan > &speed_plans, Chart *chart)
 
bool IsClearToChangeLane (ReferenceLineInfo *reference_line_info)
 A static function to check if the ChangeLanePath type of reference line is safe or if current reference line is safe to deviate away and come back
 
bool IsPerceptionBlocked (const ReferenceLineInfo &reference_line_info, const double search_beam_length, const double search_beam_radius_intensity, const double search_range, const double is_block_angle_threshold)
 A static function to estimate if an obstacle in certain range in front of ADV blocks too much space perception behind itself by beam scanning
 
bool HysteresisFilter (const double obstacle_distance, const double safe_distance, const double distance_buffer, const bool is_obstacle_blocking)
 
void UpdatePreparationDistance (const bool is_opt_succeed, const Frame *frame, const ReferenceLineInfo *const reference_line_info, PlanningContext *planning_context)
 A static function to update the prepararion distance for lane change
 
HybridAStarCreateHybridAPtr ()
 
ObstacleContainerDistanceCreateObstaclesPtr ()
 
ResultContainerDistanceCreateResultPtr ()
 
void AddObstacle (ObstacleContainer *obstacles_ptr, const double *ROI_distance_approach_parking_boundary)
 
double InterpolateUsingLinearApproximation (const double p0, const double p1, const double w)
 
std::vector< double > VectorLinearInterpolation (const std::vector< double > &x, int extend_size)
 
bool DistanceSmoothing (const apollo::planning::PlannerOpenSpaceConfig &planner_open_space_config, const ObstacleContainer &obstacles, double sx, double sy, double sphi, double ex, double ey, double ephi, const std::vector< double > &XYbounds, HybridAStartResult *hybrid_a_star_result, Eigen::MatrixXd *state_result_ds_, Eigen::MatrixXd *control_result_ds_, Eigen::MatrixXd *time_result_ds_, Eigen::MatrixXd *dual_l_result_ds_, Eigen::MatrixXd *dual_n_result_ds_, double &dual_time, double &ipopt_time)
 
bool DistancePlan (HybridAStar *hybridA_ptr, ObstacleContainer *obstacles_ptr, ResultContainer *result_ptr, double sx, double sy, double sphi, double ex, double ey, double ephi, double *XYbounds)
 
void DistanceGetResult (ResultContainer *result_ptr, ObstacleContainer *obstacles_ptr, double *x, double *y, double *phi, double *v, double *a, double *steer, double *opt_x, double *opt_y, double *opt_phi, double *opt_v, double *opt_a, double *opt_steer, double *opt_time, double *opt_dual_l, double *opt_dual_n, size_t *output_size, double *hybrid_time, double *dual_time, double *ipopt_time)
 
HybridAStarCreatePlannerPtr ()
 
HybridAObstacleContainerCreateObstaclesPtr ()
 
HybridASoftBoundaryContainerCreateSoftBoundaryPtr ()
 
HybridAResultContainerCreateResultPtr ()
 
void AddVirtualObstacle (HybridAObstacleContainer *obstacles_ptr, double *obstacle_x, double *obstacle_y, int vertice_num)
 
void AddVirtualSoftBoundary (HybridASoftBoundaryContainer *soft_boundary_ptr, double *soft_boundary_x, double *soft_boundary_y, int vertice_num)
 
bool Plan (HybridAStar *planner_ptr, HybridAObstacleContainer *obstacles_ptr, HybridASoftBoundaryContainer *soft_boundary_ptr, HybridAResultContainer *result_ptr, double sx, double sy, double sphi, double ex, double ey, double ephi, double *XYbounds)
 
void GetResult (HybridAResultContainer *result_ptr, double *x, double *y, double *phi, double *v, double *a, double *steer, size_t *output_size)
 
OpenSpaceROITestCreateROITestPtr ()
 
bool ROITest (OpenSpaceROITest *test_ptr, char *lane_id, char *parking_id, double *unrotated_roi_boundary_x, double *unrotated_roi_boundary_y, double *roi_boundary_x, double *roi_boundary_y, double *parking_spot_x, double *parking_spot_y, double *end_pose, double *xy_boundary, double *origin_pose)
 
void printMatrix (const int r, const int c, const std::vector< c_float > &P_data, const std::vector< c_int > &P_indices, const std::vector< c_int > &P_indptr)
 
bool InitialCuda ()
 
__global__ void fill_lower_left_gpu (int *iRow, int *jCol, unsigned int *rind_L, unsigned int *cind_L, const int nnz_L)
 
template<typename T >
__global__ void data_transfer_gpu (T *dst, const T *src, const int size)
 
bool fill_lower_left (int *iRow, int *jCol, unsigned int *rind_L, unsigned int *cind_L, const int nnz_L)
 
template<typename T >
bool data_transfer (T *dst, const T *src, const int size)
 
bool CheckADCReadyToCruise (const common::VehicleStateProvider *vehicle_state_provider, Frame *frame, const apollo::planning::ScenarioParkAndGoConfig &scenario_config)
 
bool CheckADCSurroundObstacles (const common::math::Vec2d adc_position, const double adc_heading, Frame *frame, const double front_obstacle_buffer)
 : front obstacle is far enough before PardAndGo cruise stage (adc_position: center of rear wheel)
 
bool CheckADCHeading (const common::math::Vec2d adc_position, const double adc_heading, const ReferenceLineInfo &reference_line_info, const double heading_diff_to_reference_line)
 Park_and_go: heading angle should be close to reference line before PardAndGo cruise stage
 
PullOverState CheckADCPullOver (const common::VehicleStateProvider *vehicle_state_provider, const ReferenceLineInfo &reference_line_info, const ScenarioPullOverConfig &scenario_config, const PlanningContext *planning_context)
 : check adc parked properly
 
PullOverState CheckADCPullOverPathPoint (const ReferenceLineInfo &reference_line_info, const ScenarioPullOverConfig &scenario_config, const common::PathPoint &path_point, const PlanningContext *planning_context)
 : check path data to see properly
 
bool CheckPullOverPositionBySL (const ReferenceLineInfo &reference_line_info, const ScenarioPullOverConfig &scenario_config, const common::math::Vec2d &adc_position, const double adc_theta, const common::math::Vec2d &target_position, const double target_theta, const bool check_s)
 
bool ComparePathData (const PathData &lhs, const PathData &rhs, const Obstacle *blocking_obstacle)
 
int ContainsOutOnReverseLane (const std::vector< PathPointDecision > &path_point_decision)
 
int GetBackToInLaneIndex (const std::vector< PathPointDecision > &path_point_decision)
 
int ContainsOutOnReverseLane (const std::vector< std::tuple< double, PathData::PathPointType, double > > &path_point_decision)
 
int GetBackToInLaneIndex (const std::vector< std::tuple< double, PathData::PathPointType, double > > &path_point_decision)
 

变量

constexpr double kAdcDistanceThreshold = 35.0
 
constexpr double kObstaclesDistanceThreshold = 15.0
 
constexpr double kIntersectionClearanceDist = 20.0
 
constexpr double kJunctionClearanceDist = 15.0
 
constexpr double kObsSpeedIgnoreThreshold = 100.0
 
constexpr double kMinObstacleArea = 1e-4
 
constexpr double kPathOptimizationFallbackCost = 2e4
 
constexpr double kSpeedOptimizationFallbackCost = 2e4
 
constexpr double kMathEpsilon = 1e-10
 
constexpr double kSTBoundsDeciderResolution = 0.1
 
constexpr double kSTPassableThreshold = 3.0
 
constexpr double kSpeedGuideLineResolution = 0.1
 
constexpr double kADCSafetyLBuffer = 0.1
 
constexpr double kSIgnoreThreshold = 0.01
 
constexpr double kTIgnoreThreshold = 0.1
 
constexpr double kOvertakenObsCautionTime = 0.5
 

详细描述

类型定义说明

◆ Condition

typedef std::pair< State, double > apollo::planning::Condition

在文件 end_condition_sampler.cc32 行定义.

◆ CrosswalkStopTimer

using apollo::planning::CrosswalkStopTimer = typedef std::unordered_map<std::string, std::unordered_map<std::string, double> >

在文件 crosswalk.cc55 行定义.

◆ CrosswalkToStop

using apollo::planning::CrosswalkToStop = typedef std::vector<std::pair<const hdmap::PathOverlap*, std::vector<std::string> >>

在文件 crosswalk.cc53 行定义.

◆ IndexedObstacles

在文件 obstacle.h293 行定义.

◆ MapPath

在文件 reference_line.cc44 行定义.

◆ ObstacleEdge

using apollo::planning::ObstacleEdge = typedef std::tuple<int, double, double, double, std::string>

在文件 path_bounds_decider_util.h32 行定义.

◆ PathBound

using apollo::planning::PathBound = typedef std::vector<PathBoundPoint>

在文件 path_boundary.h92 行定义.

◆ PathPointDecision

using apollo::planning::PathPointDecision = typedef std::tuple<double, PathData::PathPointType, double>

在文件 path_assessment_decider_util.h30 行定义.

◆ PtrTrajectory1d

using apollo::planning::PtrTrajectory1d = typedef std::shared_ptr<Trajectory1d>

在文件 trajectory_evaluator.cc41 行定义.

◆ SLState

typedef std::pair< std::array< double, 3 >, std::array< double, 3 > > apollo::planning::SLState

在文件 path_bounds_decider_util.h34 行定义.

◆ State

typedef std::array< double, 3 > apollo::planning::State

在文件 backup_trajectory_generator.cc25 行定义.

◆ StopSignLaneVehicles

typedef std::unordered_map< std::string, std::vector< std::string > > apollo::planning::StopSignLaneVehicles

在文件 stage_pre_stop.cc50 行定义.

◆ ThreadSafeIndexedObstacles

◆ Trajectory1d

在文件 trajectory_evaluator.cc40 行定义.

◆ Trajectory1DBundle

typedef std::vector<std::shared_ptr<Curve1d> > apollo::planning::Trajectory1DBundle

在文件 trajectory1d_generator.cc38 行定义.

◆ Trajectory1dPair

using apollo::planning::Trajectory1dPair = typedef std::pair<std::shared_ptr<Curve1d>, std::shared_ptr<Curve1d> >

在文件 trajectory_evaluator.cc42 行定义.

◆ TrajGearPair

枚举类型说明

◆ BoundType

枚举值
ROAD 
LANE 
OBSTACLE 
ADC 

在文件 path_boundary.h33 行定义.

◆ DistanceApproachMode

枚举值
DISTANCE_APPROACH_IPOPT 
DISTANCE_APPROACH_IPOPT_CUDA 
DISTANCE_APPROACH_IPOPT_FIXED_TS 
DISTANCE_APPROACH_IPOPT_FIXED_DUAL 
DISTANCE_APPROACH_IPOPT_RELAX_END 
DISTANCE_APPROACH_IPOPT_RELAX_END_SLACK 

在文件 planner_open_space_config.proto15 行定义.

◆ DualVariableMode

枚举值
DUAL_VARIABLE_IPOPT 
DUAL_VARIABLE_IPOPTQP 
DUAL_VARIABLE_OSQP 
DUAL_VARIABLE_DEBUG 

在文件 planner_open_space_config.proto292 行定义.

◆ DualWarmUpMode

◆ JucType

枚举值
UNKNOWN 
IN_ROAD 
CROSS_ROAD 
FORK_ROAD 
MAIN_SIDE 
DEAD_END 

在文件 planning.proto47 行定义.

47 {
48 UNKNOWN = 0;
49 IN_ROAD = 1;
50 CROSS_ROAD = 2;
51 FORK_ROAD = 3;
52 MAIN_SIDE = 4;
53 DEAD_END = 5;
54}

◆ LaneBorrowInfo

枚举值
LEFT_BORROW 
NO_BORROW 
RIGHT_BORROW 

在文件 path_bounds_decider_util.h36 行定义.

◆ NaviTaskType

枚举值
NAVI_OBSTACLE_DECIDER 
NAVI_PATH_DECIDER 
NAVI_SPEED_DECIDER 

在文件 navi_task_config.proto5 行定义.

◆ ParkingType

enum class apollo::planning::ParkingType
strong
枚举值
NO_PARKING 
PARALLEL_PARKING 
VERTICAL_PARKING 

在文件 open_space_info.h48 行定义.

◆ PullOverState

枚举值
UNKNOWN 
APPROACHING 
PARK_COMPLETE 
PARK_FAIL 
PASS_DESTINATION 

在文件 util.h41 行定义.

41 {
42 UNKNOWN = 0,
43 APPROACHING = 1,
44 PARK_COMPLETE = 2,
45 PARK_FAIL = 3,
47};

◆ ScenarioStatusType

枚举值
STATUS_UNKNOWN 
STATUS_PROCESSING 
STATUS_DONE 

在文件 process_result.h41 行定义.

◆ SidePassDirection [1/2]

枚举值
LEFT_BORROW 
RIGHT_BORROW 
LEFT_BORROW 
RIGHT_BORROW 

在文件 lane_borrow_path.h34 行定义.

34{ LEFT_BORROW = 1, RIGHT_BORROW = 2 };

◆ SidePassDirection [2/2]

枚举值
LEFT_BORROW 
RIGHT_BORROW 
LEFT_BORROW 
RIGHT_BORROW 

在文件 lane_change_path.h35 行定义.

35{ LEFT_BORROW = 1, RIGHT_BORROW = 2 };

◆ StageStatusType

枚举值
ERROR 
READY 
RUNNING 
FINISHED 

在文件 process_result.h34 行定义.

34 {
35 ERROR = 1,
36 READY = 2,
37 RUNNING = 3,
38 FINISHED = 4,
39};

◆ VirtualObjectType

枚举值
DESTINATION 
CROSSWALK 
TRAFFIC_LIGHT 
CLEAR_ZONE 
REROUTE 
DECISION_JUMP 
PRIORITY 

在文件 decision_data.h27 行定义.

函数说明

◆ AddObstacle()

void apollo::planning::AddObstacle ( ObstacleContainer obstacles_ptr,
const double *  ROI_distance_approach_parking_boundary 
)

在文件 distance_approach_problem_wrapper.cc228 行定义.

229 {
230 obstacles_ptr->AddObstacle(ROI_distance_approach_parking_boundary);
231}
void AddObstacle(const double *ROI_distance_approach_parking_boundary)

◆ AddSLFrame()

void apollo::planning::AddSLFrame ( const SLFrameDebug sl_frame,
Chart chart 
)

在文件 on_lane_planning.cc765 行定义.

765 {
766 chart->set_title(sl_frame.name());
767 PopulateChartOptions(0.0, 80.0, "s (meter)", -8.0, 8.0, "l (meter)", false,
768 chart);
769 auto* sl_line = chart->add_line();
770 sl_line->set_label("SL Path");
771 for (const auto& sl_point : sl_frame.sl_path()) {
772 auto* point_debug = sl_line->add_point();
773 point_debug->set_x(sl_point.s());
774 point_debug->set_x(sl_point.l());
775 }
776}
void PopulateChartOptions(double x_min, double x_max, std::string x_label, double y_min, double y_max, std::string y_label, bool display, Chart *chart)

◆ AddSpeedPlan()

void apollo::planning::AddSpeedPlan ( const ::google::protobuf::RepeatedPtrField< SpeedPlan > &  speed_plans,
Chart chart 
)

在文件 on_lane_planning.cc778 行定义.

780 {
781 chart->set_title("Speed Plan");
782 PopulateChartOptions(0.0, 80.0, "s (meter)", 0.0, 50.0, "v (m/s)", false,
783 chart);
784
785 for (const auto& speed_plan : speed_plans) {
786 auto* line = chart->add_line();
787 line->set_label(speed_plan.name());
788 for (const auto& point : speed_plan.speed_point()) {
789 auto* point_debug = line->add_point();
790 point_debug->set_x(point.s());
791 point_debug->set_y(point.v());
792 }
793
794 // Set chartJS's dataset properties
795 auto* properties = line->mutable_properties();
796 (*properties)["borderWidth"] = "2";
797 (*properties)["pointRadius"] = "0";
798 (*properties)["fill"] = "false";
799 (*properties)["showLine"] = "true";
800 if (speed_plan.name() == "DpStSpeedOptimizer") {
801 (*properties)["color"] = "\"rgba(27, 249, 105, 0.5)\"";
802 } else if (speed_plan.name() == "QpSplineStSpeedOptimizer") {
803 (*properties)["color"] = "\"rgba(54, 162, 235, 1)\"";
804 }
805 }
806}

◆ AddSTGraph()

void apollo::planning::AddSTGraph ( const STGraphDebug st_graph,
Chart chart 
)

在文件 on_lane_planning.cc717 行定义.

717 {
718 if (st_graph.name() == "DP_ST_SPEED_OPTIMIZER") {
719 chart->set_title("Speed Heuristic");
720 } else {
721 chart->set_title("Planning S-T Graph");
722 }
723 PopulateChartOptions(-2.0, 10.0, "t (second)", -10.0, 220.0, "s (meter)",
724 false, chart);
725
726 for (const auto& boundary : st_graph.boundary()) {
727 // from 'ST_BOUNDARY_TYPE_' to the end
728 std::string type =
729 StGraphBoundaryDebug_StBoundaryType_Name(boundary.type()).substr(17);
730
731 auto* boundary_chart = chart->add_polygon();
732 auto* properties = boundary_chart->mutable_properties();
733 (*properties)["borderWidth"] = "2";
734 (*properties)["pointRadius"] = "0";
735 (*properties)["lineTension"] = "0";
736 (*properties)["cubicInterpolationMode"] = "monotone";
737 (*properties)["showLine"] = "true";
738 (*properties)["showText"] = "true";
739 (*properties)["fill"] = "false";
740
741 if (type == "DRIVABLE_REGION") {
742 (*properties)["color"] = "\"rgba(0, 255, 0, 0.5)\"";
743 } else {
744 (*properties)["color"] = "\"rgba(255, 0, 0, 0.8)\"";
745 }
746
747 boundary_chart->set_label(boundary.name() + "_" + type);
748 for (const auto& point : boundary.point()) {
749 auto* point_debug = boundary_chart->add_point();
750 point_debug->set_x(point.t());
751 point_debug->set_y(point.s());
752 }
753 }
754
755 auto* speed_profile = chart->add_line();
756 auto* properties = speed_profile->mutable_properties();
757 (*properties)["color"] = "\"rgba(255, 255, 255, 0.5)\"";
758 for (const auto& point : st_graph.speed_profile()) {
759 auto* point_debug = speed_profile->add_point();
760 point_debug->set_x(point.t());
761 point_debug->set_y(point.s());
762 }
763}

◆ AddTrafficDecisionConstraints()

Status apollo::planning::AddTrafficDecisionConstraints ( )

在文件 navi_speed_decider.cc357 行定义.

357 {
358 // TODO(all):
359 return Status::OK();
360}

◆ AddVirtualObstacle()

void apollo::planning::AddVirtualObstacle ( HybridAObstacleContainer obstacles_ptr,
double *  obstacle_x,
double *  obstacle_y,
int  vertice_num 
)

在文件 hybrid_a_star_wrapper.cc141 行定义.

145 {
146 obstacles_ptr->AddVirtualObstacle(obstacle_x, obstacle_y, vertice_num);
147}
void AddVirtualObstacle(double *obstacle_x, double *obstacle_y, int vertice_num)

◆ AddVirtualSoftBoundary()

void apollo::planning::AddVirtualSoftBoundary ( HybridASoftBoundaryContainer soft_boundary_ptr,
double *  soft_boundary_x,
double *  soft_boundary_y,
int  vertice_num 
)

在文件 hybrid_a_star_wrapper.cc148 行定义.

152 {
153 soft_boundary_ptr->AddVirtualSoftBoundary(
154 soft_boundary_x, soft_boundary_y, vertice_num);
155}
void AddVirtualSoftBoundary(double *obstacle_x, double *obstacle_y, int vertice_num)

◆ CheckADCHeading()

bool apollo::planning::CheckADCHeading ( const common::math::Vec2d  adc_position,
const double  adc_heading,
const ReferenceLineInfo reference_line_info,
const double  heading_diff_to_reference_line 
)

Park_and_go: heading angle should be close to reference line before PardAndGo cruise stage

在文件 util.cc109 行定义.

112 {
113 const double kReducedHeadingBuffer = 0.2; // (rad) TODO(Shu) move to config
114 const auto& reference_line = reference_line_info.reference_line();
115 common::SLPoint adc_position_sl;
116 reference_line.XYToSL(adc_position, &adc_position_sl);
117 // reference line heading angle at s
118 const auto reference_point =
119 reference_line.GetReferencePoint(adc_position_sl.s());
120 const auto path_point = reference_point.ToPathPoint(adc_position_sl.s());
121 AINFO << "heading difference: "
122 << common::math::NormalizeAngle(adc_heading - path_point.theta());
123 double angle_difference =
124 common::math::NormalizeAngle(adc_heading - path_point.theta());
125 if (angle_difference >
126 -1.0 * (heading_diff_to_reference_line - kReducedHeadingBuffer) &&
127 angle_difference < heading_diff_to_reference_line) {
128 return true;
129 }
130 return false;
131}
const ReferenceLine & reference_line() const
#define AINFO
Definition log.h:42

◆ CheckADCPullOver()

PullOverState apollo::planning::CheckADCPullOver ( const common::VehicleStateProvider vehicle_state_provider,
const ReferenceLineInfo reference_line_info,
const ScenarioPullOverConfig scenario_config,
const PlanningContext planning_context 
)

: check adc parked properly

在文件 util.cc35 行定义.

39 {
40 const auto& pull_over_status =
41 planning_context->planning_status().pull_over();
42 if (!pull_over_status.has_position() ||
43 !pull_over_status.position().has_x() ||
44 !pull_over_status.position().has_y() || !pull_over_status.has_theta()) {
45 ADEBUG << "pull_over status not set properly: "
46 << pull_over_status.DebugString();
47 return PullOverState::UNKNOWN;
48 }
49
50 const auto& reference_line = reference_line_info.reference_line();
51 common::SLPoint pull_over_sl;
52 reference_line.XYToSL(pull_over_status.position(), &pull_over_sl);
53
54 const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
55 double distance = adc_front_edge_s - pull_over_sl.s();
56 if (distance >= scenario_config.pass_destination_threshold()) {
57 ADEBUG << "ADC passed pull-over spot: distance[" << distance << "]";
58 return PullOverState::PASS_DESTINATION;
59 }
60
61 const double adc_speed = vehicle_state_provider->linear_velocity();
62 const double max_adc_stop_speed = common::VehicleConfigHelper::Instance()
63 ->GetConfig()
64 .vehicle_param()
65 .max_abs_speed_when_stopped();
66 if (adc_speed > max_adc_stop_speed) {
67 ADEBUG << "ADC not stopped: speed[" << adc_speed << "]";
68 return PullOverState::APPROACHING;
69 }
70
71 static constexpr double kStartParkCheckRange = 3.0; // meter
72 if (distance <= -kStartParkCheckRange) {
73 ADEBUG << "ADC still far: distance[" << distance << "]";
74 return PullOverState::APPROACHING;
75 }
76
77 const common::math::Vec2d adc_position = {vehicle_state_provider->x(),
78 vehicle_state_provider->y()};
79 const common::math::Vec2d target_position = {pull_over_status.position().x(),
80 pull_over_status.position().y()};
81
82 const bool position_check = CheckPullOverPositionBySL(
83 reference_line_info, scenario_config, adc_position,
84 vehicle_state_provider->heading(), target_position,
85 pull_over_status.theta(), true);
86
87 return position_check ? PullOverState::PARK_COMPLETE
88 : PullOverState::PARK_FAIL;
89}
double linear_velocity() const
Get the vehicle's linear velocity.
double x() const
Get the x-coordinate of vehicle position.
double y() const
Get the y-coordinate of vehicle position.
double heading() const
Get the heading of vehicle position, which is the angle between the vehicle's heading direction and t...
const PlanningStatus & planning_status() const
const SLBoundary & AdcSlBoundary() const
#define ADEBUG
Definition log.h:41
bool CheckPullOverPositionBySL(const ReferenceLineInfo &reference_line_info, const ScenarioPullOverConfig &scenario_config, const common::math::Vec2d &adc_position, const double adc_theta, const common::math::Vec2d &target_position, const double target_theta, const bool check_s)
Definition util.cc:120

◆ CheckADCPullOverPathPoint()

PullOverState apollo::planning::CheckADCPullOverPathPoint ( const ReferenceLineInfo reference_line_info,
const ScenarioPullOverConfig scenario_config,
const common::PathPoint path_point,
const PlanningContext planning_context 
)

: check path data to see properly

在文件 util.cc94 行定义.

98 {
99 const auto& pull_over_status =
100 planning_context->planning_status().pull_over();
101 if (!pull_over_status.has_position() ||
102 !pull_over_status.position().has_x() ||
103 !pull_over_status.position().has_y() || !pull_over_status.has_theta()) {
104 ADEBUG << "pull_over status not set properly: "
105 << pull_over_status.DebugString();
106 return PullOverState::UNKNOWN;
107 }
108
109 const common::math::Vec2d target_position = {pull_over_status.position().x(),
110 pull_over_status.position().y()};
111 const bool position_check = CheckPullOverPositionBySL(
112 reference_line_info, scenario_config, {path_point.x(), path_point.y()},
113 path_point.theta(), target_position, pull_over_status.theta(),
114 false); // check l + theta only
115
116 return position_check ? PullOverState::PARK_COMPLETE
117 : PullOverState::PARK_FAIL;
118}
Implements a class of 2-dimensional vectors.
Definition vec2d.h:42
double x() const
Getter for x component
Definition vec2d.h:54

◆ CheckADCReadyToCruise()

bool apollo::planning::CheckADCReadyToCruise ( const common::VehicleStateProvider vehicle_state_provider,
Frame frame,
const apollo::planning::ScenarioParkAndGoConfig scenario_config 
)

在文件 util.cc28 行定义.

30 {
31 auto vehicle_status = vehicle_state_provider;
32 common::math::Vec2d adc_position = {vehicle_status->x(), vehicle_status->y()};
33 const double adc_heading = vehicle_status->heading();
34
35 common::SLPoint adc_position_sl;
36 // get nearest reference line
37 const auto& reference_line_list = frame->reference_line_info();
38 const auto reference_line_info = std::min_element(
39 reference_line_list.begin(), reference_line_list.end(),
40 [&](const ReferenceLineInfo& ref_a, const ReferenceLineInfo& ref_b) {
41 common::SLPoint adc_position_sl_a;
42 common::SLPoint adc_position_sl_b;
43 ref_a.reference_line().XYToSL(adc_position, &adc_position_sl_a);
44 ref_b.reference_line().XYToSL(adc_position, &adc_position_sl_b);
45 return std::fabs(adc_position_sl_a.l()) <
46 std::fabs(adc_position_sl_b.l());
47 });
48 reference_line_info->reference_line().XYToSL(adc_position, &adc_position_sl);
49 bool is_near_front_obstacle =
50 CheckADCSurroundObstacles(adc_position, adc_heading, frame,
51 scenario_config.front_obstacle_buffer());
52 bool heading_align_w_reference_line =
53 CheckADCHeading(adc_position, adc_heading, *reference_line_info,
54 scenario_config.heading_buffer());
55 ADEBUG << "is_near_front_obstacle: " << is_near_front_obstacle;
56 ADEBUG << "heading_align_w_reference_line: "
57 << heading_align_w_reference_line;
58 // check gear status
59 // TODO(SHU): align with vehicle parameters
60 static constexpr double kMinSpeed = 0.1; // m/s
61 return ((vehicle_status->gear() == canbus::Chassis::GEAR_DRIVE ||
62 std::fabs(vehicle_status->vehicle_state().linear_velocity()) <
63 kMinSpeed) &&
64 !is_near_front_obstacle && heading_align_w_reference_line &&
65 std::fabs(adc_position_sl.l()) < 0.5);
66}
const std::list< ReferenceLineInfo > & reference_line_info() const
Definition frame.cc:123

◆ CheckADCSurroundObstacles()

bool apollo::planning::CheckADCSurroundObstacles ( const common::math::Vec2d  adc_position,
const double  adc_heading,
Frame frame,
const double  front_obstacle_buffer 
)

: front obstacle is far enough before PardAndGo cruise stage (adc_position: center of rear wheel)

在文件 util.cc72 行定义.

74 {
75 const auto& vehicle_config =
76 common::VehicleConfigHelper::Instance()->GetConfig();
77 const double adc_length = vehicle_config.vehicle_param().length();
78 const double adc_width = vehicle_config.vehicle_param().width();
79 // ADC box
80 Box2d adc_box(adc_position, adc_heading, adc_length + front_obstacle_buffer,
81 adc_width);
82 double shift_distance = front_obstacle_buffer / 2 +
83 vehicle_config.vehicle_param().back_edge_to_center();
84 Vec2d shift_vec{shift_distance * std::cos(adc_heading),
85 shift_distance * std::sin(adc_heading)};
86 adc_box.Shift(shift_vec);
87 const auto& adc_polygon = Polygon2d(adc_box);
88 // obstacle boxes
89 auto obstacles = frame->obstacles();
90 for (const auto& obstacle : obstacles) {
91 if (obstacle->IsVirtual()) {
92 continue;
93 }
94 const auto& obstacle_polygon = obstacle->PerceptionPolygon();
95 const Polygon2d& nudge_polygon = obstacle_polygon.ExpandByDistance(
96 std::fabs(FLAGS_static_obstacle_nudge_l_buffer));
97 if (adc_polygon.HasOverlap(nudge_polygon)) {
98 ADEBUG << "blocked obstacle: " << obstacle->Id();
99 return true;
100 }
101 }
102 return false;
103}
Rectangular (undirected) bounding box in 2-D.
Definition box2d.h:52
const std::vector< const Obstacle * > obstacles() const
Definition frame.cc:577

◆ CheckPullOverPositionBySL()

bool apollo::planning::CheckPullOverPositionBySL ( const ReferenceLineInfo reference_line_info,
const ScenarioPullOverConfig scenario_config,
const common::math::Vec2d adc_position,
const double  adc_theta,
const common::math::Vec2d target_position,
const double  target_theta,
const bool  check_s 
)

在文件 util.cc120 行定义.

125 {
126 const auto& reference_line = reference_line_info.reference_line();
127 common::SLPoint target_sl;
128 reference_line.XYToSL(target_position, &target_sl);
129 common::SLPoint adc_position_sl;
130 reference_line.XYToSL(adc_position, &adc_position_sl);
131
132 const double s_diff = target_sl.s() - adc_position_sl.s();
133 const double l_diff = std::fabs(target_sl.l() - adc_position_sl.l());
134 const double theta_diff =
135 std::fabs(common::math::NormalizeAngle(target_theta - adc_theta));
136
137 ADEBUG << "adc_position_s[" << adc_position_sl.s() << "] adc_position_l["
138 << adc_position_sl.l() << "] target_s[" << target_sl.s()
139 << "] target_l[" << target_sl.l() << "] s_diff[" << s_diff
140 << "] l_diff[" << l_diff << "] theta_diff[" << theta_diff << "]";
141
142 // check s/l/theta diff
143 bool ret = (l_diff <= scenario_config.max_l_error_to_end_point() &&
144 theta_diff <= scenario_config.max_theta_error_to_end_point());
145 if (check_s) {
146 ret = (ret && s_diff >= 0 &&
147 s_diff <= scenario_config.max_s_error_to_end_point());
148 }
149
150 return ret;
151}

◆ ComparePathData()

bool apollo::planning::ComparePathData ( const PathData lhs,
const PathData rhs,
const Obstacle blocking_obstacle 
)

在文件 lane_borrow_path.cc700 行定义.

701 {
702 ADEBUG << "Comparing " << lhs.path_label() << " and " << rhs.path_label();
703 static constexpr double kNeighborPathLengthComparisonTolerance = 25.0;
704 double lhs_path_length = lhs.frenet_frame_path().back().s();
705 double rhs_path_length = rhs.frenet_frame_path().back().s();
706 // Select longer path.
707 // If roughly same length, then select self-lane path.
708 if (std::fabs(lhs_path_length - rhs_path_length) >
709 kNeighborPathLengthComparisonTolerance) {
710 return lhs_path_length > rhs_path_length;
711 }
712 // If roughly same length, and must borrow neighbor lane,
713 // then prefer to borrow forward lane rather than reverse lane.
714 int lhs_on_reverse =
715 ContainsOutOnReverseLane(lhs.path_point_decision_guide());
716 int rhs_on_reverse =
717 ContainsOutOnReverseLane(rhs.path_point_decision_guide());
718 // TODO(jiacheng): make this a flag.
719 if (std::abs(lhs_on_reverse - rhs_on_reverse) > 6) {
720 return lhs_on_reverse < rhs_on_reverse;
721 }
722 // For two lane-borrow directions, based on ADC's position,
723 // select the more convenient one.
724 if (blocking_obstacle) {
725 // select left/right path based on blocking_obstacle's position
726 const double obstacle_l =
727 (blocking_obstacle->PerceptionSLBoundary().start_l() +
728 blocking_obstacle->PerceptionSLBoundary().end_l()) /
729 2;
730 ADEBUG << "obstacle[" << blocking_obstacle->Id() << "] l[" << obstacle_l
731 << "]";
732 return (obstacle_l > 0.0
733 ? (lhs.path_label().find("right") != std::string::npos)
734 : (lhs.path_label().find("left") != std::string::npos));
735 } else {
736 // select left/right path based on ADC's position
737 double adc_l = lhs.frenet_frame_path().front().l();
738 if (adc_l < -1.0) {
739 return lhs.path_label().find("right") != std::string::npos;
740 } else if (adc_l > 1.0) {
741 return lhs.path_label().find("left") != std::string::npos;
742 }
743 }
744 // If same length, both neighbor lane are forward,
745 // then select the one that returns to in-lane earlier.
746 static constexpr double kBackToSelfLaneComparisonTolerance = 20.0;
747 int lhs_back_idx = GetBackToInLaneIndex(lhs.path_point_decision_guide());
748 int rhs_back_idx = GetBackToInLaneIndex(rhs.path_point_decision_guide());
749 double lhs_back_s = lhs.frenet_frame_path()[lhs_back_idx].s();
750 double rhs_back_s = rhs.frenet_frame_path()[rhs_back_idx].s();
751 if (std::fabs(lhs_back_s - rhs_back_s) > kBackToSelfLaneComparisonTolerance) {
752 return lhs_back_idx < rhs_back_idx;
753 }
754 // If same length, both forward, back to inlane at same time,
755 // select the left one to side-pass.
756 bool lhs_on_leftlane = lhs.path_label().find("left") != std::string::npos;
757 return lhs_on_leftlane;
758}
const std::string & Id() const
Definition obstacle.h:75
const SLBoundary & PerceptionSLBoundary() const
Definition obstacle.cc:694
const std::string & path_label() const
Definition path_data.cc:264
const FrenetFramePath & frenet_frame_path() const
Definition path_data.cc:94
const std::vector< std::tuple< double, PathPointType, double > > & path_point_decision_guide() const
Definition path_data.cc:99
int GetBackToInLaneIndex(const std::vector< PathPointDecision > &path_point_decision)

◆ ContainsOutOnReverseLane() [1/2]

int apollo::planning::ContainsOutOnReverseLane ( const std::vector< PathPointDecision > &  path_point_decision)

在文件 lane_borrow_path.cc760 行定义.

761 {
762 int ret = 0;
763 for (const auto& curr_decision : path_point_decision) {
764 if (std::get<1>(curr_decision) ==
765 PathData::PathPointType::OUT_ON_REVERSE_LANE) {
766 ++ret;
767 }
768 }
769 return ret;
770}

◆ ContainsOutOnReverseLane() [2/2]

int apollo::planning::ContainsOutOnReverseLane ( const std::vector< std::tuple< double, PathData::PathPointType, double > > &  path_point_decision)

◆ CopyTrajectory()

void apollo::planning::CopyTrajectory ( const DiscretizedTrajectory  trajectory_src,
apollo::common::Trajectory trajectory_tgt_ptr 
)

在文件 open_space_info.cc26 行定义.

27 {
28 const size_t horizon = trajectory_src.NumOfPoints();
29 for (size_t i = 0; i < horizon; ++i) {
30 *trajectory_tgt_ptr->add_trajectory_point() =
31 trajectory_src.TrajectoryPointAt(i);
32 }
33}
const common::TrajectoryPoint & TrajectoryPointAt(const size_t index) const

◆ CreateHybridAPtr()

HybridAStar * apollo::planning::CreateHybridAPtr ( )

在文件 distance_approach_problem_wrapper.cc215 行定义.

215 {
216 apollo::planning::PlannerOpenSpaceConfig planner_open_space_config_;
218 FLAGS_planner_open_space_config_filename, &planner_open_space_config_))
219 << "Failed to load open space config file "
220 << FLAGS_planner_open_space_config_filename;
221 return new HybridAStar(planner_open_space_config_);
222}
#define ACHECK(cond)
Definition log.h:80
bool GetProtoFromFile(const std::string &file_name, google::protobuf::Message *message)
Parses the content of the file specified by the file_name as a representation of protobufs,...
Definition file.cc:132

◆ CreateObstaclesPtr()

HybridAObstacleContainer * apollo::planning::CreateObstaclesPtr ( )

◆ CreatePlannerPtr()

HybridAStar * apollo::planning::CreatePlannerPtr ( )

在文件 hybrid_a_star_wrapper.cc120 行定义.

120 {
121 FLAGS_planner_open_space_config_filename
122 = "modules/planning/planning_component/"
123 "conf/planner_hybrid_a_star_config.pb.txt";
124 apollo::planning::WarmStartConfig planner_open_space_config_;
125
127 FLAGS_planner_open_space_config_filename, &planner_open_space_config_))
128 << "Failed to load open space config file "
129 << FLAGS_planner_open_space_config_filename;
130 return new HybridAStar(planner_open_space_config_);
131}

◆ CreateResultPtr()

HybridAResultContainer * apollo::planning::CreateResultPtr ( )

◆ CreateROITestPtr()

OpenSpaceROITest * apollo::planning::CreateROITestPtr ( )

◆ CreateSoftBoundaryPtr()

HybridASoftBoundaryContainer * apollo::planning::CreateSoftBoundaryPtr ( )

◆ data_transfer()

template<typename T >
bool apollo::planning::data_transfer ( T *  dst,
const T *  src,
const int  size 
)

◆ data_transfer_gpu()

template<typename T >
__global__ void apollo::planning::data_transfer_gpu ( T *  dst,
const T *  src,
const int  size 
)

◆ DECLARE_double() [1/3]

apollo::planning::DECLARE_double ( look_backward_distance  )

◆ DECLARE_double() [2/3]

apollo::planning::DECLARE_double ( look_forward_long_distance  )

◆ DECLARE_double() [3/3]

apollo::planning::DECLARE_double ( look_forward_short_distance  )

◆ DECLARE_string() [1/8]

apollo::planning::DECLARE_string ( test_chassis_file  )

◆ DECLARE_string() [2/8]

apollo::planning::DECLARE_string ( test_data_dir  )

◆ DECLARE_string() [3/8]

apollo::planning::DECLARE_string ( test_localization_file  )

◆ DECLARE_string() [4/8]

apollo::planning::DECLARE_string ( test_prediction_file  )

◆ DECLARE_string() [5/8]

apollo::planning::DECLARE_string ( test_previous_planning_file  )

◆ DECLARE_string() [6/8]

apollo::planning::DECLARE_string ( test_relative_map_file  )

◆ DECLARE_string() [7/8]

apollo::planning::DECLARE_string ( test_routing_response_file  )

◆ DECLARE_string() [8/8]

apollo::planning::DECLARE_string ( test_traffic_light_file  )

◆ DEFINE_bool()

apollo::planning::DEFINE_bool ( test_update_golden_log  ,
false  ,
"true to update decision golden log file."   
)

◆ DEFINE_double() [1/3]

apollo::planning::DEFINE_double ( look_backward_distance  ,
50  ,
"look backward this distance when creating reference line from routing"   
)

◆ DEFINE_double() [2/3]

apollo::planning::DEFINE_double ( look_forward_long_distance  ,
250  ,
"look forward this distance when creating reference line from routing"   
)

◆ DEFINE_double() [3/3]

apollo::planning::DEFINE_double ( look_forward_short_distance  ,
180  ,
"short look forward this distance when creating reference line " "from routing when ADC is slow"   
)

◆ DEFINE_string() [1/9]

apollo::planning::DEFINE_string ( test_chassis_file  ,
""  ,
"The chassis test file"   
)

◆ DEFINE_string() [2/9]

apollo::planning::DEFINE_string ( test_data_dir  ,
""  ,
"the test data folder"   
)

◆ DEFINE_string() [3/9]

apollo::planning::DEFINE_string ( test_localization_file  ,
""  ,
"The localization test file"   
)

◆ DEFINE_string() [4/9]

apollo::planning::DEFINE_string ( test_planning_config_file  ,
""  ,
"planning config file for test"   
)

◆ DEFINE_string() [5/9]

apollo::planning::DEFINE_string ( test_prediction_file  ,
""  ,
"The prediction module test file"   
)

◆ DEFINE_string() [6/9]

apollo::planning::DEFINE_string ( test_previous_planning_file  ,
""  ,
"The previous planning test file"   
)

◆ DEFINE_string() [7/9]

apollo::planning::DEFINE_string ( test_relative_map_file  ,
""  ,
"The relative map test file"   
)

◆ DEFINE_string() [8/9]

apollo::planning::DEFINE_string ( test_routing_response_file  ,
""  ,
"The routing file used in test"   
)

◆ DEFINE_string() [9/9]

apollo::planning::DEFINE_string ( test_traffic_light_file  ,
""  ,
"The traffic light test file"   
)

◆ DistanceBlockingObstacleToIntersection()

double apollo::planning::DistanceBlockingObstacleToIntersection ( const ReferenceLineInfo reference_line_info,
const std::string &  blocking_obstacle_id 
)

在文件 obstacle_blocking_analyzer.cc283 行定义.

285 {
286 if (blocking_obstacle_id.empty()) {
287 ADEBUG << "There is no blocking obstacle.";
288 return true;
289 }
290 const Obstacle* blocking_obstacle =
291 reference_line_info.path_decision().obstacles().Find(
292 blocking_obstacle_id);
293 if (blocking_obstacle == nullptr) {
294 ADEBUG << "Blocking obstacle is no longer there.";
295 return true;
296 }
297
298 // Get blocking obstacle's s.
299 double blocking_obstacle_s =
300 blocking_obstacle->PerceptionSLBoundary().end_s();
301 double min_distance = std::numeric_limits<double>::max();
302 ADEBUG << "Blocking obstacle is at s = " << blocking_obstacle_s;
303 // Get intersection's s and compare with threshold.
304 const auto& first_encountered_overlaps =
305 reference_line_info.FirstEncounteredOverlaps();
306 for (const auto& overlap : first_encountered_overlaps) {
307 ADEBUG << overlap.first << ", " << overlap.second.DebugString();
308 if (overlap.first != ReferenceLineInfo::SIGNAL &&
309 overlap.first != ReferenceLineInfo::STOP_SIGN) {
310 continue;
311 }
312 min_distance =
313 std::min(min_distance, overlap.second.start_s - blocking_obstacle_s);
314 }
315
316 return min_distance;
317}
T * Find(const I id)
Find object by id in the container
const IndexedList< std::string, Obstacle > & obstacles() const
const std::vector< std::pair< OverlapType, hdmap::PathOverlap > > & FirstEncounteredOverlaps() const

◆ DistanceBlockingObstacleToJunction()

double apollo::planning::DistanceBlockingObstacleToJunction ( const ReferenceLineInfo reference_line_info,
const std::string &  blocking_obstacle_id 
)

在文件 obstacle_blocking_analyzer.cc319 行定义.

321 {
322 if (blocking_obstacle_id.empty()) {
323 ADEBUG << "There is no blocking obstacle.";
324 return true;
325 }
326 const Obstacle* blocking_obstacle =
327 reference_line_info.path_decision().obstacles().Find(
328 blocking_obstacle_id);
329 if (blocking_obstacle == nullptr) {
330 ADEBUG << "Blocking obstacle is no longer there.";
331 return true;
332 }
333
334 // Get blocking obstacle's s.
335 double blocking_obstacle_start_s =
336 blocking_obstacle->PerceptionSLBoundary().start_s();
337 double blocking_obstacle_end_s =
338 blocking_obstacle->PerceptionSLBoundary().end_s();
339 double min_distance = std::numeric_limits<double>::max();
340 AINFO << "Blocking obstacle start s = " << blocking_obstacle_start_s
341 << ", end_s: " << blocking_obstacle_end_s;
342 // Get intersection's s and compare with threshold.
343 const auto& first_encountered_overlaps =
344 reference_line_info.FirstEncounteredOverlaps();
345 for (const auto& overlap :
346 reference_line_info.reference_line().map_path().junction_overlaps()) {
347 AINFO << overlap.DebugString();
348 double distance = std::numeric_limits<double>::max();
349 if ((blocking_obstacle_start_s >= overlap.start_s &&
350 blocking_obstacle_start_s <= overlap.end_s) ||
351 (blocking_obstacle_end_s >= overlap.start_s &&
352 blocking_obstacle_end_s <= overlap.end_s)) {
353 distance = 0.0;
354 } else if (blocking_obstacle_end_s < overlap.start_s) {
355 distance = overlap.start_s - blocking_obstacle_end_s;
356 } else {
357 distance = blocking_obstacle_start_s - overlap.end_s;
358 }
359 min_distance = std::min(min_distance, distance);
360 }
361
362 return min_distance;
363}

◆ DistanceCreateObstaclesPtr()

ObstacleContainer * apollo::planning::DistanceCreateObstaclesPtr ( )

◆ DistanceCreateResultPtr()

ResultContainer * apollo::planning::DistanceCreateResultPtr ( )

◆ DistanceGetResult()

void apollo::planning::DistanceGetResult ( ResultContainer result_ptr,
ObstacleContainer obstacles_ptr,
double *  x,
double *  y,
double *  phi,
double *  v,
double *  a,
double *  steer,
double *  opt_x,
double *  opt_y,
double *  opt_phi,
double *  opt_v,
double *  opt_a,
double *  opt_steer,
double *  opt_time,
double *  opt_dual_l,
double *  opt_dual_n,
size_t *  output_size,
double *  hybrid_time,
double *  dual_time,
double *  ipopt_time 
)

在文件 distance_approach_problem_wrapper.cc624 行定义.

631 {
632 result_ptr->LoadHybridAResult();
633 size_t size = result_ptr->GetX()->size();
634 size_t size_by_distance = result_ptr->PrepareStateResult()->cols();
635 AERROR_IF(size != size_by_distance)
636 << "sizes by hybrid A and distance approach not consistent";
637 for (size_t i = 0; i < size; ++i) {
638 x[i] = result_ptr->GetX()->at(i);
639 y[i] = result_ptr->GetY()->at(i);
640 phi[i] = result_ptr->GetPhi()->at(i);
641 v[i] = result_ptr->GetV()->at(i);
642 }
643 for (size_t i = 0; i + 1 < size; ++i) {
644 a[i] = result_ptr->GetA()->at(i);
645 steer[i] = result_ptr->GetSteer()->at(i);
646 }
647 output_size[0] = size;
648
649 size_t obstacles_edges_sum = obstacles_ptr->GetObstaclesEdgesNum().sum();
650 size_t obstacles_num_to_car = 4 * obstacles_ptr->GetObstaclesNum();
651 for (size_t i = 0; i < size_by_distance; ++i) {
652 opt_x[i] = (*(result_ptr->PrepareStateResult()))(0, i);
653 opt_y[i] = (*(result_ptr->PrepareStateResult()))(1, i);
654 opt_phi[i] = (*(result_ptr->PrepareStateResult()))(2, i);
655 opt_v[i] = (*(result_ptr->PrepareStateResult()))(3, i);
656 }
657
658 if (result_ptr->PrepareTimeResult() != 0) {
659 for (size_t i = 0; i + 1 < size_by_distance; ++i) {
660 opt_time[i] = (*(result_ptr->PrepareTimeResult()))(0, i);
661 }
662 }
663
664 if (result_ptr->PrepareLResult()->cols() != 0 &&
665 result_ptr->PrepareNResult() != 0) {
666 for (size_t i = 0; i + 1 < size_by_distance; ++i) {
667 for (size_t j = 0; j < obstacles_edges_sum; ++j) {
668 opt_dual_l[i * obstacles_edges_sum + j] =
669 (*(result_ptr->PrepareLResult()))(j, i);
670 }
671 for (size_t k = 0; k < obstacles_num_to_car; ++k) {
672 opt_dual_n[i * obstacles_num_to_car + k] =
673 (*(result_ptr->PrepareNResult()))(k, i);
674 }
675 }
676 }
677 for (size_t i = 0; i + 1 < size_by_distance; ++i) {
678 opt_a[i] = (*(result_ptr->PrepareControlResult()))(1, i);
679 opt_steer[i] = (*(result_ptr->PrepareControlResult()))(0, i);
680 }
681
682 hybrid_time[0] = *(result_ptr->GetHybridTime());
683 dual_time[0] = *(result_ptr->GetDualTime());
684 ipopt_time[0] = *(result_ptr->GetIpoptTime());
685}
#define AERROR_IF(cond)
Definition log.h:74

◆ DistancePlan()

bool apollo::planning::DistancePlan ( HybridAStar hybridA_ptr,
ObstacleContainer obstacles_ptr,
ResultContainer result_ptr,
double  sx,
double  sy,
double  sphi,
double  ex,
double  ey,
double  ephi,
double *  XYbounds 
)

在文件 distance_approach_problem_wrapper.cc419 行定义.

422 {
423 apollo::planning::PlannerOpenSpaceConfig planner_open_space_config_;
425 FLAGS_planner_open_space_config_filename, &planner_open_space_config_))
426 << "Failed to load open space config file "
427 << FLAGS_planner_open_space_config_filename;
428 AINFO << "FLAGS_planner_open_space_config_filename: "
429 << FLAGS_planner_open_space_config_filename;
430
431 double hybrid_total = 0.0;
432 double dual_total = 0.0;
433 double ipopt_total = 0.0;
434
435 std::string flag_file_path =
436 "modules/planning/planning_component/conf/planning.conf";
437 google::SetCommandLineOption("flagfile", flag_file_path.c_str());
438
439 HybridAStartResult hybrid_astar_result;
440 std::vector<double> XYbounds_(XYbounds, XYbounds + 4);
441 std::vector<std::vector<common::math::Vec2d>> soft_boundary_vertices_vec;
442
443 const auto start_timestamp = std::chrono::system_clock::now();
444 if (!hybridA_ptr->Plan(sx, sy, sphi, ex, ey, ephi, XYbounds_,
445 obstacles_ptr->GetObstacleVec(),
446 &hybrid_astar_result,
447 soft_boundary_vertices_vec, false)) {
448 AINFO << "Hybrid A Star fails";
449 return false;
450 }
451 const auto end_timestamp = std::chrono::system_clock::now();
452 std::chrono::duration<double> time_diff = end_timestamp - start_timestamp;
453 hybrid_total = time_diff.count() * 1000;
454
455 if (FLAGS_enable_parallel_trajectory_smoothing) {
456 std::vector<HybridAStartResult> partition_trajectories;
457 if (!hybridA_ptr->TrajectoryPartition(hybrid_astar_result,
458 &partition_trajectories)) {
459 AERROR << "TrajectoryPartition failed";
460 *(result_ptr->PrepareHybridAResult()) = hybrid_astar_result;
461 return false;
462 }
463 size_t size = partition_trajectories.size();
464 std::vector<Eigen::MatrixXd> state_result_ds_vec;
465 std::vector<Eigen::MatrixXd> control_result_ds_vec;
466 std::vector<Eigen::MatrixXd> time_result_ds_vec;
467 std::vector<Eigen::MatrixXd> dual_l_result_ds_vec;
468 std::vector<Eigen::MatrixXd> dual_n_result_ds_vec;
469 state_result_ds_vec.resize(size);
470 control_result_ds_vec.resize(size);
471 time_result_ds_vec.resize(size);
472 dual_l_result_ds_vec.resize(size);
473 dual_n_result_ds_vec.resize(size);
474 std::vector<std::future<bool>> results;
475
476 // In parallel
477 // TODO(Jinyun): fix memory issue
478 // for (size_t i = 0; i < size; ++i) {
479 // double piece_wise_sx = partition_trajectories[i].x.front();
480 // double piece_wise_sy = partition_trajectories[i].y.front();
481 // double piece_wise_sphi = partition_trajectories[i].phi.front();
482 // double piece_wise_ex = partition_trajectories[i].x.back();
483 // double piece_wise_ey = partition_trajectories[i].y.back();
484 // double piece_wise_ephi = partition_trajectories[i].phi.back();
485 // auto* ith_trajectories = &partition_trajectories[i];
486 // auto* ith_state_result = &state_result_ds_vec[i];
487 // auto* ith_control_result = &control_result_ds_vec[i];
488 // auto* ith_time_result = &time_result_ds_vec[i];
489 // auto* ith_dual_l_result = &dual_l_result_ds_vec[i];
490 // auto* ith_dual_n_result = &dual_n_result_ds_vec[i];
491 // results.push_back(
492 // cyber::Async(&DistanceSmoothing,
493 // std::ref(planner_open_space_config_),
494 // std::ref(*obstacles_ptr), piece_wise_sx,
495 // piece_wise_sy, piece_wise_sphi, piece_wise_ex,
496 // piece_wise_ey, piece_wise_ephi, std::ref(XYbounds_),
497 // ith_trajectories, ith_state_result,
498 // ith_control_result, ith_time_result,
499 // ith_dual_l_result, ith_dual_n_result));
500 // }
501 // for (auto& result : results) {
502 // if (!result.get()) {
503 // AERROR << "Failure in a piece of trajectory.";
504 // return false;
505 // }
506 // }
507
508 // In for loop
509 double dual_tmp = 0.0;
510 double ipopt_tmp = 0.0;
511 for (size_t i = 0; i < size; ++i) {
512 double piece_wise_sx = partition_trajectories[i].x.front();
513 double piece_wise_sy = partition_trajectories[i].y.front();
514 double piece_wise_sphi = partition_trajectories[i].phi.front();
515 double piece_wise_ex = partition_trajectories[i].x.back();
516 double piece_wise_ey = partition_trajectories[i].y.back();
517 double piece_wise_ephi = partition_trajectories[i].phi.back();
518
519 if (!DistanceSmoothing(planner_open_space_config_, *obstacles_ptr,
520 piece_wise_sx, piece_wise_sy, piece_wise_sphi,
521 piece_wise_ex, piece_wise_ey, piece_wise_ephi,
522 XYbounds_, &partition_trajectories[i],
523 &state_result_ds_vec[i], &control_result_ds_vec[i],
524 &time_result_ds_vec[i], &dual_l_result_ds_vec[i],
525 &dual_n_result_ds_vec[i], dual_tmp, ipopt_tmp)) {
526 *(result_ptr->PrepareHybridAResult()) = hybrid_astar_result;
527 return false;
528 }
529 dual_total += dual_tmp;
530 ipopt_total += ipopt_tmp;
531 }
532
533 // Retrieve result in one single trajectory
534 size_t trajectory_point_size = 0;
535 for (size_t i = 0; i < size; ++i) {
536 if (state_result_ds_vec[i].cols() < 2) {
537 AERROR << "state horizon smaller than 2";
538 return false;
539 }
540 AINFO << "trajectory idx: "
541 << "idx range: " << trajectory_point_size << ", "
542 << trajectory_point_size +
543 static_cast<size_t>(state_result_ds_vec[i].cols()) - 1;
544 trajectory_point_size +=
545 static_cast<size_t>(state_result_ds_vec[i].cols()) - 1;
546 }
547 ++trajectory_point_size;
548
549 const uint64_t state_dimension = state_result_ds_vec.front().rows();
550 Eigen::MatrixXd state_result_ds;
551 state_result_ds.resize(state_dimension, trajectory_point_size);
552 uint64_t k = 0;
553 for (size_t i = 0; i < size; ++i) {
554 // leave out the last repeated point so set column minus one
555 uint64_t state_col_num = state_result_ds_vec[i].cols() - 1;
556 for (uint64_t j = 0; j < state_col_num; ++j) {
557 state_result_ds.col(k) = state_result_ds_vec[i].col(j);
558 ++k;
559 }
560 }
561 state_result_ds.col(k) =
562 state_result_ds_vec.back().col(state_result_ds_vec.back().cols() - 1);
563
564 const uint64_t control_dimension = control_result_ds_vec.front().rows();
565 Eigen::MatrixXd control_result_ds;
566 control_result_ds.resize(control_dimension, trajectory_point_size - 1);
567 k = 0;
568
569 for (size_t i = 0; i < size; ++i) {
570 uint64_t control_col_num = control_result_ds_vec[i].cols() - 1;
571 for (uint64_t j = 0; j < control_col_num; ++j) {
572 control_result_ds.col(k) = control_result_ds_vec[i].col(j);
573 ++k;
574 }
575 }
576
577 const uint64_t time_dimension = time_result_ds_vec.front().rows();
578 Eigen::MatrixXd time_result_ds;
579 time_result_ds.resize(time_dimension, trajectory_point_size - 1);
580 k = 0;
581 for (size_t i = 0; i < size; ++i) {
582 uint64_t time_col_num = time_result_ds_vec[i].cols() - 1;
583 for (uint64_t j = 0; j < time_col_num; ++j) {
584 time_result_ds.col(k) = time_result_ds_vec[i].col(j);
585 ++k;
586 }
587 }
588
589 *(result_ptr->PrepareHybridAResult()) = hybrid_astar_result;
590 *(result_ptr->PrepareStateResult()) = state_result_ds;
591 *(result_ptr->PrepareControlResult()) = control_result_ds;
592 *(result_ptr->PrepareTimeResult()) = time_result_ds;
593 *(result_ptr->GetHybridTime()) = hybrid_total;
594 *(result_ptr->GetDualTime()) = dual_total;
595 *(result_ptr->GetIpoptTime()) = ipopt_total;
596 } else {
597 Eigen::MatrixXd state_result_ds;
598 Eigen::MatrixXd control_result_ds;
599 Eigen::MatrixXd time_result_ds;
600 Eigen::MatrixXd dual_l_result_ds;
601 Eigen::MatrixXd dual_n_result_ds;
602 if (!DistanceSmoothing(planner_open_space_config_, *obstacles_ptr, sx, sy,
603 sphi, ex, ey, ephi, XYbounds_, &hybrid_astar_result,
604 &state_result_ds, &control_result_ds,
605 &time_result_ds, &dual_l_result_ds,
606 &dual_n_result_ds, dual_total, ipopt_total)) {
607 AERROR << "DistanceSmoothing failed";
608 *(result_ptr->PrepareHybridAResult()) = hybrid_astar_result;
609 return false;
610 }
611 *(result_ptr->PrepareHybridAResult()) = hybrid_astar_result;
612 *(result_ptr->PrepareStateResult()) = state_result_ds;
613 *(result_ptr->PrepareControlResult()) = control_result_ds;
614 *(result_ptr->PrepareTimeResult()) = time_result_ds;
615 *(result_ptr->PrepareLResult()) = dual_l_result_ds;
616 *(result_ptr->PrepareNResult()) = dual_n_result_ds;
617 *(result_ptr->GetHybridTime()) = hybrid_total;
618 *(result_ptr->GetDualTime()) = dual_total;
619 *(result_ptr->GetIpoptTime()) = ipopt_total;
620 }
621 return true;
622}
bool TrajectoryPartition(const HybridAStartResult &result, std::vector< HybridAStartResult > *partitioned_result)
bool Plan(double sx, double sy, double sphi, double ex, double ey, double ephi, const std::vector< double > &XYbounds, const std::vector< std::vector< common::math::Vec2d > > &obstacles_vertices_vec, HybridAStartResult *result, const std::vector< std::vector< common::math::Vec2d > > &soft_boundary_vertices_vec={}, bool reeds_sheep_last_straight=false)
const std::vector< std::vector< Vec2d > > & GetObstacleVec() const
#define AERROR
Definition log.h:44
bool DistanceSmoothing(const apollo::planning::PlannerOpenSpaceConfig &planner_open_space_config, const ObstacleContainer &obstacles, double sx, double sy, double sphi, double ex, double ey, double ephi, const std::vector< double > &XYbounds, HybridAStartResult *hybrid_a_star_result, Eigen::MatrixXd *state_result_ds_, Eigen::MatrixXd *control_result_ds_, Eigen::MatrixXd *time_result_ds_, Eigen::MatrixXd *dual_l_result_ds_, Eigen::MatrixXd *dual_n_result_ds_, double &dual_time, double &ipopt_time)

◆ DistanceSmoothing()

bool apollo::planning::DistanceSmoothing ( const apollo::planning::PlannerOpenSpaceConfig planner_open_space_config,
const ObstacleContainer obstacles,
double  sx,
double  sy,
double  sphi,
double  ex,
double  ey,
double  ephi,
const std::vector< double > &  XYbounds,
HybridAStartResult hybrid_a_star_result,
Eigen::MatrixXd *  state_result_ds_,
Eigen::MatrixXd *  control_result_ds_,
Eigen::MatrixXd *  time_result_ds_,
Eigen::MatrixXd *  dual_l_result_ds_,
Eigen::MatrixXd *  dual_n_result_ds_,
double &  dual_time,
double &  ipopt_time 
)

在文件 distance_approach_problem_wrapper.cc258 行定义.

265 {
266 // load Warm Start result(horizon is the "N", not the size of step points)
267 size_t horizon_ = hybrid_a_star_result->x.size() - 1;
268 // nominal sampling time
269 float ts_ = planner_open_space_config.delta_t();
270
271 Eigen::VectorXd x;
272 Eigen::VectorXd y;
273 Eigen::VectorXd phi;
274 Eigen::VectorXd v;
275 Eigen::VectorXd steer;
276 Eigen::VectorXd a;
277
278 // TODO(Runxin): extend logics in future
279 if (horizon_ <= 10 && horizon_ > 2 &&
280 planner_open_space_config.enable_linear_interpolation()) {
281 // TODO(Runxin): extend this number
282 int extend_size = 5;
283 // modify state and control vectors sizes
284 horizon_ = extend_size * horizon_;
285 // modify delta t
286 ts_ = ts_ / static_cast<float>(extend_size);
287
288 std::vector<double> x_extend =
289 VectorLinearInterpolation(hybrid_a_star_result->x, extend_size);
290 x = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(x_extend.data(),
291 horizon_ + 1);
292
293 std::vector<double> y_extend =
294 VectorLinearInterpolation(hybrid_a_star_result->y, extend_size);
295 y = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(y_extend.data(),
296 horizon_ + 1);
297
298 std::vector<double> phi_extend =
299 VectorLinearInterpolation(hybrid_a_star_result->phi, extend_size);
300 phi = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(phi_extend.data(),
301 horizon_ + 1);
302
303 std::vector<double> v_extend =
304 VectorLinearInterpolation(hybrid_a_star_result->v, extend_size);
305 v = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(v_extend.data(),
306 horizon_ + 1);
307
308 steer = Eigen::VectorXd(horizon_);
309 a = Eigen::VectorXd(horizon_);
310 for (size_t i = 0; i < static_cast<size_t>(horizon_); ++i) {
311 steer[i] = hybrid_a_star_result->steer[i / extend_size];
312 a[i] = hybrid_a_star_result->a[i / extend_size];
313 }
314 ADEBUG << "hybrid A x: ";
315 for (size_t i = 0; i < hybrid_a_star_result->x.size(); ++i) {
316 ADEBUG << "i: " << i << ", val: " << hybrid_a_star_result->x[i];
317 }
318 ADEBUG << "interpolated x: \n" << x;
319
320 ADEBUG << "hybrid A steer: ";
321 for (size_t i = 0; i < hybrid_a_star_result->steer.size(); ++i) {
322 ADEBUG << "i: " << i << ", val: " << hybrid_a_star_result->steer[i];
323 }
324 ADEBUG << "interpolated steer: \n" << steer;
325 } else {
326 x = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
327 hybrid_a_star_result->x.data(), horizon_ + 1);
328 y = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
329 hybrid_a_star_result->y.data(), horizon_ + 1);
330 phi = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
331 hybrid_a_star_result->phi.data(), horizon_ + 1);
332 v = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
333 hybrid_a_star_result->v.data(), horizon_ + 1);
334 steer = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
335 hybrid_a_star_result->steer.data(), horizon_);
336 a = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
337 hybrid_a_star_result->a.data(), horizon_);
338 }
339
340 Eigen::MatrixXd xWS = Eigen::MatrixXd::Zero(4, horizon_ + 1);
341 Eigen::MatrixXd uWS = Eigen::MatrixXd::Zero(2, horizon_);
342 xWS.row(0) = x;
343 xWS.row(1) = y;
344 xWS.row(2) = phi;
345 xWS.row(3) = v;
346 uWS.row(0) = steer;
347 uWS.row(1) = a;
348
349 Eigen::MatrixXd x0(4, 1);
350 x0 << sx, sy, sphi, 0.0;
351
352 Eigen::MatrixXd xF(4, 1);
353 xF << ex, ey, ephi, 0.0;
354
355 Eigen::MatrixXd last_time_u(2, 1);
356 last_time_u << 0.0, 0.0;
357
358 common::VehicleParam vehicle_param_ =
359 common::VehicleConfigHelper::GetConfig().vehicle_param();
360
361 // load vehicle configuration
362 Eigen::MatrixXd ego_(4, 1);
363 double front_to_center = vehicle_param_.front_edge_to_center();
364 double back_to_center = vehicle_param_.back_edge_to_center();
365 double left_to_center = vehicle_param_.left_edge_to_center();
366 double right_to_center = vehicle_param_.right_edge_to_center();
367 ego_ << front_to_center, right_to_center, back_to_center, left_to_center;
368
369 // result for distance approach problem
370 Eigen::MatrixXd l_warm_up;
371 Eigen::MatrixXd n_warm_up;
372 Eigen::MatrixXd s_warm_up =
373 Eigen::MatrixXd::Zero(obstacles.GetObstaclesNum(), horizon_ + 1);
374
375 DualVariableWarmStartProblem* dual_variable_warm_start_ptr =
376 new DualVariableWarmStartProblem(planner_open_space_config);
377
378 const auto t1 = std::chrono::system_clock::now();
379 if (FLAGS_use_dual_variable_warm_start) {
380 bool dual_variable_warm_start_status = dual_variable_warm_start_ptr->Solve(
381 horizon_, ts_, ego_, obstacles.GetObstaclesNum(),
382 obstacles.GetObstaclesEdgesNum(), obstacles.GetAMatrix(),
383 obstacles.GetbMatrix(), xWS, &l_warm_up, &n_warm_up, &s_warm_up);
384
385 if (dual_variable_warm_start_status) {
386 AINFO << "Dual variable problem solved successfully!";
387 } else {
388 AERROR << "Dual variable problem solving failed";
389 return false;
390 }
391 } else {
392 l_warm_up = Eigen::MatrixXd::Zero(obstacles.GetObstaclesEdgesNum().sum(),
393 horizon_ + 1);
394 n_warm_up =
395 Eigen::MatrixXd::Zero(4 * obstacles.GetObstaclesNum(), horizon_ + 1);
396 }
397 const auto t2 = std::chrono::system_clock::now();
398 dual_time = std::chrono::duration<double>(t2 - t1).count() * 1000;
399
400 DistanceApproachProblem* distance_approach_ptr =
401 new DistanceApproachProblem(planner_open_space_config);
402
403 bool status = distance_approach_ptr->Solve(
404 x0, xF, last_time_u, horizon_, ts_, ego_, xWS, uWS, l_warm_up, n_warm_up,
405 s_warm_up, XYbounds, obstacles.GetObstaclesNum(),
406 obstacles.GetObstaclesEdgesNum(), obstacles.GetAMatrix(),
407 obstacles.GetbMatrix(), state_result_ds_, control_result_ds_,
408 time_result_ds_, dual_l_result_ds_, dual_n_result_ds_);
409 const auto t3 = std::chrono::system_clock::now();
410 ipopt_time = std::chrono::duration<double>(t3 - t2).count() * 1000;
411
412 if (!status) {
413 AERROR << "Distance fail";
414 return false;
415 }
416 return true;
417}
std::vector< double > VectorLinearInterpolation(const std::vector< double > &x, int extend_size)

◆ EvaluateTrajectory()

void apollo::planning::EvaluateTrajectory ( )

在文件 evaluate_trajectory.cc33 行定义.

33 {
34 const std::vector<std::string> inputs =
35 absl::StrSplit(FLAGS_planning_source_dirs, ':');
36 Evaluator evaluator;
37 evaluator.Init();
38 for (const auto& input : inputs) {
39 std::vector<std::string> source_files;
40 util::GetFilesByPath(boost::filesystem::path(input), &source_files);
41 auto it = source_files.begin();
42 while (it != source_files.end()) {
43 if ((*it).substr((*it).size() - 4) != ".bin") {
44 it = source_files.erase(it);
45 } else {
46 ++it;
47 }
48 }
49 AINFO << "For input " << input << ", found " << source_files.size()
50 << " files to process";
51 for (std::size_t i = 0; i < source_files.size(); ++i) {
52 AINFO << "\tProcessing: [ " << i + 1 << " / " << source_files.size()
53 << " ]: " << source_files[i];
54 evaluator.Evaluate(source_files[i]);
55 }
56 }
57 evaluator.Close();
58}
void Evaluate(const std::string &source_file)
Definition evaluator.cc:49

◆ fill_lower_left()

bool apollo::planning::fill_lower_left ( int *  iRow,
int *  jCol,
unsigned int *  rind_L,
unsigned int *  cind_L,
const int  nnz_L 
)

◆ fill_lower_left_gpu()

__global__ void apollo::planning::fill_lower_left_gpu ( int *  iRow,
int *  jCol,
unsigned int *  rind_L,
unsigned int *  cind_L,
const int  nnz_L 
)

◆ GenerateLearningData()

void apollo::planning::GenerateLearningData ( )

在文件 record_to_learning_data.cc34 行定义.

34 {
35 AINFO << "map_dir: " << FLAGS_map_dir;
36 if (FLAGS_planning_offline_bags.empty()) {
37 return;
38 }
39
40 if (!FeatureOutput::Ready()) {
41 AERROR << "Feature output is not ready.";
42 return;
43 }
44
45 const std::string planning_config_file =
46 "modules/planning/planning_component/conf/planning_config.pb.txt";
47 PlanningConfig planning_config;
48 ACHECK(
49 cyber::common::GetProtoFromFile(planning_config_file, &planning_config))
50 << "failed to load planning config file " << planning_config_file;
51
52 MessageProcess message_process;
53 if (!message_process.Init(planning_config)) {
54 return;
55 }
56
57 const std::vector<std::string> inputs =
58 absl::StrSplit(FLAGS_planning_offline_bags, ':');
59 for (const auto& input : inputs) {
60 std::vector<std::string> offline_bags;
61 util::GetFilesByPath(boost::filesystem::path(input), &offline_bags);
62 std::sort(offline_bags.begin(), offline_bags.end());
63 AINFO << "For input " << input << ", found " << offline_bags.size()
64 << " rosbags to process";
65 for (std::size_t i = 0; i < offline_bags.size(); ++i) {
66 AINFO << "\tProcessing: [ " << i + 1 << " / " << offline_bags.size()
67 << " ]: " << offline_bags[i];
68 message_process.ProcessOfflineData(offline_bags[i]);
69 FeatureOutput::WriteRemainderiLearningData(offline_bags[i]);
70 }
71 }
72 message_process.Close();
73}

◆ GetBackToInLaneIndex() [1/2]

int apollo::planning::GetBackToInLaneIndex ( const std::vector< PathPointDecision > &  path_point_decision)

在文件 lane_borrow_path.cc772 行定义.

773 {
774 // ACHECK(!path_point_decision.empty());
775 // ACHECK(std::get<1>(path_point_decision.back()) ==
776 // PathData::PathPointType::IN_LANE);
777
778 for (int i = static_cast<int>(path_point_decision.size()) - 1; i >= 0; --i) {
779 if (std::get<1>(path_point_decision[i]) !=
780 PathData::PathPointType::IN_LANE) {
781 return i;
782 }
783 }
784 return 0;
785}

◆ GetBackToInLaneIndex() [2/2]

int apollo::planning::GetBackToInLaneIndex ( const std::vector< std::tuple< double, PathData::PathPointType, double > > &  path_point_decision)

◆ GetDistanceBetweenADCAndObstacle()

double apollo::planning::GetDistanceBetweenADCAndObstacle ( const Frame frame,
const Obstacle obstacle 
)

在文件 obstacle_blocking_analyzer.cc172 行定义.

173 {
174 const auto& reference_line_info = frame.reference_line_info().front();
175 const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
176 double distance_between_adc_and_obstacle =
177 obstacle->PerceptionSLBoundary().start_s() - adc_sl_boundary.end_s();
178 return distance_between_adc_and_obstacle;
179}

◆ GetResult()

void apollo::planning::GetResult ( HybridAResultContainer result_ptr,
double *  x,
double *  y,
double *  phi,
double *  v,
double *  a,
double *  steer,
size_t *  output_size 
)

在文件 hybrid_a_star_wrapper.cc183 行定义.

191 {
192 result_ptr->LoadResult();
193 size_t size = result_ptr->GetX()->size();
194 std::cout << "return size is " << size << std::endl;
195 for (size_t i = 0; i < size; i++) {
196 x[i] = result_ptr->GetX()->at(i);
197 y[i] = result_ptr->GetY()->at(i);
198 phi[i] = result_ptr->GetPhi()->at(i);
199 v[i] = result_ptr->GetV()->at(i);
200 }
201 for (size_t i = 0; i < size - 1; i++) {
202 a[i] = result_ptr->GetA()->at(i);
203 steer[i] = result_ptr->GetSteer()->at(i);
204 }
205 *output_size = size;
206}

◆ HysteresisFilter()

bool apollo::planning::HysteresisFilter ( const double  obstacle_distance,
const double  safe_distance,
const double  distance_buffer,
const bool  is_obstacle_blocking 
)

在文件 lane_change_util.cc176 行定义.

178 {
179 if (is_obstacle_blocking) {
180 return obstacle_distance < safe_distance + distance_buffer;
181 } else {
182 return obstacle_distance < safe_distance - distance_buffer;
183 }
184}

◆ InitialCuda()

bool apollo::planning::InitialCuda ( )

◆ InterpolateUsingLinearApproximation()

double apollo::planning::InterpolateUsingLinearApproximation ( const double  p0,
const double  p1,
const double  w 
)

在文件 distance_approach_problem_wrapper.cc233 行定义.

234 {
235 return p0 * (1.0 - w) + p1 * w;
236}

◆ IsBlockingDrivingPathObstacle()

bool apollo::planning::IsBlockingDrivingPathObstacle ( const ReferenceLine reference_line,
const Obstacle obstacle 
)

在文件 obstacle_blocking_analyzer.cc181 行定义.

182 {
183 const double driving_width =
184 reference_line.GetDrivingWidth(obstacle->PerceptionSLBoundary());
185 const double adc_width =
186 VehicleConfigHelper::GetConfig().vehicle_param().width();
187 ADEBUG << " (driving width = " << driving_width
188 << ", adc_width = " << adc_width << ")";
189 if (driving_width > adc_width + FLAGS_static_obstacle_nudge_l_buffer +
190 FLAGS_side_pass_driving_width_l_buffer) {
191 // TODO(jiacheng): make this a GFLAG:
192 // side_pass_context_.scenario_config_.min_l_nudge_buffer()
193 ADEBUG << "It is NOT blocking our path.";
194 return false;
195 }
196
197 ADEBUG << "It is blocking our path.";
198 return true;
199}
double GetDrivingWidth(const SLBoundary &sl_boundary) const

◆ IsBlockingObstacleFarFromIntersection()

bool apollo::planning::IsBlockingObstacleFarFromIntersection ( const ReferenceLineInfo reference_line_info,
const std::string &  blocking_obstacle_id 
)

在文件 obstacle_blocking_analyzer.cc234 行定义.

236 {
237 if (blocking_obstacle_id.empty()) {
238 ADEBUG << "There is no blocking obstacle.";
239 return true;
240 }
241 const Obstacle* blocking_obstacle =
242 reference_line_info.path_decision().obstacles().Find(
243 blocking_obstacle_id);
244 if (blocking_obstacle == nullptr) {
245 ADEBUG << "Blocking obstacle is no longer there.";
246 return true;
247 }
248
249 // Get blocking obstacle's s.
250 double blocking_obstacle_s =
251 blocking_obstacle->PerceptionSLBoundary().end_s();
252 ADEBUG << "Blocking obstacle is at s = " << blocking_obstacle_s;
253 // Get intersection's s and compare with threshold.
254 const auto& first_encountered_overlaps =
255 reference_line_info.FirstEncounteredOverlaps();
256 for (const auto& overlap : first_encountered_overlaps) {
257 ADEBUG << overlap.first << ", " << overlap.second.DebugString();
258 if (overlap.first != ReferenceLineInfo::SIGNAL &&
259 overlap.first != ReferenceLineInfo::STOP_SIGN) {
260 continue;
261 }
262
263 auto distance = overlap.second.start_s - blocking_obstacle_s;
264 if (overlap.first == ReferenceLineInfo::SIGNAL ||
265 overlap.first == ReferenceLineInfo::STOP_SIGN) {
266 if (distance < kIntersectionClearanceDist) {
267 ADEBUG << "Too close to signal intersection (" << distance
268 << "m); don't SIDE_PASS.";
269 return false;
270 }
271 } else {
272 if (distance < kJunctionClearanceDist) {
273 ADEBUG << "Too close to overlap_type[" << overlap.first << "] ("
274 << distance << "m); don't SIDE_PASS";
275 return false;
276 }
277 }
278 }
279
280 return true;
281}

◆ IsBlockingObstacleToSidePass()

bool apollo::planning::IsBlockingObstacleToSidePass ( const Frame frame,
const Obstacle obstacle,
double  block_obstacle_min_speed,
double  min_front_sidepass_distance,
bool  enable_obstacle_blocked_check 
)

Decide whether an obstacle is a blocking one that needs to be side-passed.

参数
Theframe that contains reference_line and other info.
Theobstacle of interest.
Thespeed threshold to tell whether an obstacle is stopped.
Theminimum distance to front blocking obstacle for side-pass. (if too close, don't side-pass for safety consideration)
Whetherto take into consideration that the blocking obstacle itself is blocked by others as well. In other words, if the front blocking obstacle is blocked by others, then don't try to side-pass it. (Parked obstacles are never blocked by others)

在文件 obstacle_blocking_analyzer.cc88 行定义.

91 {
92 // Get the necessary info.
93 const auto& reference_line_info = frame.reference_line_info().front();
94 const auto& reference_line = reference_line_info.reference_line();
95 const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
96 const PathDecision& path_decision = reference_line_info.path_decision();
97 ADEBUG << "Evaluating Obstacle: " << obstacle->Id();
98
99 // Obstacle is virtual.
100 if (obstacle->IsVirtual()) {
101 ADEBUG << " - It is virtual.";
102 return false;
103 }
104
105 // Obstacle is moving.
106 if (!obstacle->IsStatic() || obstacle->speed() > block_obstacle_min_speed) {
107 ADEBUG << " - It is non-static.";
108 return false;
109 }
110
111 // Obstacle is behind ADC.
112 if (obstacle->PerceptionSLBoundary().start_s() <= adc_sl_boundary.end_s()) {
113 ADEBUG << " - It is behind ADC.";
114 return false;
115 }
116
117 // Obstacle is far away.
118 static constexpr double kAdcDistanceSidePassThreshold = 15.0;
119 if (obstacle->PerceptionSLBoundary().start_s() >
120 adc_sl_boundary.end_s() + kAdcDistanceSidePassThreshold) {
121 ADEBUG << " - It is too far ahead.";
122 return false;
123 }
124
125 // Obstacle is too close.
126 if (adc_sl_boundary.end_s() + min_front_sidepass_distance >
127 obstacle->PerceptionSLBoundary().start_s()) {
128 ADEBUG << " - It is too close to side-pass.";
129 return false;
130 }
131
132 // Obstacle is not blocking our path.
133 if (!IsBlockingDrivingPathObstacle(reference_line, obstacle)) {
134 ADEBUG << " - It is not blocking our way.";
135 return false;
136 }
137
138 // Obstacle is blocked by others too.
139 if (enable_obstacle_blocked_check &&
140 !IsParkedVehicle(reference_line, obstacle)) {
141 for (const auto* other_obstacle : path_decision.obstacles().Items()) {
142 if (other_obstacle->Id() == obstacle->Id()) {
143 continue;
144 }
145 if (other_obstacle->IsVirtual()) {
146 continue;
147 }
148 if (other_obstacle->PerceptionSLBoundary().start_l() >
149 obstacle->PerceptionSLBoundary().end_l() ||
150 other_obstacle->PerceptionSLBoundary().end_l() <
151 obstacle->PerceptionSLBoundary().start_l()) {
152 // not blocking the backside vehicle
153 continue;
154 }
155 double delta_s = other_obstacle->PerceptionSLBoundary().start_s() -
156 obstacle->PerceptionSLBoundary().end_s();
157 if (delta_s < 0.0 || delta_s > kAdcDistanceThreshold) {
158 continue;
159 }
160
161 // TODO(All): Fix the segmentation bug for large vehicles, otherwise
162 // the follow line will be problematic.
163 ADEBUG << " - It is blocked by others, too.";
164 return false;
165 }
166 }
167
168 ADEBUG << "IT IS BLOCKING!";
169 return true;
170}
double speed() const
Definition obstacle.h:78
PathDecision represents all obstacle decisions on one path.
bool IsBlockingDrivingPathObstacle(const ReferenceLine &reference_line, const Obstacle *obstacle)
bool IsParkedVehicle(const ReferenceLine &reference_line, const Obstacle *obstacle)

◆ IsBlockingObstacleWithinDestination()

bool apollo::planning::IsBlockingObstacleWithinDestination ( const ReferenceLineInfo reference_line_info,
const std::string &  blocking_obstacle_id,
const double  threshold 
)

在文件 obstacle_blocking_analyzer.cc365 行定义.

367 {
368 if (blocking_obstacle_id.empty()) {
369 ADEBUG << "There is no blocking obstacle.";
370 return true;
371 }
372 const Obstacle* blocking_obstacle =
373 reference_line_info.path_decision().obstacles().Find(
374 blocking_obstacle_id);
375 if (blocking_obstacle == nullptr) {
376 ADEBUG << "Blocking obstacle is no longer there.";
377 return true;
378 }
379
380 double blocking_obstacle_s =
381 blocking_obstacle->PerceptionSLBoundary().start_s();
382 double adc_end_s = reference_line_info.AdcSlBoundary().end_s();
383 ADEBUG << "Blocking obstacle is at s = " << blocking_obstacle_s;
384 ADEBUG << "ADC is at s = " << adc_end_s;
385 ADEBUG << "Destination is at s = "
386 << reference_line_info.SDistanceToDestination() + adc_end_s;
387 if (blocking_obstacle_s - adc_end_s + threshold >
388 reference_line_info.SDistanceToDestination()) {
389 return false;
390 }
391 return true;
392}

◆ IsClearToChangeLane()

bool apollo::planning::IsClearToChangeLane ( ReferenceLineInfo reference_line_info)

A static function to check if the ChangeLanePath type of reference line is safe or if current reference line is safe to deviate away and come back

在文件 lane_change_util.cc26 行定义.

26 {
27 double ego_start_s = reference_line_info->AdcSlBoundary().start_s();
28 double ego_end_s = reference_line_info->AdcSlBoundary().end_s();
29 double ego_v =
30 std::abs(reference_line_info->vehicle_state().linear_velocity());
31
32 for (const auto* obstacle :
33 reference_line_info->path_decision()->obstacles().Items()) {
34 if (obstacle->IsVirtual() || obstacle->IsStatic()) {
35 ADEBUG << "skip one virtual or static obstacle";
36 continue;
37 }
38
39 double start_s = std::numeric_limits<double>::max();
40 double end_s = -std::numeric_limits<double>::max();
41 double start_l = std::numeric_limits<double>::max();
42 double end_l = -std::numeric_limits<double>::max();
43
44 for (const auto& p : obstacle->PerceptionPolygon().points()) {
46 reference_line_info->reference_line().XYToSL(p, &sl_point);
47 start_s = std::fmin(start_s, sl_point.s());
48 end_s = std::fmax(end_s, sl_point.s());
49
50 start_l = std::fmin(start_l, sl_point.l());
51 end_l = std::fmax(end_l, sl_point.l());
52 }
53
54 if (reference_line_info->IsChangeLanePath()) {
55 double left_width(0), right_width(0);
56 reference_line_info->mutable_reference_line()->GetLaneWidth(
57 (start_s + end_s) * 0.5, &left_width, &right_width);
58 if (end_l < -right_width || start_l > left_width) {
59 continue;
60 }
61 }
62
63 // Raw estimation on whether same direction with ADC or not based on
64 // prediction trajectory
65 bool same_direction = true;
66 if (obstacle->HasTrajectory()) {
67 double obstacle_moving_direction =
68 obstacle->Trajectory().trajectory_point(0).path_point().theta();
69 const auto& vehicle_state = reference_line_info->vehicle_state();
70 double vehicle_moving_direction = vehicle_state.heading();
71 if (vehicle_state.gear() == canbus::Chassis::GEAR_REVERSE) {
72 vehicle_moving_direction =
73 common::math::NormalizeAngle(vehicle_moving_direction + M_PI);
74 }
75 double heading_difference = std::abs(common::math::NormalizeAngle(
76 obstacle_moving_direction - vehicle_moving_direction));
77 same_direction = heading_difference < (M_PI / 2.0);
78 }
79
80 // TODO(All) move to confs
81 static constexpr double kSafeTimeOnSameDirection = 3.0;
82 static constexpr double kSafeTimeOnOppositeDirection = 5.0;
83 static constexpr double kForwardMinSafeDistanceOnSameDirection = 10.0;
84 static constexpr double kBackwardMinSafeDistanceOnSameDirection = 10.0;
85 static constexpr double kForwardMinSafeDistanceOnOppositeDirection = 50.0;
86 static constexpr double kBackwardMinSafeDistanceOnOppositeDirection = 1.0;
87 static constexpr double kDistanceBuffer = 0.5;
88
89 double kForwardSafeDistance = 0.0;
90 double kBackwardSafeDistance = 0.0;
91 if (same_direction) {
92 kForwardSafeDistance =
93 std::fmax(kForwardMinSafeDistanceOnSameDirection,
94 (ego_v - obstacle->speed()) * kSafeTimeOnSameDirection);
95 kBackwardSafeDistance =
96 std::fmax(kBackwardMinSafeDistanceOnSameDirection,
97 (obstacle->speed() - ego_v) * kSafeTimeOnSameDirection);
98 } else {
99 kForwardSafeDistance =
100 std::fmax(kForwardMinSafeDistanceOnOppositeDirection,
101 (ego_v + obstacle->speed()) * kSafeTimeOnOppositeDirection);
102 kBackwardSafeDistance = kBackwardMinSafeDistanceOnOppositeDirection;
103 }
104
105 if (HysteresisFilter(ego_start_s - end_s, kBackwardSafeDistance,
106 kDistanceBuffer, obstacle->IsLaneChangeBlocking()) &&
107 HysteresisFilter(start_s - ego_end_s, kForwardSafeDistance,
108 kDistanceBuffer, obstacle->IsLaneChangeBlocking())) {
109 reference_line_info->path_decision()
110 ->Find(obstacle->Id())
111 ->SetLaneChangeBlocking(true);
112 ADEBUG << "Lane Change is blocked by obstacle" << obstacle->Id();
113 return false;
114 } else {
115 reference_line_info->path_decision()
116 ->Find(obstacle->Id())
117 ->SetLaneChangeBlocking(false);
118 }
119 }
120 return true;
121}
void SetLaneChangeBlocking(const bool is_distance_clear)
Definition obstacle.cc:767
const Obstacle * Find(const std::string &object_id) const
const common::VehicleState & vehicle_state() const
bool IsChangeLanePath() const
Check if the current reference line is a change lane reference line, i.e., ADC's current position is ...
bool XYToSL(const common::math::Vec2d &xy_point, common::SLPoint *const sl_point, double warm_start_s=-1.0) const
Transvert Cartesian coordinates to Frenet.
bool GetLaneWidth(const double s, double *const lane_left_width, double *const lane_right_width) const
bool HysteresisFilter(const double obstacle_distance, const double safe_distance, const double distance_buffer, const bool is_obstacle_blocking)

◆ IsNonmovableObstacle()

bool apollo::planning::IsNonmovableObstacle ( const ReferenceLineInfo reference_line_info,
const Obstacle obstacle 
)

在文件 obstacle_blocking_analyzer.cc40 行定义.

41 {
42 // Obstacle is far away.
43 const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
44 if (obstacle.PerceptionSLBoundary().start_s() >
45 adc_sl_boundary.end_s() + kAdcDistanceThreshold) {
46 ADEBUG << " - It is too far ahead and we are not so sure of its status.";
47 return false;
48 }
49
50 // Obstacle is parked obstacle.
51 if (IsParkedVehicle(reference_line_info.reference_line(), &obstacle)) {
52 ADEBUG << "It is Parked and NON-MOVABLE.";
53 return true;
54 }
55
56 // Obstacle is blocked by others too.
57 for (const auto* other_obstacle :
58 reference_line_info.path_decision().obstacles().Items()) {
59 if (other_obstacle->Id() == obstacle.Id()) {
60 continue;
61 }
62 if (other_obstacle->IsVirtual()) {
63 continue;
64 }
65 if (other_obstacle->Perception().type() !=
67 continue;
68 }
69 const auto& other_boundary = other_obstacle->PerceptionSLBoundary();
70 const auto& this_boundary = obstacle.PerceptionSLBoundary();
71 if (other_boundary.start_l() > this_boundary.end_l() ||
72 other_boundary.end_l() < this_boundary.start_l()) {
73 // not blocking the backside vehicle
74 continue;
75 }
76 double delta_s = other_boundary.start_s() - this_boundary.end_s();
77 if (delta_s < 0.0 || delta_s > kObstaclesDistanceThreshold) {
78 continue;
79 }
80 return false;
81 }
82 ADEBUG << "IT IS NON-MOVABLE!";
83 return true;
84}

◆ IsParkedVehicle()

bool apollo::planning::IsParkedVehicle ( const ReferenceLine reference_line,
const Obstacle obstacle 
)

在文件 obstacle_blocking_analyzer.cc201 行定义.

202 {
203 if (!FLAGS_enable_scenario_side_pass_multiple_parked_obstacles) {
204 return false;
205 }
206 double road_left_width = 0.0;
207 double road_right_width = 0.0;
208 double max_road_right_width = 0.0;
209 reference_line.GetRoadWidth(obstacle->PerceptionSLBoundary().start_s(),
210 &road_left_width, &road_right_width);
211 max_road_right_width = road_right_width;
212 reference_line.GetRoadWidth(obstacle->PerceptionSLBoundary().end_s(),
213 &road_left_width, &road_right_width);
214 max_road_right_width = std::max(max_road_right_width, road_right_width);
215 bool is_at_road_edge = std::abs(obstacle->PerceptionSLBoundary().start_l()) >
216 max_road_right_width - 0.1;
217
218 std::vector<std::shared_ptr<const hdmap::LaneInfo>> lanes;
219 auto obstacle_box = obstacle->PerceptionBoundingBox();
220 HDMapUtil::BaseMapPtr()->GetLanes(
221 common::util::PointFactory::ToPointENU(obstacle_box.center().x(),
222 obstacle_box.center().y()),
223 std::min(obstacle_box.width(), obstacle_box.length()), &lanes);
224 bool is_on_parking_lane = false;
225 if (lanes.size() == 1 &&
226 lanes.front()->lane().type() == apollo::hdmap::Lane::PARKING) {
227 is_on_parking_lane = true;
228 }
229
230 bool is_parked = is_on_parking_lane || is_at_road_edge;
231 return is_parked && obstacle->IsStatic();
232}
const common::math::Box2d & PerceptionBoundingBox() const
Definition obstacle.h:90
bool GetRoadWidth(const double s, double *const road_left_width, double *const road_right_width) const

◆ IsPerceptionBlocked()

bool apollo::planning::IsPerceptionBlocked ( const ReferenceLineInfo reference_line_info,
const double  search_beam_length,
const double  search_beam_radius_intensity,
const double  search_range,
const double  is_block_angle_threshold 
)

A static function to estimate if an obstacle in certain range in front of ADV blocks too much space perception behind itself by beam scanning

参数
search_beam_lengthis the length of scanning beam
search_beam_radius_intensityis the resolution of scanning
search_rangeis the scanning range centering at ADV heading
is_block_angle_thresholdis the threshold to tell how big a block angle range is perception blocking

在文件 lane_change_util.cc123 行定义.

127 {
128 const auto& vehicle_state = reference_line_info.vehicle_state();
129 const common::math::Vec2d adv_pos(vehicle_state.x(), vehicle_state.y());
130 const double adv_heading = vehicle_state.heading();
131
132 for (auto* obstacle :
133 reference_line_info.path_decision().obstacles().Items()) {
134 double left_most_angle =
135 common::math::NormalizeAngle(adv_heading + 0.5 * search_range);
136 double right_most_angle =
137 common::math::NormalizeAngle(adv_heading - 0.5 * search_range);
138 bool right_most_found = false;
139 if (obstacle->IsVirtual()) {
140 ADEBUG << "skip one virtual obstacle";
141 continue;
142 }
143 const auto& obstacle_polygon = obstacle->PerceptionPolygon();
144 for (double search_angle = 0.0; search_angle < search_range;
145 search_angle += search_beam_radius_intensity) {
146 common::math::Vec2d search_beam_end(search_beam_length, 0.0);
147 const double beam_heading = common::math::NormalizeAngle(
148 adv_heading - 0.5 * search_range + search_angle);
149 search_beam_end.SelfRotate(beam_heading);
150 search_beam_end += adv_pos;
151 common::math::LineSegment2d search_beam(adv_pos, search_beam_end);
152
153 if (!right_most_found && obstacle_polygon.HasOverlap(search_beam)) {
154 right_most_found = true;
155 right_most_angle = beam_heading;
156 }
157
158 if (right_most_found && !obstacle_polygon.HasOverlap(search_beam)) {
159 left_most_angle = beam_heading;
160 break;
161 }
162 }
163 if (!right_most_found) {
164 // obstacle is not in search range
165 continue;
166 }
167 if (std::fabs(common::math::NormalizeAngle(
168 left_most_angle - right_most_angle)) > is_block_angle_threshold) {
169 return true;
170 }
171 }
172
173 return false;
174}

◆ Plan()

bool apollo::planning::Plan ( HybridAStar planner_ptr,
HybridAObstacleContainer obstacles_ptr,
HybridASoftBoundaryContainer soft_boundary_ptr,
HybridAResultContainer result_ptr,
double  sx,
double  sy,
double  sphi,
double  ex,
double  ey,
double  ephi,
double *  XYbounds 
)

在文件 hybrid_a_star_wrapper.cc156 行定义.

167 {
168 AERROR << "sx: " << sx;
169 std::vector<double> XYbounds_(XYbounds, XYbounds + 4);
170 return planner_ptr->Plan(
171 sx,
172 sy,
173 sphi,
174 ex,
175 ey,
176 ephi,
177 XYbounds_,
178 obstacles_ptr->GetObstaclesVerticesVec(),
179 result_ptr->PrepareResult(),
180 soft_boundary_ptr->GetSoftBoundaryVerticesVec(),
181 false);
182}
const std::vector< std::vector< common::math::Vec2d > > & GetObstaclesVerticesVec()
const std::vector< std::vector< common::math::Vec2d > > & GetSoftBoundaryVerticesVec()

◆ PopulateChartOptions()

void apollo::planning::PopulateChartOptions ( double  x_min,
double  x_max,
std::string  x_label,
double  y_min,
double  y_max,
std::string  y_label,
bool  display,
Chart chart 
)

在文件 on_lane_planning.cc704 行定义.

706 {
707 auto* options = chart->mutable_options();
708 options->mutable_x()->set_min(x_min);
709 options->mutable_x()->set_max(x_max);
710 options->mutable_y()->set_min(y_min);
711 options->mutable_y()->set_max(y_max);
712 options->mutable_x()->set_label_string(x_label);
713 options->mutable_y()->set_label_string(y_label);
714 options->set_legend_display(display);
715}

◆ printMatrix()

void apollo::planning::printMatrix ( const int  r,
const int  c,
const std::vector< c_float > &  P_data,
const std::vector< c_int > &  P_indices,
const std::vector< c_int > &  P_indptr 
)

在文件 dual_variable_warm_start_osqp_interface.cc79 行定义.

81 {
82 Eigen::MatrixXf tmp = Eigen::MatrixXf::Zero(r, c);
83
84 for (size_t i = 0; i < P_indptr.size() - 1; ++i) {
85 if (P_indptr[i] < 0 || P_indptr[i] >= static_cast<int>(P_indices.size())) {
86 continue;
87 }
88
89 for (auto idx = P_indptr[i]; idx < P_indptr[i + 1]; ++idx) {
90 int tmp_c = static_cast<int>(i);
91 int tmp_r = static_cast<int>(P_indices[idx]);
92 tmp(tmp_r, tmp_c) = static_cast<float>(P_data[idx]);
93 }
94 }
95
96 AINFO << "row number: " << r;
97 AINFO << "col number: " << c;
98 for (int i = 0; i < r; ++i) {
99 AINFO << "row number: " << i;
100 AINFO << tmp.row(i);
101 }
102}

◆ ROITest()

bool apollo::planning::ROITest ( OpenSpaceROITest test_ptr,
char *  lane_id,
char *  parking_id,
double *  unrotated_roi_boundary_x,
double *  unrotated_roi_boundary_y,
double *  roi_boundary_x,
double *  roi_boundary_y,
double *  parking_spot_x,
double *  parking_spot_y,
double *  end_pose,
double *  xy_boundary,
double *  origin_pose 
)

在文件 open_space_roi_wrapper.cc476 行定义.

488 {
489 std::string lane_id_str(lane_id);
490 std::string parking_id_str(parking_id);
491 if (!test_ptr->VPresentationObstacle(lane_id_str, parking_id_str)) {
492 AINFO << "VPresentationObstacle fail";
493 return false;
494 }
495 std::vector<std::vector<Vec2d>>* unrotated_roi_boundary_ =
497 std::vector<std::vector<Vec2d>>* roi_boundary_ =
498 test_ptr->GetROIParkingBoundary();
499 Polygon2d parking_spot_ = test_ptr->GetParkingSpotBox();
500 std::vector<double>* end_pose_ = test_ptr->GetEndPose();
501 std::vector<double>* xy_boundary_ = test_ptr->GetROIXYBoundary();
502 double origin_heading_ = test_ptr->GetOriginHeading();
503 Vec2d origin_point_ = test_ptr->GetOriginPose();
504
505 // load all into array
506 size_t index = 0;
507 for (size_t i = 0; i < unrotated_roi_boundary_->size(); i++) {
508 std::vector<Vec2d> boundary = unrotated_roi_boundary_->at(i);
509 for (size_t j = 0; j < boundary.size(); j++) {
510 unrotated_roi_boundary_x[index] = boundary[j].x();
511 unrotated_roi_boundary_y[index] = boundary[j].y();
512 index++;
513 }
514 }
515
516 index = 0;
517 for (size_t i = 0; i < roi_boundary_->size(); i++) {
518 std::vector<Vec2d> boundary = roi_boundary_->at(i);
519 for (size_t j = 0; j < boundary.size(); j++) {
520 roi_boundary_x[index] = boundary[j].x();
521 roi_boundary_y[index] = boundary[j].y();
522 index++;
523 }
524 }
525
526 index = 0;
527 std::vector<Vec2d> parking_spot_vec_ = parking_spot_.points();
528 for (size_t i = 0; i < parking_spot_vec_.size(); i++) {
529 parking_spot_x[index] = parking_spot_vec_[i].x();
530 parking_spot_y[index] = parking_spot_vec_[i].y();
531 index++;
532 }
533
534 for (size_t i = 0; i < end_pose_->size(); i++) {
535 end_pose[i] = end_pose_->at(i);
536 }
537
538 for (size_t i = 0; i < xy_boundary_->size(); i++) {
539 xy_boundary[i] = xy_boundary_->at(i);
540 }
541
542 // x, y, heading
543 origin_pose[0] = origin_point_.x();
544 origin_pose[1] = origin_point_.y();
545 origin_pose[2] = origin_heading_;
546
547 return true;
548}
std::vector< std::vector< Vec2d > > * GetROIParkingBoundary()
std::vector< std::vector< Vec2d > > * GetNoRotateROIParkingBoundary()
bool VPresentationObstacle(const std::string &lane_id, const std::string &parking_id)

◆ SetChartminmax()

void apollo::planning::SetChartminmax ( apollo::dreamview::Chart chart,
std::string  label_name_x,
std::string  label_name_y 
)

在文件 on_lane_planning.cc65 行定义.

66 {
67 auto* options = chart->mutable_options();
68 double xmin(std::numeric_limits<double>::max()),
69 xmax(std::numeric_limits<double>::lowest()),
70 ymin(std::numeric_limits<double>::max()),
71 ymax(std::numeric_limits<double>::lowest());
72 for (int i = 0; i < chart->line_size(); i++) {
73 auto* line = chart->mutable_line(i);
74 for (auto& pt : line->point()) {
75 xmin = std::min(xmin, pt.x());
76 ymin = std::min(ymin, pt.y());
77 xmax = std::max(xmax, pt.x());
78 ymax = std::max(ymax, pt.y());
79 }
80 auto* properties = line->mutable_properties();
81 (*properties)["borderWidth"] = "2";
82 (*properties)["pointRadius"] = "0";
83 (*properties)["lineTension"] = "0";
84 (*properties)["fill"] = "false";
85 (*properties)["showLine"] = "true";
86 }
87 options->mutable_x()->set_min(xmin);
88 options->mutable_x()->set_max(xmax);
89 options->mutable_x()->set_label_string(label_name_x);
90 options->mutable_y()->set_min(ymin);
91 options->mutable_y()->set_max(ymax);
92 options->mutable_y()->set_label_string(label_name_y);
93 // Set chartJS's dataset properties
94}

◆ UpdatePreparationDistance()

void apollo::planning::UpdatePreparationDistance ( const bool  is_opt_succeed,
const Frame frame,
const ReferenceLineInfo *const  reference_line_info,
PlanningContext planning_context 
)

A static function to update the prepararion distance for lane change

参数
is_opt_succeedif the optimization succeed
frameframe data for each planning cycle
reference_line_infostruct containing reference line information
planning_contextplanning context for each planning cycle

◆ VectorLinearInterpolation()

std::vector< double > apollo::planning::VectorLinearInterpolation ( const std::vector< double > &  x,
int  extend_size 
)

在文件 distance_approach_problem_wrapper.cc238 行定义.

239 {
240 // interplation example:
241 // x: [x0, x1, x2], extend_size: 3
242 // output: [y0(x0), y1, y2, y3(x1), y4, y5, y6(x2)]
243 size_t origin_last = x.size() - 1;
244 std::vector<double> res(origin_last * extend_size + 1, 0.0);
245
246 for (size_t i = 0; i < origin_last * extend_size; ++i) {
247 size_t idx0 = i / extend_size;
248 size_t idx1 = idx0 + 1;
249 double w =
250 static_cast<double>(i % extend_size) / static_cast<double>(extend_size);
251 res[i] = InterpolateUsingLinearApproximation(x[idx0], x[idx1], w);
252 }
253
254 res.back() = x.back();
255 return res;
256}

◆ WithinOverlap()

bool apollo::planning::WithinOverlap ( const hdmap::PathOverlap overlap,
double  s 
)

在文件 reference_line_info.cc328 行定义.

328 {
329 static constexpr double kEpsilon = 1e-2;
330 return overlap.start_s - kEpsilon <= s && s <= overlap.end_s + kEpsilon;
331}

变量说明

◆ kAdcDistanceThreshold

constexpr double apollo::planning::kAdcDistanceThreshold = 35.0
constexpr

在文件 obstacle_blocking_analyzer.cc35 行定义.

◆ kADCSafetyLBuffer

constexpr double apollo::planning::kADCSafetyLBuffer = 0.1
constexpr

在文件 st_obstacles_processor.h43 行定义.

◆ kIntersectionClearanceDist

constexpr double apollo::planning::kIntersectionClearanceDist = 20.0
constexpr

在文件 obstacle_blocking_analyzer.cc37 行定义.

◆ kJunctionClearanceDist

constexpr double apollo::planning::kJunctionClearanceDist = 15.0
constexpr

在文件 obstacle_blocking_analyzer.cc38 行定义.

◆ kMathEpsilon

constexpr double apollo::planning::kMathEpsilon = 1e-10
constexpr

在文件 open_space_roi_wrapper.cc42 行定义.

◆ kMinObstacleArea

constexpr double apollo::planning::kMinObstacleArea = 1e-4
constexpr

在文件 path_assessment_decider_util.h31 行定义.

◆ kObsSpeedIgnoreThreshold

constexpr double apollo::planning::kObsSpeedIgnoreThreshold = 100.0
constexpr

在文件 st_graph_data.h36 行定义.

◆ kObstaclesDistanceThreshold

constexpr double apollo::planning::kObstaclesDistanceThreshold = 15.0
constexpr

在文件 obstacle_blocking_analyzer.cc36 行定义.

◆ kOvertakenObsCautionTime

constexpr double apollo::planning::kOvertakenObsCautionTime = 0.5
constexpr

在文件 st_obstacles_processor.h46 行定义.

◆ kPathOptimizationFallbackCost

constexpr double apollo::planning::kPathOptimizationFallbackCost = 2e4
constexpr

在文件 trajectory_fallback_task.cc30 行定义.

◆ kSIgnoreThreshold

constexpr double apollo::planning::kSIgnoreThreshold = 0.01
constexpr

在文件 st_obstacles_processor.h44 行定义.

◆ kSpeedGuideLineResolution

constexpr double apollo::planning::kSpeedGuideLineResolution = 0.1
constexpr

在文件 st_guide_line.cc26 行定义.

◆ kSpeedOptimizationFallbackCost

constexpr double apollo::planning::kSpeedOptimizationFallbackCost = 2e4
constexpr

在文件 trajectory_fallback_task.cc31 行定义.

◆ kSTBoundsDeciderResolution

constexpr double apollo::planning::kSTBoundsDeciderResolution = 0.1
constexpr

在文件 st_bounds_decider.h41 行定义.

◆ kSTPassableThreshold

constexpr double apollo::planning::kSTPassableThreshold = 3.0
constexpr

在文件 st_bounds_decider.h42 行定义.

◆ kTIgnoreThreshold

constexpr double apollo::planning::kTIgnoreThreshold = 0.1
constexpr

在文件 st_obstacles_processor.h45 行定义.