| 174 | } |
| 175 | |
| 176 | bool RosbagDataProvider::spin() { |
| 177 | Timestamp timestamp_last_frame = rosbag_data_.timestamps_.at(0); |
| 178 | |
| 179 | // Stereo matching parameters |
| 180 | const StereoMatchingParams& stereo_matching_params = |
| 181 | frontend_params_.getStereoMatchingParams(); |
| 182 | |
| 183 | for (size_t k = 0; k < rosbag_data_.getNumberOfImages(); k++) { |
| 184 | if (nh_.ok() && ros::ok() && !ros::isShuttingDown()) { |
| 185 | // Main spin of the data provider: Interpolates IMU data |
| 186 | // and builds StereoImuSyncPacket |
| 187 | // (Think of this as the spin of the other parser/data-providers) |
| 188 | const Timestamp& timestamp_frame_k = rosbag_data_.timestamps_.at(k); |
| 189 | if (timestamp_frame_k > timestamp_last_frame) { |
| 190 | ImuMeasurements imu_meas; |
| 191 | utils::ThreadsafeImuBuffer::QueryResult imu_query = |
| 192 | imu_data_.imu_buffer_.getImuDataInterpolatedUpperBorder( |
| 193 | timestamp_last_frame, |
| 194 | timestamp_frame_k, |
| 195 | &imu_meas.timestamps_, |
| 196 | &imu_meas.measurements_); |
| 197 | if (imu_query == |
| 198 | utils::ThreadsafeImuBuffer::QueryResult::kDataAvailable) { |
| 199 | // Call VIO Pipeline. |
| 200 | VLOG(10) << "Call VIO processing for frame k: " << k |
| 201 | << " with timestamp: " << timestamp_frame_k << '\n' |
| 202 | << "////////////////////////////////// Creating packet!\n" |
| 203 | << "STAMPS IMU rows : \n" |
| 204 | << imu_meas.timestamps_.rows() << '\n' |
| 205 | << "STAMPS IMU cols : \n" |
| 206 | << imu_meas.timestamps_.cols() << '\n' |
| 207 | << "STAMPS IMU: \n" |
| 208 | << imu_meas.timestamps_ << '\n' |
| 209 | << "ACCGYR IMU rows : \n" |
| 210 | << imu_meas.measurements_.rows() << '\n' |
| 211 | << "ACCGYR IMU cols : \n" |
| 212 | << imu_meas.measurements_.cols() << '\n' |
| 213 | << "ACCGYR IMU: \n" |
| 214 | << imu_meas.measurements_ << '\n'; |
| 215 | |
| 216 | timestamp_last_frame = timestamp_frame_k; |
| 217 | |
| 218 | // Publish Output |
| 219 | vio_callback_(StereoImuSyncPacket( |
| 220 | StereoFrame(k, timestamp_frame_k, |
| 221 | readRosImage(rosbag_data_.left_imgs_.at(k)), |
| 222 | stereo_calib_.left_camera_info_, |
| 223 | readRosImage(rosbag_data_.right_imgs_.at(k)), |
| 224 | stereo_calib_.right_camera_info_, |
| 225 | stereo_calib_.camL_Pose_camR_, |
| 226 | stereo_matching_params), |
| 227 | imu_meas.timestamps_, imu_meas.measurements_)); |
| 228 | // Publish ground-truth data if available |
| 229 | if (rosbag_data_.gt_odometry_.size() > k) { |
| 230 | publishGroundTruthOdometry(rosbag_data_.gt_odometry_.at(k)); |
| 231 | } |
| 232 | VLOG(10) << "Finished VIO processing for frame k = " << k; |
| 233 | } else { |
nothing calls this directly
no test coverage detected