| 153 | } |
| 154 | |
| 155 | bool RosDataProvider::spin() { |
| 156 | // ros::Rate rate(60); |
| 157 | while (ros::ok()) { |
| 158 | // Main spin of the data provider: Interpolates IMU data and build |
| 159 | // StereoImuSyncPacket (Think of this as the spin of the other |
| 160 | // parser/data-providers) |
| 161 | const Timestamp ×tamp = stereo_buffer_.getEarliestTimestamp(); |
| 162 | if (timestamp <= last_timestamp_) { |
| 163 | if (stereo_buffer_.size() != 0) { |
| 164 | ROS_WARN( |
| 165 | "Next frame in image buffer is from the same or " |
| 166 | "earlier time than the last processed frame. Skip " |
| 167 | "frame."); |
| 168 | // remove next frame (this would usually happen for the first |
| 169 | // frame) |
| 170 | stereo_buffer_.removeNext(); |
| 171 | } |
| 172 | // else just waiting for next stereo frames |
| 173 | } else { |
| 174 | // Test if IMU data available |
| 175 | ImuMeasurements imu_meas; |
| 176 | |
| 177 | utils::ThreadsafeImuBuffer::QueryResult imu_query = |
| 178 | imu_data_.imu_buffer_.getImuDataInterpolatedUpperBorder( |
| 179 | last_timestamp_, timestamp, &imu_meas.timestamps_, |
| 180 | &imu_meas.measurements_); |
| 181 | if (imu_query == |
| 182 | utils::ThreadsafeImuBuffer::QueryResult::kDataAvailable) { |
| 183 | // data available |
| 184 | sensor_msgs::ImageConstPtr left_ros_img, right_ros_img; |
| 185 | CHECK(stereo_buffer_.extractLatestImages(left_ros_img, right_ros_img)); |
| 186 | |
| 187 | const StereoMatchingParams &stereo_matching_params = |
| 188 | frontend_params_.getStereoMatchingParams(); |
| 189 | |
| 190 | // Read ROS images to cv type |
| 191 | cv::Mat left_image, right_image; |
| 192 | |
| 193 | switch (stereo_matching_params.vision_sensor_type_) { |
| 194 | case VisionSensorType::STEREO: |
| 195 | left_image = readRosImage(left_ros_img); |
| 196 | right_image = readRosImage(right_ros_img); |
| 197 | break; |
| 198 | case VisionSensorType::RGBD: // just use depth to "fake |
| 199 | // right pixel matches" apply |
| 200 | // conversion |
| 201 | left_image = readRosImage(left_ros_img); |
| 202 | right_image = readRosDepthImage(right_ros_img); |
| 203 | break; |
| 204 | default: |
| 205 | LOG(FATAL) << "vision sensor type not recognised."; |
| 206 | break; |
| 207 | } |
| 208 | // Reset reinit flag for reinit packet |
| 209 | resetReinitFlag(); |
| 210 | |
| 211 | // Send input data to VIO! |
| 212 | vio_callback_(StereoImuSyncPacket( |
no test coverage detected