tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix(ros): Align imu time stamp

Browse Source
This commit is contained in:
kalman 2019-01-25 17:33:12 +08:00
parent 0b71d05813
commit aa089a5859
1 changed files with 68 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -783,7 +783,75 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
pub_imu_.publish(msg);
}
void timestampAlign() {
static bool get_first_acc = false;
static bool get_first_gyro = false;
static bool has_gyro = false;
static ImuData acc;
static ImuData gyro;
if (!get_first_acc && imu_accel_ != nullptr) {
imu_accel_->flag = 0;
acc = *imu_accel_;
imu_accel_ = nullptr;
get_first_acc = true;
return;
}
if (!get_first_gyro && imu_gyro_ != nullptr) {
imu_gyro_->flag = 0;
gyro = *imu_gyro_;
imu_gyro_ = nullptr;
get_first_gyro = true;
return;
}
if (imu_accel_ != nullptr) {
if (!has_gyro) {
imu_accel_->flag = 0;
acc = *imu_accel_;
imu_accel_ = nullptr;
return;
}
if (acc.timestamp <= gyro.timestamp) {
ImuData acc_temp;
acc_temp = *imu_accel_;
acc_temp.timestamp = gyro.timestamp;
double k = static_cast<double>(imu_accel_->timestamp - acc.timestamp);
k = static_cast<double>(gyro.timestamp - acc.timestamp) / k;
acc_temp.accel[0] = acc.accel[0] +
(imu_accel_->accel[0] - acc.accel[0]) * k;
acc_temp.accel[1] = acc.accel[1] +
(imu_accel_->accel[1] - acc.accel[1]) * k;
acc_temp.accel[2] = acc.accel[2] +
(imu_accel_->accel[2] - acc.accel[2]) * k;
acc = *imu_accel_;
*imu_accel_ = acc_temp;
imu_gyro_.reset(new ImuData(gyro));
has_gyro = false;
return;
} else {
acc = *imu_accel_;
imu_accel_ = nullptr;
return;
}
}
if (imu_gyro_ != nullptr) {
has_gyro = true;
gyro = *imu_gyro_;
imu_gyro_ = nullptr;
return;
}
}
void publishImuBySync(ros::Time stamp) {
timestampAlign();
if (imu_accel_ == nullptr || imu_gyro_ == nullptr) {
return;
}