Apollo 2.0 Localization 源码

Posted on |

there are two localization methods: RTK and multi-sensor fusion(MSF). RTK using GPS and IMU inputs, MSF using GPS, IMU and Lidar sensor and HD map as inputs. the output is the localization estimated object instance.

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
Status Localization::Start()
{
	localization_ = localization_factory_.CreateObject(cofig_.type());
	localization_->Start();
	return Status::OK();
}
Status RTKLocalization::Start(){
	AdapterManager::Init(FLAGS_rtk_adapter_config_file);
	timer_ = AdapterManager::CreateTimer(ros::Duration(duration), &RTKLocalization::OnTimer, this); 
	AdapterManager::GetGps();
	AdapterManager::GetImu();	
	tf2_broadcaster_ = new tf2_ros::TransformBroadcaster() ;
	return Status::OK();
}
void RTKLocalization::OnTimer(const ros::TimerEvent &event)
{
	AdapterManager::Observe();
	PublishLocalization(); 
	RunWatchDog();	
}
void RTKLocalization::PublishLocalization(){
	LocalizationEstimate localization ;
	PrepareLocalizationMsg(&localization);
	AdapterManager::PublishLocalization(localization);
	PublishPoseBroadcastTF(localization);
}
void RTKLocalization::PrepareLocalizationMsg(LocalizationEstimate *localization){
	const auto &gps_msg = AdapterManager::GetGps()->GetLatestObserved();
	Imu imu_msg = AdapterManager::GetImu()->GetLatestObserved();
	ComposeLocalizationMsg(gps_msg, imu_msg, localization);
}
void RTKLocalization::ComposeLocalizationMsg(const localization::Gps& gps_msg, const localization::Imu &img_msg, LocalizationEstimate* localization)
{
	// add header 
	// set measurement time 
	// combine gps and imu 
	
	auto mutable_pose = localization->mutable_pose(); 
	
	if(gps_msg.has_localization()){
		const auto &pose = gps_msg.localization();
		if(pose.has_position()){ // update mutable_pose };
		if(pose.has_orientation()) { //update mutable orientation };
		if(pose.has_linear_velocity()){};
	}
	
	if(imu.has_linear_acceleration()){	//update mutable_pose acc };
	if(imu.has_angular_velocity()){};
	if(imu.has_euler_angles()){};
}

MSF

vehicle localization based on multi-sensor fusion

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
class MSFLocalization {	
	void InitParams();
	void OnPointCloud(const sensor_msgs::PointCloud2 &message);
	void OnRawImu(const drivers::gnss::Imu &imu_msg);
	void OnGnssRtkObs(const EpochObservation &raw_obs_msg);
	// ...
	void PublishPoseBroadcastTF(const LocalizationEstimate& localization);
}
Status MSFLocalization::Start()
{
	AdapterManager::Init(FLAGS_msf_adapter_config_file);
	Status &&status = Init();
	
	AdapterManager::GetRawImu();
	AdapterManager::ADDRawImuCallback(&MSFLocalization::OnRawImu, this);
	
	AdapterManager::GetPointCloud();
	AdapterManager::AddPointCloudCallback(&MSFLocalization::OnPointCloud, this);
	
	// ...
	
	return Status::OK();
}

there is addtional local_map module, which will be review later.