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

fix(ros): fix imu publish bug

Browse Source
This commit is contained in:
kalman 2019-04-01 19:54:50 +08:00
parent bdfe88ef32
commit 06e8e28b71
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -712,10 +712,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
if (data.imu) { if (data.imu) {
if (data.imu->flag == 1) { // accelerometer if (data.imu->flag == 1) { // accelerometer
imu_accel_ = data.imu; imu_accel_ = data.imu;
publishImuBySync(stamp); publishImuBySync();
} else if (data.imu->flag == 2) { // gyroscope } else if (data.imu->flag == 2) { // gyroscope
imu_gyro_ = data.imu; imu_gyro_ = data.imu;
publishImuBySync(stamp); publishImuBySync();
} else { } else {
publishImu(data, imu_count_, stamp); publishImu(data, imu_count_, stamp);
publishTemperature(data.imu->temperature, imu_count_, stamp); publishTemperature(data.imu->temperature, imu_count_, stamp);
@ -980,14 +980,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
} }
void publishImuBySync(ros::Time stamp) { void publishImuBySync() {
timestampAlign(); timestampAlign();
if (imu_accel_ == nullptr || imu_gyro_ == nullptr) { if (imu_accel_ == nullptr || imu_gyro_ == nullptr) {
return; return;
} }
sensor_msgs::Imu msg; sensor_msgs::Imu msg;
ros::Time stamp = checkUpImuTimeStamp(imu_accel_->timestamp);
msg.header.seq = imu_sync_count_; msg.header.seq = imu_sync_count_;
msg.header.stamp = stamp; msg.header.stamp = stamp;
msg.header.frame_id = imu_frame_id_; msg.header.frame_id = imu_frame_id_;