| 71 | } |
| 72 | |
| 73 | bool SyncMeasure(MeasureGroup &measgroup) { |
| 74 | if (lidar_buffer.empty() || imu_buffer.empty()) { |
| 75 | /// Note: this will happen |
| 76 | return false; |
| 77 | } |
| 78 | |
| 79 | if (imu_buffer.front()->header.stamp.toSec() > |
| 80 | lidar_buffer.back()->header.stamp.toSec()) { |
| 81 | lidar_buffer.clear(); |
| 82 | ROS_ERROR("clear lidar buffer, only happen at the beginning"); |
| 83 | return false; |
| 84 | } |
| 85 | |
| 86 | if (imu_buffer.back()->header.stamp.toSec() < |
| 87 | lidar_buffer.front()->header.stamp.toSec()) { |
| 88 | return false; |
| 89 | } |
| 90 | |
| 91 | /// Add lidar data, and pop from buffer |
| 92 | measgroup.lidar = lidar_buffer.front(); |
| 93 | lidar_buffer.pop_front(); |
| 94 | double lidar_time = measgroup.lidar->header.stamp.toSec(); |
| 95 | |
| 96 | /// Add imu data, and pop from buffer |
| 97 | measgroup.imu.clear(); |
| 98 | int imu_cnt = 0; |
| 99 | for (const auto &imu : imu_buffer) { |
| 100 | double imu_time = imu->header.stamp.toSec(); |
| 101 | if (imu_time <= lidar_time) { |
| 102 | measgroup.imu.push_back(imu); |
| 103 | imu_cnt++; |
| 104 | } |
| 105 | } |
| 106 | for (int i = 0; i < imu_cnt; ++i) { |
| 107 | imu_buffer.pop_front(); |
| 108 | } |
| 109 | // ROS_DEBUG("add %d imu msg", imu_cnt); |
| 110 | |
| 111 | return true; |
| 112 | } |
| 113 | |
| 114 | void ProcessLoop(std::shared_ptr<ImuProcess> p_imu) { |
| 115 | ROS_INFO("Start ProcessLoop"); |
no test coverage detected