From 345ba7c1f3c47b6f27af0322e274e22eb132db15 Mon Sep 17 00:00:00 2001
From: Spyros Maniatopoulos <spmaniato@gmail.com>
Date: Tue, 16 Aug 2016 12:23:51 -0500
Subject: [PATCH 1/2] [Mapper] Add new parameter, MinimumTimeInterval

---
 include/open_karto/Mapper.h | 15 +++++++++++++++
 src/Mapper.cpp              | 22 ++++++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/include/open_karto/Mapper.h b/include/open_karto/Mapper.h
index 6959068..0593950 100644
--- a/include/open_karto/Mapper.h
+++ b/include/open_karto/Mapper.h
@@ -1709,6 +1709,19 @@ namespace karto
      */
     Parameter<kt_bool>* m_pUseScanBarycenter;
 
+    /**
+     * Sets the minimum time between scans. If a new scan's time stamp is
+     * longer than MinimumTimeInterval from the previously processed scan,
+     * the mapper will use the data from the new scan. Otherwise, it will
+     * discard the new scan if it also does not meet the minimum travel
+     * distance and heading requirements. For performance reasons, it is
+     * generally it is a good idea to only process scans if a reasonable
+     * amount of time has passed. This parameter is particularly useful
+     * when there is a need to process scans while the robot is stationary.
+     * Default value is 3600 (seconds), effectively disabling this parameter.
+     */
+    Parameter<kt_double>* m_pMinimumTimeInterval;
+
     /**
      * Sets the minimum travel between scans.  If a new scan's position is more than minimumTravelDistance
      * from the previous scan, the mapper will use the data from the new scan. Otherwise, it will discard the
@@ -1873,6 +1886,7 @@ namespace karto
     // General Parameters
     bool getParamUseScanMatching();
     bool getParamUseScanBarycenter();
+    double getParamMinimumTimeInterval();
     double getParamMinimumTravelDistance();
     double getParamMinimumTravelHeading();
     int getParamScanBufferSize();
@@ -1910,6 +1924,7 @@ namespace karto
     // General Parameters
     void setParamUseScanMatching(bool b);
     void setParamUseScanBarycenter(bool b);
+    void setParamMinimumTimeInterval(double d);
     void setParamMinimumTravelDistance(double d);
     void setParamMinimumTravelHeading(double d);
     void setParamScanBufferSize(int i);
diff --git a/src/Mapper.cpp b/src/Mapper.cpp
index efefaaf..8c01732 100644
--- a/src/Mapper.cpp
+++ b/src/Mapper.cpp
@@ -1676,6 +1676,18 @@ namespace karto
         "scans.",
         true, GetParameterManager());
 
+    m_pMinimumTimeInterval = new Parameter<kt_double>(
+        "MinimumTimeInterval",
+        "Sets the minimum time between scans. If a new scan's time stamp is "
+        "longer than MinimumTimeInterval from the previously processed scan, "
+        "the mapper will use the data from the new scan. Otherwise, it will "
+        "discard the new scan if it also does not meet the minimum travel "
+        "distance and heading requirements. For performance reasons, it is "
+        "generally it is a good idea to only process scans if a reasonable "
+        "amount of time has passed. This parameter is particularly useful "
+        "when there is a need to process scans while the robot is stationary.",
+        3600, GetParameterManager());
+
     m_pMinimumTravelDistance = new Parameter<kt_double>(
         "MinimumTravelDistance",
         "Sets the minimum travel between scans.  If a new scan's position is "
@@ -1865,6 +1877,11 @@ namespace karto
     return static_cast<bool>(m_pUseScanBarycenter->GetValue());
   }
 
+  double Mapper::getParamMinimumTimeInterval()
+  {
+    return static_cast<double>(m_pMinimumTimeInterval->GetValue());
+  }
+
   double Mapper::getParamMinimumTravelDistance()
   {
     return static_cast<double>(m_pMinimumTravelDistance->GetValue());
@@ -2013,6 +2030,11 @@ namespace karto
     m_pUseScanBarycenter->SetValue((kt_bool)b);
   }
 
+  void Mapper::setParamMinimumTimeInterval(double d)
+  {
+    m_pMinimumTimeInterval->SetValue((kt_double)d);
+  }
+
   void Mapper::setParamMinimumTravelDistance(double d)
   {
     m_pMinimumTravelDistance->SetValue((kt_double)d);

From 70c514523394f8143b83234bfda86a67a1df8472 Mon Sep 17 00:00:00 2001
From: Spyros Maniatopoulos <spmaniato@gmail.com>
Date: Tue, 16 Aug 2016 12:24:50 -0500
Subject: [PATCH 2/2] [Mapper] Take time into account in HasMoveEnough

The function HasMovedEnough now also returns true if more than MinimumTimeInterval time has elapsed since the previously processed laser scan
---
 src/Mapper.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/Mapper.cpp b/src/Mapper.cpp
index 8c01732..7d114b4 100644
--- a/src/Mapper.cpp
+++ b/src/Mapper.cpp
@@ -2303,6 +2303,13 @@ namespace karto
       return true;
     }
 
+    // test if enough time has passed
+    kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime();
+    if (timeInterval >= m_pMinimumTimeInterval->GetValue())
+    {
+      return true;
+    }
+
     Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose());
     Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose());