Apollo2.0 Control 源码 (3)

Posted on |

the input of Apollo control module includes: chassis info, localization info, and planning info ,the output is steering angle, acc, throttle.

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
// 模块入口
#define APOLLO_MAIN(APP)                                       
  int main(int argc, char **argv) {                            
    google::InitGoogleLogging(argv[0]);                        
    google::ParseCommandLineFlags(&argc, &argv, true);         
    signal(SIGINT, apollo::common::apollo_app_sigint_handler); 
    APP apollo_app_;                                           
    ros::init(argc, argv, apollo_app_.Name());                 
    apollo_app_.Spin();    //check previous blog(1)                                  
    return 0;                                                  
  }
	
Status Control::Init(){
	init_time_ = Clock::NowInSeconds(); 
	common::util::GetProtoFromFile(FLAGS_control_conf_file, &control_conf_);
	AdapterManager::Init(FLAGS_control_adapter_config_filename);
	
	common::monitor::MonitorLogBuffer buffer(&monitor_logger_);
	
	controller_agent_.Init(&control_conf_); 
	
	AdapterManager::GetLocalization(); 
	AdapterManager::GetChassis();
	AdapterManager::GetPlanning(); 
	AdpaterManager::GetControlCommand(); 	AdapterManager::GetMonitor();
	
	AdapterManager::AddMonitorCallback(&Control::OnMonitor, this);
	
	return Status::OK();
}

conf_file: /modules/control/conf/lincoln.pd/txt,

which is derieved from

/modules/calibration/data/mkz8/calibration_table.pd.txt

the topics in control modules are :

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
125
config {
	type: LOCALIZATION
	mode: RECEIVE_ONLY
}
config {
	type: PLANNING_TRAJECTORY
	mode: RECEIVE_ONLY
}
config {
	type: CHASSIS
	mode: RECEIVE_ONLY
}
config {
	type: CONTROL_COMMAND
	mode: PUBLISH_ONLY
}
config {
	type: MONITOR
	mode: DUPLEX
} 
``` 
basically, control module will receive topic about /localization,  /planning,  /chassis, and publish /control_command 
the controller_agent is an interface class, so can support user defined controller algorithms, which only need to configure through  `control_conf` 
```c
Status ControllerAgent::Init(const ControlConf* control_conf)
{
	RegisterControllers(control_conf); 
	InitializeConf(control_conf); 
	
	for(auto &controller : controller_list_)
	{
		if(controller == NULL || !controller->Init(control_conf_).ok())
		{
			return Status(ErrorCode);
		}
	}
	
	return Status::OK();
}
void ControllerAgent::RegisterControllers(const ControlConf *control_conf)
{
	for(auto active_controller : control_conf->active_controllers())
	{
		switch(active_controller){
			
			case ControlConf::MPC_CONTROLLER:
				controller_factory_.Register(
					ControlConf::MPC_CONTROLLER, 
					[]()->Controller * {return new MPCController();});
			break;
			
			//case LAT_CONTROLLER 
			
			//case LON_CONTROLLER 
			
		   }
		}
 	}
 
 
 Status Control::Start(){
 
 	//sleep for advertised channel to ready
 	
 	std::this_thread::sleep_for(std::chrono::millisecons(1000)); 	
 	timer_ = AdapterManager::CreateTimer(ros::Duration(control_conf_.control_period()), 
 	&Control::OnTimer, this);
 	
 	common::monitor::MonitorLogBuffer buffer(&monitor_logger_);
 	
 	return Status::OK();
 }
 
 
 void Control::OnTimer(const ros::TimerEvent &)
 {
 	double start_timestamp = Clock::NowInSeconds();
 	
 	ControlCommand control_command ;
 	Status status = ProduceControlCommand(&control_command);
 	
 	double end_timestamp = Clock::NowInSeconds(); 
 	
 	status.Save(control_command.mutable_header()->mutable_status());
 	
 	SendCmd(&control_command);
 }
 
 
  Status Control::ProduceControlCommand(ControlCommand *control_command)
  {
  	Status status = CheckInput();
  	Status status_ts = CheckTimestamp();
 
 	Status status_compute = controller_agent_.ComputeControlCommand(
 		&localization_, &chassis_, &trajectory_, control_command);
 	
 	return status;
  }
 		
 
 
 Status ControllerAgent::ComputeControlCommand(
 	const localization::LocalizationEstimate *localization,
 	cosnt canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
 	control::ControlCommand *cmd){
 	for(auto &controller : controller_list_)
 	{
 		controller->ComputeControlCommand(localization, chassis, trajectory, cmd) ;
 	}
 	return status::OK();
 }

ControllerAgent::ComputeCmd() is the interface, the real control cmd will be computed inside each specified controller modes. there are few controller modes: MPC, Lattice e.g in Apollo.

in next few days continue work on localization, prediction, and decision modules in Apollo 2.0