Merge pull request #11 from Maidbot/minimum_time_interval

Process scan if enough time has elapsed since the previous one

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);

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);
@@ -2281,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());