From be592f55fa46bf602f55384c961c39e63328720b Mon Sep 17 00:00:00 2001
From: gaschler <gaschlera@gmail.com>
Date: Mon, 26 Feb 2018 18:16:59 +0100
Subject: [PATCH] Validate tool checks per-point time stamps. (#737)

Checks for per-point timing issues in a bag file.
Feature is tracked in #529.
---
 .../cartographer_ros/rosbag_validate_main.cc  | 67 +++++++++++++++++--
 1 file changed, 62 insertions(+), 5 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc
index 627b661..b663ee7 100644
--- a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc
+++ b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc
@@ -149,7 +149,31 @@ class RangeDataChecker {
   template <typename MessageType>
   void CheckMessage(const MessageType& message) {
     const std::string& frame_id = message.header.frame_id;
-    RangeChecksum current_checksum = ComputeRangeChecksum(message);
+    ros::Time current_time_stamp = message.header.stamp;
+    RangeChecksum current_checksum;
+    ros::Duration range_duration;
+    ReadRangeMessage(message, &current_checksum, &range_duration);
+    ros::Time current_time_first_point = current_time_stamp - range_duration;
+    auto previous_time_stamp_it =
+        frame_id_to_previous_time_stamp_.find(frame_id);
+    if (previous_time_stamp_it != frame_id_to_previous_time_stamp_.end() &&
+        previous_time_stamp_it->second >= current_time_first_point) {
+      LOG_FIRST_N(WARNING, 3)
+          << "Sensor with frame_id \"" << frame_id
+          << "\" measurements overlap in time "
+          << "Previous range message, ending at time stamp "
+          << previous_time_stamp_it->second
+          << ", must finish before current range message, "
+          << "which ranges from " << current_time_first_point << " to "
+          << current_time_stamp;
+      double overlap =
+          (previous_time_stamp_it->second - current_time_first_point).toSec();
+      auto it = frame_id_to_max_overlap_duration_.find(frame_id);
+      if (it == frame_id_to_max_overlap_duration_.end() ||
+          overlap > frame_id_to_max_overlap_duration_.at(frame_id)) {
+        frame_id_to_max_overlap_duration_[frame_id] = overlap;
+      }
+    }
     if (current_checksum.first == 0) {
       return;
     }
@@ -160,11 +184,21 @@ class RangeDataChecker {
         LOG_FIRST_N(ERROR, 3)
             << "Sensor with frame_id \"" << frame_id
             << "\" sends exactly the same range measurements multiple times. "
-            << "Range data at time " << message.header.stamp
-            << " equals preceding data.";
+            << "Range data at time " << current_time_stamp
+            << " equals preceding data with " << current_checksum.first
+            << " points.";
       }
     }
     frame_id_to_range_checksum_[frame_id] = current_checksum;
+    frame_id_to_previous_time_stamp_[frame_id] = message.header.stamp;
+  }
+
+  void PrintReport() {
+    for (auto& it : frame_id_to_max_overlap_duration_) {
+      LOG(WARNING) << "Sensor with frame_id \"" << it.first
+                   << "\" range measurements have longest overlap of "
+                   << it.second << " s";
+    }
   }
 
  private:
@@ -172,7 +206,9 @@ class RangeDataChecker {
       RangeChecksum;
 
   template <typename MessageType>
-  RangeChecksum ComputeRangeChecksum(const MessageType& message) {
+  static void ReadRangeMessage(const MessageType& message,
+                               RangeChecksum* range_checksum,
+                               ros::Duration* range_duration) {
     auto point_cloud_with_intensities = ToPointCloudWithIntensities(message);
     const cartographer::sensor::TimedPointCloud& point_cloud =
         std::get<0>(point_cloud_with_intensities).points;
@@ -180,10 +216,29 @@ class RangeDataChecker {
     for (const Eigen::Vector4f& point : point_cloud) {
       points_sum += point;
     }
-    return {point_cloud.size(), points_sum};
+    if (point_cloud.size() > 0) {
+      double first_point_relative_time = point_cloud[0][3];
+      double last_point_relative_time = (*point_cloud.rbegin())[3];
+      if (first_point_relative_time > 0) {
+        LOG_FIRST_N(ERROR, 1)
+            << "Sensor with frame_id \"" << message.header.frame_id
+            << "\" has range data that has positive relative time "
+            << first_point_relative_time << " s, must negative or zero.";
+      }
+      if (last_point_relative_time != 0) {
+        LOG_FIRST_N(ERROR, 1)
+            << "Sensor with frame_id \"" << message.header.frame_id
+            << "\" has range data whose last point has relative time "
+            << last_point_relative_time << " s, must be zero.";
+      }
+    }
+    *range_checksum = {point_cloud.size(), points_sum};
+    *range_duration = ros::Duration(-point_cloud[0][3]);
   }
 
   std::map<std::string, RangeChecksum> frame_id_to_range_checksum_;
+  std::map<std::string, ros::Time> frame_id_to_previous_time_stamp_;
+  std::map<std::string, double> frame_id_to_max_overlap_duration_;
 };
 
 void Run(const std::string& bag_filename, const bool dump_timing) {
@@ -292,6 +347,8 @@ void Run(const std::string& bag_filename, const bool dump_timing) {
   }
   bag.close();
 
+  range_data_checker.PrintReport();
+
   if (num_imu_messages > 0) {
     double average_imu_acceleration = sum_imu_acceleration / num_imu_messages;
     if (std::isnan(average_imu_acceleration) ||