1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
syntax = "proto2";
​
package apollo.prediction;
​
import "modules/common/proto/error_code.proto";
import "modules/common/proto/header.proto";
import "modules/prediction/proto/scenario.proto";
import "modules/perception/proto/perception_obstacle.proto";
import "modules/prediction/proto/feature.proto";
​
// estimated obstacle intent 预测障碍物的行为/意图
message ObstacleIntent {
enum Type {
UNKNOWN = 0;
STOP = 1;
STATIONARY = 2;
MOVING = 3;
CHANGE_LANE = 4;
LOW_ACCELERATION = 5;
HIGH_ACCELERATION = 6;
LOW_DECELERATION = 7;
HIGH_DECELERATION = 8;
}
optional Type type = 1 [default = UNKNOWN];
}
​
// self driving car intent
自车的驾驶意图
message Intent {
enum Type {
UNKNOWN = 0;
STOP = 1;
CRUISE = 2;
CHANGE_LANE = 3;
}
optional Type type = 1 [default = UNKNOWN];
}
​
message PredictionObstacle {
optional apollo.perception.PerceptionObstacle perception_obstacle = 1;
// 障碍物信息
optional double timestamp = 2;
// GPS time in seconds 时间戳
// the length of the time for this prediction (e.g. 10s)
optional double predicted_period = 3;
// 预测周期
// can have multiple trajectories per obstacle
repeated Trajectory trajectory = 4;
// 障碍物的预测轨迹,一个障碍物可能有多条预测轨迹,每条轨迹有概率值
​
// estimated obstacle intent
optional ObstacleIntent intent = 5;
// 障碍物的意图
​
optional ObstaclePriority priority = 6; // 预测给出的障碍物的处理的等级: caution/normal/ignore
​
optional bool is_static = 7 [default = false];
// 是否是静态障碍物
​
// Feature history latest -> earliest sequence
repeated Feature feature = 8;
// 用于做预测的特征信息, PNC模块不用关心,是prediction使用的
}
​
message PredictionObstacles {
// timestamp is included in header
optional apollo.common.Header header = 1;
​
// make prediction for multiple obstacles
repeated PredictionObstacle prediction_obstacle = 2;
​
// perception error code
optional apollo.common.ErrorCode perception_error_code = 3;
​
// start timestamp
optional double start_timestamp = 4;
​
// end timestamp
optional double end_timestamp = 5;
​
// self driving car intent
optional Intent intent = 6;
​
// Scenario
optional Scenario scenario = 7; // 场景信息, prediction模块使用的,PNC不用关心, Planning有自己的scenario,且二者定义不同
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
syntax = "proto2";
​
package apollo.planning;
​
import "modules/canbus/proto/chassis.proto";
import "modules/common/proto/drive_state.proto";
import "modules/common/proto/geometry.proto";
import "modules/common/proto/header.proto";
import "modules/common/proto/pnc_point.proto";
import "modules/map/proto/map_id.proto";
import "modules/planning/proto/decision.proto";
import "modules/planning/proto/planning_internal.proto";
​
message EStop {
// is_estop == true when emergency stop is required
optional bool is_estop = 1;
// 是否紧急刹车
optional string reason = 2;
// 紧急刹车原因
}
​
message TaskStats {
optional string name = 1;
optional double time_ms = 2;
}
​
message LatencyStats {
optional double total_time_ms = 1;
repeated TaskStats task_stats = 2;
optional double init_frame_time_ms = 3;
}
​
message RSSInfo {
optional bool is_rss_safe = 1;
optional double cur_dist_lon = 2;
optional double rss_safe_dist_lon = 3;
optional double acc_lon_range_minimum = 4;
optional double acc_lon_range_maximum = 5;
optional double acc_lat_left_range_minimum = 6;
optional double acc_lat_left_range_maximum = 7;
optional double acc_lat_right_range_minimum = 8;
optional double acc_lat_right_range_maximum = 9;
}
​
// next id: 24
message ADCTrajectory {
optional apollo.common.Header header = 1;
​
optional double total_path_length = 2;
// in meters, 规划轨迹的长度
optional double total_path_time = 3;
// in seconds, 规划轨迹的时间
​
// path data + speed data
repeated apollo.common.TrajectoryPoint trajectory_point = 12; // 轨迹
​
optional EStop estop = 6; // 紧急刹车
​
// path point without speed info
repeated apollo.common.PathPoint path_point = 13; // 规划的路径信息(没有速度信息)
​
// is_replan == true mean replan triggered
optional bool is_replan = 9 [default = false];
// 是否是重规划的轨迹
optional string replan_reason = 22;
// 重规划的原因
​
// Specify trajectory gear
optional apollo.canbus.Chassis.GearPosition gear = 10;
// 档位信息
​
optional apollo.planning.DecisionResult decision = 14;
//决策信息:任务决策(停车换道等),障碍物决策,灯光信息(转向信号等)
​
optional LatencyStats latency_stats = 15;
// Planning的运算时间造成的延迟
​
// the routing used for current planning result
optional apollo.common.Header routing_header = 16;
// 导航信息的时间戳
optional apollo.planning_internal.Debug debug = 8;
// planning的debug信息
​
enum RightOfWayStatus { // adc右侧道路的状态:有保护,无保护
UNPROTECTED = 0;
PROTECTED = 1;
}
optional RightOfWayStatus right_of_way_status = 17;
​
// lane id along current reference line
repeated apollo.hdmap.Id lane_id = 18; // 当前车道id
​
// set the engage advice for based on current planning result.
optional apollo.common.EngageAdvice engage_advice = 19;
​
// the region where planning cares most
message CriticalRegion {
repeated apollo.common.Polygon region = 1;
}
​
// critical region will be empty when planning is NOT sure which region is
// critical
// critical regions may or may not overlap
optional CriticalRegion critical_region = 20;
//
​
enum TrajectoryType {
UNKNOWN = 0;
NORMAL = 1;
// 规划求解成功
PATH_FALLBACK = 2;
// 路径规划求解失败,fallback
SPEED_FALLBACK = 3; // 速度规划求解失败,fallback
PATH_REUSED = 4;
// 复用上一帧路路径
}
optional TrajectoryType trajectory_type = 21 [default = UNKNOWN];
​
// lane id along target reference line
repeated apollo.hdmap.Id target_lane_id = 23; // 目标车道ID
​
// output related to RSS
optional RSSInfo rss_info = 100;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
apollo/modules/planning/common/
├── decision_data.h
├── dependency_injector.h
├── ego_info.h
├── feature_output.h
├── frame.h
├── history.h
├── indexed_list.h
├── indexed_queue.h
├── learning_based_data.h
├── local_view.h
├── message_process.h
├── obstacle.h
├── obstacle_blocking_analyzer.h
├── open_space_info.h
├── path
│   ├── discretized_path.h
│   ├── frenet_frame_path.h
│   └── path_data.h
├── path_boundary.h
├── path_decision.h
├── planning_context.h
├── planning_gflags.h
├── reference_line_info.h
├── smothers
│   └── smother.h
├── speed
│   ├── speed_data.h
│   ├── st_boundary.h
│   └── st_point.h
├── speed_limit.h
├── speed_profile_generator.h
├── st_graph_data.h
├── trajectory
│   ├── discretized_trajectory.h
│   └── publishable_trajectory.h
├── trajectory1d
│   ├── constant_deceleration_trajectory1d.h
│   ├── constant_jerk_trajectory1d.h
│   ├── piecewise_acceleration_trajectory1d.h
│   ├── piecewise_jerk_trajectory1d.h
│   ├── piecewise_trajectory1d.h
│   └── standing_still_trajectory1d.h
├── trajectory_evaluator.h
├── trajectory_stitcher.h
└── util
   ├── common.h
├── math_util.h
  
└── util.h

1
2
3
4
5
6
/**
* @brief main logic of the planning module, runs periodically triggered by
* timer.
*/
void RunOnce(const LocalView& local_view,
ADCTrajectory* const ptr_trajectory_pb) override;

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
class OnLanePlannerDispatcherTest : public ::testing::Test {
public:
virtual void SetUp() {}
protected:
std::unique_ptr<PlannerDispatcher> pd_;
};
TEST_F(OnLanePlannerDispatcherTest, Simple) {
auto injector = std::make_shared<DependencyInjector>();
pd_.reset(new OnLanePlannerDispatcher());
pd_->Init();
const std::string planning_config_file =
"/apollo/modules/planning/conf/planning_config.pb.txt";
PlanningConfig planning_config;
apollo::cyber::common::GetProtoFromFile(planning_config_file,
&planning_config);
auto planner = pd_->DispatchPlanner(planning_config, injector);
EXPECT_EQ(planner->Name(), "PUBLIC_ROAD");
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
topic_config {
chassis_topic: "/apollo/canbus/chassis"
hmi_status_topic: "/apollo/hmi/status"
localization_topic: "/apollo/localization/pose"
planning_pad_topic: "/apollo/planning/pad"
planning_trajectory_topic: "/apollo/planning"
prediction_topic: "/apollo/prediction"
relative_map_topic: "/apollo/relative_map"
routing_request_topic: "/apollo/routing_request"
routing_response_topic: "/apollo/routing_response"
story_telling_topic: "/apollo/storytelling"
traffic_light_detection_topic: "/apollo/perception/traffic_light"
}
standard_planning_config {
planner_type: PUBLIC_ROAD
planner_public_road_config {
}
}

1
2
3
4
5
6
7
8
9
10
struct LocalView {
std::shared_ptr<prediction::PredictionObstacles> prediction_obstacles;
std::shared_ptr<canbus::Chassis> chassis;
std::shared_ptr<localization::LocalizationEstimate> localization_estimate;
std::shared_ptr<perception::TrafficLightDetection> traffic_light;
std::shared_ptr<routing::RoutingResponse> routing;
std::shared_ptr<relative_map::MapMsg> relative_map;
std::shared_ptr<PadMessage> pad_msg;
std::shared_ptr<storytelling::Stories> stories;
};

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
Frame* frame,
ADCTrajectory* ptr_computed_trajectory) {
//确定当前的场景：因为Frame中包含了当前状态的所有信息，所以通过它就可以确定目前是处于哪一个场景下。
scenario_manager_.Update(planning_start_point, *frame);
//获取当前Scenario。
scenario_ = scenario_manager_.mutable_scenario();
//针对当前场景，进行处理
auto result = scenario_->Process(planning_start_point, frame);
if (FLAGS_enable_record_debug) {
auto scenario_debug = ptr_computed_trajectory->mutable_debug()
->mutable_planning_data()
->mutable_scenario();
scenario_debug->set_scenario_type(scenario_->scenario_type());
scenario_debug->set_stage_type(scenario_->GetStage());
scenario_debug->set_msg(scenario_->GetMsg());
}
if (result == scenario::Scenario::STATUS_DONE) {
// only updates scenario manager when previous scenario's status is
// STATUS_DONE
// 如果处理成功，则再次通过ScenarioManager更新。
scenario_manager_.Update(planning_start_point, *frame);
} else if (result == scenario::Scenario::STATUS_UNKNOWN) {
return Status(common::PLANNING_ERROR, "scenario returned unknown");
}
return Status::OK();
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
Status OnLanePlanning::InitFrame(const uint32_t sequence_num,
const TrajectoryPoint& planning_start_point,
const VehicleState& vehicle_state) {
frame_.reset(new Frame(sequence_num, local_view_, planning_start_point,
vehicle_state, reference_line_provider_.get()));
if (frame_ == nullptr) {
return Status(ErrorCode::PLANNING_ERROR, "Fail to init frame: nullptr.");
}
std::list<ReferenceLine> reference_lines;
std::list<hdmap::RouteSegments> segments;
if (!reference_line_provider_->GetReferenceLines(&reference_lines,
&segments)) {
const std::string msg = "Failed to create reference line";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
......

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
bool Frame::CreateReferenceLineInfo(
const std::list<ReferenceLine> &reference_lines,
const std::list<hdmap::RouteSegments> &segments) {
reference_line_info_.clear();
auto ref_line_iter = reference_lines.begin();
auto segments_iter = segments.begin();
while (ref_line_iter != reference_lines.end()) {
if (segments_iter->StopForDestination()) {
is_near_destination_ = true;
}
reference_line_info_.emplace_back(vehicle_state_, planning_start_point_,
*ref_line_iter, *segments_iter);
++ref_line_iter;
++segments_iter;
}
......

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
bool Stage::ExecuteTaskOnReferenceLine(
const common::TrajectoryPoint& planning_start_point, Frame* frame) {
for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
if (!reference_line_info.IsDrivable()) {
AERROR << "The generated path is not drivable";
return false;
}
for (auto* task : task_list_) {
const double start_timestamp = Clock::NowInSeconds();
const auto ret = task->Execute(frame, &reference_line_info);
const double end_timestamp = Clock::NowInSeconds();
const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
ADEBUG << "after task[" << task->Name()
<< "]: " << reference_line_info.PathSpeedDebugString();
ADEBUG << task->Name() << " time spend: " << time_diff_ms << " ms.";
RecordDebugInfo(&reference_line_info, task->Name(), time_diff_ms);
if (!ret.ok()) {
AERROR << "Failed to run tasks[" << task->Name()
<< "], Error message: " << ret.error_message();
break;
}
}
if (reference_line_info.speed_data().empty()) {
*reference_line_info.mutable_speed_data() =
SpeedProfileGenerator::GenerateFallbackSpeed(injector_->ego_info());
reference_line_info.AddCost(kSpeedOptimizationFallbackCost);
reference_line_info.set_trajectory_type(ADCTrajectory::SPEED_FALLBACK);
} else {
reference_line_info.set_trajectory_type(ADCTrajectory::NORMAL);
}
DiscretizedTrajectory trajectory;
if (!reference_line_info.CombinePathAndSpeedProfile(
planning_start_point.relative_time(),
planning_start_point.path_point().s(), &trajectory)) {
AERROR << "Fail to aggregate planning trajectory.";
return false;
}
reference_line_info.SetTrajectory(trajectory);
reference_line_info.SetDrivable(true);
return true;
}
return true;
}