From b3e3dfd7d46b48023c4da1578a589398fc78b897 Mon Sep 17 00:00:00 2001
From: gaschler <gaschlera@gmail.com>
Date: Thu, 5 Apr 2018 12:18:16 +0200
Subject: [PATCH] Fix sequential subdivisions (#806)

FIXES=https://github.com/googlecartographer/cartographer/issues/1026
---
 cartographer_ros/cartographer_ros/sensor_bridge.cc | 10 ++++++++++
 cartographer_ros/cartographer_ros/sensor_bridge.h  |  2 ++
 2 files changed, 12 insertions(+)

diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc
index 8e3d28e..416efe4 100644
--- a/cartographer_ros/cartographer_ros/sensor_bridge.cc
+++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc
@@ -201,6 +201,16 @@ void SensorBridge::HandleLaserScan(
     // send all other sensor data first.
     const carto::common::Time subdivision_time =
         time + carto::common::FromSeconds(time_to_subdivision_end);
+    auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
+    if (it != sensor_to_previous_subdivision_time_.end() &&
+        it->second >= subdivision_time) {
+      LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
+                   << sensor_id << " because previous subdivision time "
+                   << it->second << " is not before current subdivision time "
+                   << subdivision_time;
+      continue;
+    }
+    sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
     for (Eigen::Vector4f& point : subdivision) {
       point[3] -= time_to_subdivision_end;
     }
diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h
index c2ab811..8485ea0 100644
--- a/cartographer_ros/cartographer_ros/sensor_bridge.h
+++ b/cartographer_ros/cartographer_ros/sensor_bridge.h
@@ -85,6 +85,8 @@ class SensorBridge {
                          const ::cartographer::sensor::TimedPointCloud& ranges);
 
   const int num_subdivisions_per_laser_scan_;
+  std::map<std::string, cartographer::common::Time>
+      sensor_to_previous_subdivision_time_;
   const TfBridge tf_bridge_;
   ::cartographer::mapping::TrajectoryBuilderInterface* const
       trajectory_builder_;