Merge pull request #137 from SPARK/feature/robust_rosbag_data_provider

Drop IMU msg that are equal, improve logging
MIT-SPARK · Dec 26, 2020 · 8913fbf · 8913fbf
2 parents 9f67f45 + d2db25a
commit 8913fbf
Showing 1 changed file with 36 additions and 24 deletions.
diff --git a/src/RosBagDataProvider.cpp b/src/RosBagDataProvider.cpp
@@ -125,9 +125,6 @@ bool RosbagDataProvider::spin() {
       static const CameraParams& right_cam_info =
           vio_params_.camera_params_.at(1);
 
-      LOG_IF(WARNING, timestamp_frame_k == timestamp_last_frame_)
-          << "Timestamps for current and previous frames are equal! This should"
-             "not happen...";
       if (timestamp_frame_k > timestamp_last_frame_) {
         // Send left frame data to Kimera:
         CHECK(left_frame_callback_)
@@ -161,11 +158,18 @@ bool RosbagDataProvider::spin() {
 
         timestamp_last_frame_ = timestamp_frame_k;
       } else {
-        LOG(WARNING) << "Skipping frame: " << k_ << '\n'
-                     << " Frame timestamps out of order:\n"
-                     << " Timestamp Current Frame: " << timestamp_frame_k
-                     << "\n"
-                     << " Timestamp Last Frame:    " << timestamp_last_frame_;
+        if (timestamp_frame_k == timestamp_last_frame_) {
+          LOG(WARNING)
+              << "Timestamps for current and previous frames are equal! \n"
+              << "This should not happen, dropping this frame... \n "
+              << "- Offending timestamp: " << timestamp_frame_k;
+        } else {
+          LOG(WARNING) << "Skipping frame: " << k_ << '\n'
+                       << " Frame timestamps out of order:\n"
+                       << " Timestamp Current Frame: " << timestamp_frame_k
+                       << "\n"
+                       << " Timestamp Last Frame:    " << timestamp_last_frame_;
+        }
       }
 
       publishRosbagInfo(timestamp_frame_k);
@@ -239,8 +243,18 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path,
         rosbag_data->imu_msgs_.push_back(imu_msg);
         last_imu_timestamp = imu_data_timestamp;
       } else {
-        LOG(FATAL) << "IMU timestamps in rosbag are out of order: consider "
-                      "re-ordering rosbag.";
+        if (imu_data_timestamp - last_imu_timestamp == 0u) {
+          LOG(WARNING) << "IMU timestamps in rosbag are repeated!\n"
+                       << "Offending timestamp: " << imu_data_timestamp;
+        } else {
+          LOG(FATAL) << "IMU timestamps in rosbag are out of order: consider "
+                     << "re-ordering rosbag: \n"
+                     << "- Current IMU timestamp: " << imu_data_timestamp
+                     << '\n'
+                     << "- Last IMU timestamp: " << last_imu_timestamp << '\n'
+                     << "Difference (current - last) = "
+                     << imu_data_timestamp - last_imu_timestamp;
+        }
       }
       start_parsing_stereo = true;
       continue;
@@ -321,25 +335,23 @@ void RosbagDataProvider::logGtData(const nav_msgs::OdometryConstPtr& odometry) {
                   << "bgx,bgy,bgz,bax,bay,baz" << std::endl;
     is_header_written = true;
   }
-  output_stream << odometry->header.stamp.toNSec()   << ","  //
-                << odometry->pose.pose.position.x    << ","    //
-                << odometry->pose.pose.position.y    << ","    //
-                << odometry->pose.pose.position.z    << ","    //
+  output_stream << odometry->header.stamp.toNSec() << ","  //
+                << odometry->pose.pose.position.x << ","   //
+                << odometry->pose.pose.position.y << ","   //
+                << odometry->pose.pose.position.z << ","   //
                 << odometry->pose.pose.orientation.w << ","
                 << odometry->pose.pose.orientation.x << ","
                 << odometry->pose.pose.orientation.y << ","
                 << odometry->pose.pose.orientation.z << ","
-                << odometry->twist.twist.linear.x    << ","
-                << odometry->twist.twist.linear.y    << ","
-                << odometry->twist.twist.linear.z    << ","
-                << 0.0 << ","         //
-                << 0.0 << ","         //
-                << 0.0 << ","         //
-                << 0.0 << ","          //
-                << 0.0 << ","          //
-                << 0.0                 //
+                << odometry->twist.twist.linear.x << ","
+                << odometry->twist.twist.linear.y << ","
+                << odometry->twist.twist.linear.z << "," << 0.0 << ","  //
+                << 0.0 << ","                                           //
+                << 0.0 << ","                                           //
+                << 0.0 << ","                                           //
+                << 0.0 << ","                                           //
+                << 0.0                                                  //
                 << std::endl;
-
 }
 
 VioNavState RosbagDataProvider::getGroundTruthVioNavState(