Apollo 2.0 Prediction源码

Posted on |

Prediction

prediction inputs are: obstacles from perception module nad localization from localization module. outputs are obstacles will predicted trajectories.

there are three classes in prediction modules:

* container
store input dat from subscribed channelds, e.g. perception obstacles, vehicle localization 
* evalutor
predicts paths and speed separately for any given obstacles
* predictor
generate predicted trajectories for obstacles. e.g. lane sequence(obstacle moves following the lanes),  free movement, regional moves(move in a possbile region)

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
// 模块入口
APOLLO_MAIN(apollo::prediction::Prediction);
Status Prediction::Init()
{
	predicition_conf_.Clear();
	adapter_conf_.Clear();
common::util::GetProtoFromFile(FLAGS_prediction_adapter_config_filename, &adapter_conf_);
	
	//Initial managers
	AdapterManager::Init(adapter_conf_);
	ContainerManager::instance()->Init(adapter_conf_);
	EvaluatorManager::instance()->Init(prediction_conf_);
	PredictorManager::instance()->Init(prediction_conf_);
	
	AdapterManager::GetLocalization();
	AdapterManager::GetPerceptionObstacles(); 	
			AdapterManger::AddPerceptionObstaclesCallback(&Prediction::RunOnce, this);
	AdapterManger::AddLocalizationCallback(&Prediction::OnLocalization, this);
AdapterManger::AddPlanningCallback(&Prediction::OnPlanning, this);
	
return Status::OK();
}
void Prediction::RunOnce(const PerceptionObstacles& perception_obstacles)
{
	ObstaclesContainer* obstacles_container = dynamic_cast<ObstaclesContainer*>(ContainerManager::instance()->GetContainer(AdapterConfig::PERCEPTION_OBSTACLES));
	
	obstacles_container->Insert(perception_obstacles);
	EvaluatorManager::instace()->Run(perception_obstacles);
	PredictorManager::instance()->Run(perception_obstacles);
	
	auto prediction_obstacles = PredictorManager::instance()->prediction_obstacles();
	for(auto const& prediction_obstacle : prediction_obstacles.prediction_obstalces()){
	for(auto const& trajectory:prediction_obstacle.trajectory())
	{
		for(auto const& trajectory_point : trajectory.trajectory_point())
		{
			if(!IsValidTrajectoryPoint(trajectory_point)){
				return ;
			}
		}
	} 
	
	Publish(&prediction_obstacles);
	
}
void Prediction::OnLocalization(const LocalizationEstimate& localization, ObstaclesContainer* obstacles_container)
void Prediction::OnPlanning(const planning::ADCTrajectory& adc_trajectory, ADCTrajectoryContainer* adc_trajectory_container)

Prediction interface has defined: RunOnce() and Pulish(). the Prediction class has defined listener’s callback: OnLocalization, OnPlanning, and Start(), Stop() and implemented interface functions. For both EvaluatorManager and PredictorManager, there are Init() and Run() functions, and there are bunch of apis from container class.

Evaluator

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
void EvaluatorManager::Init(const PredictionConf& config)
{
	//...
	switch(config.obstacle_type()){
		case PerceptionObstacle::VEHICLE :{
			vehicle_on_lane_evaluator_ = obstalce_conf.evaluator_type();
			break; 
		
		case PerceptionObstacle::BICYCLE:{
			cyclist_on_lane_evaluator_ = obstlce_conf.evluator_type();
			break;
			
		case PerceptionObstacle::PEDESTRAIN:{
			break;
		}
		case PerceptionObstacle::UNKNOWN:{
			default_on_lane_evaluator_ = obstacle_conf.evalutor_type();
			break;
		}
	}
}
Evaluator* EvaluatorManager::Run(const perception::PerceptionObstacles& perception_obstacles)
{
	ObstaclesContainer* container = dynamic_cast<ObstaclesContainer*>();
	
	Evaluator* evaluator = nullptr ;
	
	for(const auto& perception_obstacle : perception_obstacles.perception_obstalce())
	{
		int id = perception_obstalce.id(); 
		Obstacle* obstacle = container->GetObstalce(id);
		switch(perception_obstalce.type())
		{
			case PerceptionObstacle::VEHICLE: {
				if(obstacle->IsOnLane()){
					evaluator = GetEvaluator(vehicle_on_lane_evaluator_); 
				}
				break;
			}
			
			case PerceptionObstacle::BICYCLE:{
				if(obstacle->IsOnLane()){
					evaluator = GetEvaluator(cyclist_on_lane_evaluator_);
				}
				break;
			}
	
			// ...
			
			if(evaluator != nullptr)
			{
				evaluator->Evaluate(obstacle);
			}
		}	
	}
}

and there are a few different evaluators: (multilayer perception approach) MLP and RNN(deep neural network), both will discuss in details in future.

Predictor

predictor will generate trajectories, a few apis:

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
virtual void predictor::Predict(Obstacle* obstacle) = 0;
void predictor::TrimTrajectories(const Obstacle, const ADCTrajectoryContainer* );
static predictor::Trajectory GenerateTrajectory(const std::vector<apollo::common::TrajectoryPoint>& points) ;
void PredictorManager::Init(const PredictionConf& config)
{
	// ...
	switch(obstacle_conf.obstacle_type())
	{
		case PerceptionObstacle::VEHICLE: {
			if(obstacle_conf.obstacle_status() == ObstacleConfg::ON_LANE){
				vehicle_on_lane_predictor_ = obstacle_conf.predictor_type();
			}else if(obstacle_conf.obstacle_status() == ObstacleConf::OFF_LANE){
			vehicle_off_lane_predictor_ = obstacle_conf.predictor_type();
			}
		}
		break; 
		
		// ...
		
 }
 
 void PredictorManager::Run(const PerceptionObstacles& perception_obstacles){
 	ObstaclesContainer* obstacles_container =  dynamic_cast<ObstaclesContainer*>(AdapterConfig::PERCEPTION_OBSTACLES) ;
 	ADCTrajectoryContainer *adc_trajectory_container = dynamic_cast<ADCTrajectoryContainer*>(AdapterConfig::PLANNING_TRAJECTORY);
 	
 	Predictor* predictor = nullptr ; 
 	for(const auto* perception_obstacle : perception_obstacles.perception_obstacle())
 	{
 		int id = perception_obstacle.id();
 		PredictionObstacle prediction_obstacle ;
 		Obstacle* obstacle = obstacle_container->GetObstacle(id);
 		
 		if(obstacle != nullptr)
 		{
 			switch(perception_obstacle.type())
 			{
 				case PerceptionObstacle::VEHICLE: {
 					if(obstacle->IsOnLane()){
 						predictor = GetPredictor(vehicle_on_lane_predictor_);
 						}else{
 							predictor = GetPredictor(vehicle_off_lane_predictor_);
 						}
 						break;
 					}
 					
 					// ...
 			 }
 			 
 			 if(predictor != nullptr)
 			 {
 			 	predictor->Predict(obstacle);
 			 	if(obstacle->type() == PerceptionObstacle::VEHICLE){
 			 		predictor->TrimTrajectories(obstacle, adc_trajectory_container);
 			 	}
 			 	
 			 	for(const auto& trajectory : predictor->trajectories())
 			 	{
 			 		prediction_obstacle.add_trajectory()->CopyFrom(trajectory);
 			 	}
 			 }
 			 
 		}
 			
 		prediction_obstacle.mutable_perception_obstacle()->CopyFrom(perception_obstacle);
 		
 		prediction_obstacles_.add_prediction_obstacle()->CopyFrom(prediction_obstacle); 
 	}

the predictor has a few types:: regional based, free move, lane sequence e.t.c, which defined as subclasses, will discuss more in future.

Container

container manager class has a few subclass as adc_trajctory container, obstacles contianer and pose constainer, which should be discussed in details later.

Adapter

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
// in adapter_manager.cc 
case AdapterConfig::LOCALIZATION:
	EnableLocalization(FLAGS_localization_topic, config);
case AdatperConfig::PERCEPTION_OBSTACLES:
	EnablePerceptionObstacles(FLAGS_perception_obstacle_topic, config);
	
// in messsage_adapters.h 
using PerceptionObstaclesAdapter = Adapter<perception::PerceptionObstacles>; 
using LocalizationAdapter = Adapter<apollo::localization::LocalizationEstimate>;
// in adapter_manager.h 
static voi Enable##name(){
	instance()->InternalEnable##name(topic_name, config);
}; 
static name##Adapter *Get##name(){
	return instance()->InternaleGet##name();
}
static void Add##name##Callback(name##Adapter::Callback callback){
	instance()->name##_->AddCallback(callback);
}
template<class T> staic void Add##name##Callback(void(T:: *fp)(const name##Adapter::DataType &data), T* obj)
{
	Add##name##Callback(std::bind(fp, obj, std::placeholders::_1));
}