Apollo 11.0
自动驾驶开放平台
Public 类型 | Public 成员函数 | 所有成员列表
apollo::localization::msf::LocalizationIntegProcess类 参考

process Imu msg for localization 更多...

#include <localization_integ_process.h>

apollo::localization::msf::LocalizationIntegProcess 的协作图:

Public 类型

typedef Eigen::Affine3d TransformD
 

Public 成员函数

 LocalizationIntegProcess ()
 
 ~LocalizationIntegProcess ()
 
apollo::common::Status Init (const LocalizationIntegParam &params)
 
void RawImuProcess (const ImuData &imu_msg)
 
void GetState (IntegState *state)
 
void GetResult (IntegState *state, LocalizationEstimate *localization)
 
void GetResult (IntegState *state, InsPva *sins_pva, double pva_covariance[9][9])
 
void GetCorrectedImu (ImuData *imu_data)
 
void GetEarthParameter (InertialParameter *earth_param)
 
void MeasureDataProcess (const MeasureData &measure_msg)
 

详细描述

process Imu msg for localization

在文件 localization_integ_process.h 第 47 行定义.

成员类型定义说明

◆ TransformD

typedef Eigen::Affine3d apollo::localization::msf::LocalizationIntegProcess::TransformD

在文件 localization_integ_process.h 第 49 行定义.

构造及析构函数说明

◆ LocalizationIntegProcess()

apollo::localization::msf::LocalizationIntegProcess::LocalizationIntegProcess ( )

在文件 localization_integ_process.cc 第 33 行定义.

34 : sins_(new Sins()),
35 gnss_antenna_extrinsic_(TransformD::Identity()),
36 integ_state_(IntegState::NOT_INIT),
37 ins_pva_(),
38 pva_covariance_{0.0},
39 corrected_imu_(),
40 earth_param_(),
41 keep_running_(false),
42 measure_data_queue_size_(150),
43 delay_output_counter_(0) {}

◆ ~LocalizationIntegProcess()

apollo::localization::msf::LocalizationIntegProcess::~LocalizationIntegProcess ( )

在文件 localization_integ_process.cc 第 45 行定义.

45 {
46 StopThreadLoop();
47
48 delete sins_;
49 sins_ = nullptr;
50}

成员函数说明

◆ GetCorrectedImu()

void apollo::localization::msf::LocalizationIntegProcess::GetCorrectedImu ( ImuData *  imu_data)

在文件 localization_integ_process.cc 第 261 行定义.

261 {
262 CHECK_NOTNULL(imu_data);
263
264 *imu_data = corrected_imu_;
265}

◆ GetEarthParameter()

void apollo::localization::msf::LocalizationIntegProcess::GetEarthParameter ( InertialParameter *  earth_param)

在文件 localization_integ_process.cc 第 267 行定义.

268 {
269 CHECK_NOTNULL(earth_param);
270
271 *earth_param = earth_param_;
272}

◆ GetResult() [1/2]

void apollo::localization::msf::LocalizationIntegProcess::GetResult ( IntegState *  state,
InsPva *  sins_pva,
double  pva_covariance[9][9] 
)

在文件 localization_integ_process.cc 第 250 行定义.

251 {
252 CHECK_NOTNULL(state);
253 CHECK_NOTNULL(sins_pva);
254 CHECK_NOTNULL(pva_covariance);
255
256 *state = integ_state_;
257 *sins_pva = ins_pva_;
258 memcpy(pva_covariance, pva_covariance_, sizeof(double) * 9 * 9);
259}

◆ GetResult() [2/2]

void apollo::localization::msf::LocalizationIntegProcess::GetResult ( IntegState *  state,
LocalizationEstimate *  localization 
)

在文件 localization_integ_process.cc 第 172 行定义.

173 {
174 CHECK_NOTNULL(state);
175 CHECK_NOTNULL(localization);
176
177 // state
178 *state = integ_state_;
179
180 if (*state != IntegState::NOT_INIT) {
181 ADEBUG << std::setprecision(16)
182 << "IntegratedLocalization Debug Log: integ_pose msg: "
183 << "[time:" << ins_pva_.time << "]"
184 << "[x:" << ins_pva_.pos.longitude * 57.295779513082323 << "]"
185 << "[y:" << ins_pva_.pos.latitude * 57.295779513082323 << "]"
186 << "[z:" << ins_pva_.pos.height << "]"
187 << "[ve:" << ins_pva_.vel.ve << "]"
188 << "[vn:" << ins_pva_.vel.vn << "]"
189 << "[vu:" << ins_pva_.vel.vu << "]"
190 << "[pitch: " << ins_pva_.att.pitch * 57.295779513082323 << "]"
191 << "[roll:" << ins_pva_.att.roll * 57.295779513082323 << "]"
192 << "[yaw:" << ins_pva_.att.yaw * 57.295779513082323 << "]";
193 }
194
195 // LocalizationEstimation
196 apollo::common::Header *headerpb_loc = localization->mutable_header();
197 apollo::localization::Pose *posepb_loc = localization->mutable_pose();
198
199 localization->set_measurement_time(ins_pva_.time);
200 headerpb_loc->set_timestamp_sec(apollo::cyber::Clock::NowInSeconds());
201
202 apollo::common::PointENU *position_loc = posepb_loc->mutable_position();
203 apollo::common::Quaternion *quaternion = posepb_loc->mutable_orientation();
204 UTMCoor utm_xy;
205 FrameTransform::LatlonToUtmXY(ins_pva_.pos.longitude, ins_pva_.pos.latitude,
206 &utm_xy);
207 position_loc->set_x(utm_xy.x);
208 position_loc->set_y(utm_xy.y);
209 position_loc->set_z(ins_pva_.pos.height);
210
211 quaternion->set_qx(ins_pva_.qbn[1]);
212 quaternion->set_qy(ins_pva_.qbn[2]);
213 quaternion->set_qz(ins_pva_.qbn[3]);
214 quaternion->set_qw(ins_pva_.qbn[0]);
215
216 apollo::common::Point3D *velocitylinear =
217 posepb_loc->mutable_linear_velocity();
218 velocitylinear->set_x(ins_pva_.vel.ve);
219 velocitylinear->set_y(ins_pva_.vel.vn);
220 velocitylinear->set_z(ins_pva_.vel.vu);
221
222 apollo::common::Point3D *eulerangles = posepb_loc->mutable_euler_angles();
223 eulerangles->set_x(ins_pva_.att.pitch);
224 eulerangles->set_y(ins_pva_.att.roll);
225 eulerangles->set_z(ins_pva_.att.yaw);
226
227 posepb_loc->set_heading(ins_pva_.att.yaw);
228
229 apollo::localization::Uncertainty *uncertainty =
230 localization->mutable_uncertainty();
231 apollo::common::Point3D *position_std_dev =
232 uncertainty->mutable_position_std_dev();
233 position_std_dev->set_x(std::sqrt(pva_covariance_[0][0]));
234 position_std_dev->set_y(std::sqrt(pva_covariance_[1][1]));
235 position_std_dev->set_z(std::sqrt(pva_covariance_[2][2]));
236
237 apollo::common::Point3D *linear_velocity_std_dev =
238 uncertainty->mutable_linear_velocity_std_dev();
239 linear_velocity_std_dev->set_x(std::sqrt(pva_covariance_[3][3]));
240 linear_velocity_std_dev->set_y(std::sqrt(pva_covariance_[4][4]));
241 linear_velocity_std_dev->set_z(std::sqrt(pva_covariance_[5][5]));
242
243 apollo::common::Point3D *orientation_std_dev =
244 uncertainty->mutable_orientation_std_dev();
245 orientation_std_dev->set_x(std::sqrt(pva_covariance_[6][6]));
246 orientation_std_dev->set_y(std::sqrt(pva_covariance_[7][7]));
247 orientation_std_dev->set_z(std::sqrt(pva_covariance_[8][8]));
248}
apollo::cyber::Clock::NowInSeconds
static double NowInSeconds()
gets the current time in second.
Definition clock.cc:56
apollo::localization::msf::FrameTransform::LatlonToUtmXY
static bool LatlonToUtmXY(double lon, double lat, UTMCoor *utm_xy)
Definition frame_transform.cc:27
ADEBUG
#define ADEBUG
Definition log.h:41

◆ GetState()

void apollo::localization::msf::LocalizationIntegProcess::GetState ( IntegState *  state)

在文件 localization_integ_process.cc 第 166 行定义.

166 {
167 CHECK_NOTNULL(state);
168
169 *state = integ_state_;
170}

◆ Init()

Status apollo::localization::msf::LocalizationIntegProcess::Init ( const LocalizationIntegParam &  params)

在文件 localization_integ_process.cc 第 52 行定义.

52 {
53 // sins init
54 sins_->Init(param.is_ins_can_self_align);
55 sins_->SetSinsAlignFromVel(param.is_sins_align_with_vel);
56
57 sins_->SetResetSinsPoseStd(param.sins_state_pos_std);
58 sins_->SetResetSinsMeasSpanTime(param.sins_state_span_time);
59
60 if (param.is_using_raw_gnsspos) {
61 gnss_antenna_extrinsic_.translation()(0) = param.imu_to_ant_offset.offset_x;
62 gnss_antenna_extrinsic_.translation()(1) = param.imu_to_ant_offset.offset_y;
63 gnss_antenna_extrinsic_.translation()(2) = param.imu_to_ant_offset.offset_z;
64 } else {
65 gnss_antenna_extrinsic_ = TransformD::Identity();
66 }
67 AINFO << "gnss and imu lever arm: "
68 << gnss_antenna_extrinsic_.translation()(0) << " "
69 << gnss_antenna_extrinsic_.translation()(1) << " "
70 << gnss_antenna_extrinsic_.translation()(2);
71
72 sins_->SetImuAntennaLeverArm(gnss_antenna_extrinsic_.translation()(0),
73 gnss_antenna_extrinsic_.translation()(1),
74 gnss_antenna_extrinsic_.translation()(2));
75
76 sins_->SetVelThresholdGetYaw(param.vel_threshold_get_yaw);
77
78 StartThreadLoop();
79
80 return Status::OK();
81}
apollo::common::Status::OK
static Status OK()
generate a success status.
Definition status.h:60
AINFO
#define AINFO
Definition log.h:42

◆ MeasureDataProcess()

void apollo::localization::msf::LocalizationIntegProcess::MeasureDataProcess ( const MeasureData &  measure_msg)

在文件 localization_integ_process.cc 第 274 行定义.

275 {
276 measure_data_queue_mutex_.lock();
277 measure_data_queue_.push(measure_msg);
278 measure_data_queue_mutex_.unlock();
279}

◆ RawImuProcess()

void apollo::localization::msf::LocalizationIntegProcess::RawImuProcess ( const ImuData &  imu_msg)

在文件 localization_integ_process.cc 第 83 行定义.

83 {
84 integ_state_ = IntegState::NOT_INIT;
85 double cur_imu_time = imu_msg.measurement_time;
86
87 if (cur_imu_time < 3000) {
88 AERROR << "the imu time is error: " << cur_imu_time;
89 return;
90 }
91
92 static double pre_imu_time = cur_imu_time;
93 double delta_time = cur_imu_time - pre_imu_time;
94 if (delta_time > 0.1) {
95 AERROR << std::setprecision(16) << "the imu message loss more than 10, "
96 << "the pre time and current time: " << pre_imu_time << " "
97 << cur_imu_time;
98 } else if (delta_time < 0.0) {
99 AERROR << std::setprecision(16)
100 << "received imu message's time is eary than last imu message, "
101 << "the pre time and current time: " << pre_imu_time << " "
102 << cur_imu_time;
103 }
104
105 double cur_system_time = Clock::NowInSeconds();
106 static double pre_system_time = cur_system_time;
107
108 double delta_system_time = cur_system_time - pre_system_time;
109 if (delta_system_time > 0.1) {
110 AERROR << std::setprecision(16)
111 << "the imu message loss more than 10 according to system time, "
112 << "the pre system time and current system time: " << pre_system_time
113 << " " << cur_system_time;
114 } else if (delta_system_time < 0.0) {
115 AERROR << std::setprecision(16)
116 << "received imu message's time is eary than last imu message "
117 "according to system time, "
118 << "the pre system time and current system time: " << pre_system_time
119 << " " << cur_system_time;
120 }
121
122 // add imu msg and get current predict pose
123 sins_->AddImu(imu_msg);
124 sins_->GetPose(&ins_pva_, pva_covariance_);
125 sins_->GetRemoveBiasImu(&corrected_imu_);
126 sins_->GetEarthParameter(&earth_param_);
127
128 if (sins_->IsSinsAligned()) {
129 integ_state_ = IntegState::NOT_STABLE;
130 if (delay_output_counter_ < 3000) {
131 ++delay_output_counter_;
132 } else {
133 integ_state_ = IntegState::OK;
134 GetValidFromOK();
135 }
136
137 if (cur_imu_time - 0.5 > pre_imu_time) {
138 AINFO << "SINS has completed alignment!" << std::endl;
139 pre_imu_time = cur_imu_time;
140 }
141 } else {
142 delay_output_counter_ = 0;
143 if (cur_imu_time - 0.5 > pre_imu_time) {
144 AINFO << "SINS is aligning!" << std::endl;
145 pre_imu_time = cur_imu_time;
146 }
147 }
148
149 pre_imu_time = cur_imu_time;
150 pre_system_time = cur_system_time;
151}
AERROR
#define AERROR
Definition log.h:44
该类的文档由以下文件生成: