fix shouldProcessScan() to do what is advertised

CMakeLists.txt

@@ -199,6 +199,7 @@ rclcpp_components_register_nodes(map_and_localization_slam_toolbox "slam_toolbox
 install(TARGETS async_slam_toolbox_node
                 sync_slam_toolbox_node
                 localization_slam_toolbox_node
+		lifelong_slam_toolbox_node
                 map_and_localization_slam_toolbox_node
                 merge_maps_kinematic
                 ${libraries}

include/slam_toolbox/slam_toolbox_common.hpp

@@ -143,7 +143,7 @@ class SlamToolbox : public rclcpp::Node
   // Storage for ROS parameters
   std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_;
   bool use_map_saver_;
-  rclcpp::Duration transform_timeout_, minimum_time_interval_;
+  rclcpp::Duration transform_timeout_;
   std_msgs::msg::Header scan_header;
   int throttle_scans_, scan_queue_size_;
 

lib/karto_sdk/include/karto_sdk/Mapper.h

@@ -2091,17 +2091,33 @@ class KARTO_EXPORT Mapper : public Module
     return m_LocalizationScanVertices;
   }
 
-protected:
-  void InitializeParameters();
+  /**
+   * Test if the enough time has passed since the last processed scan.
+   * @param scanTime scan to be checked
+   * @param lastScanTime last scan added to mapper
+   * @return true if the minimum alloted time has passed
+   */
+  kt_bool EnoughTimeHasPassed(const kt_double& scanTime, const kt_double& lastScanTime) const;
 
   /**
    * Test if the scan is "sufficiently far" from the last scan added.
+   * @param scannerPose scanner pose to be checked
+   * @param lastScannerPose pose of the last scan added to mapper
+   * @return true if the scan is "sufficiently far" from the last scan added
+   */
+  kt_bool HasMovedEnough(const Pose2& scannerPose, const Pose2& lastScannerPose) const;
+
+  /**
+   * See if we should continue processing this scan.
    * @param pScan scan to be checked
    * @param pLastScan last scan added to mapper
-   * @return true if the scan is "sufficiently far" from the last scan added or
-   * the scan is the first scan to be added
+   * @return true if it is the first scan, if enough time has passed since the last one or 
+   * the scan pose has moved enough.
    */
-  kt_bool HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * pLastScan) const;
+  kt_bool ContinueProcessingScan(LocalizedRangeScan * pScan, LocalizedRangeScan * pLastScan) const;
+
+protected:
+  void InitializeParameters();
 
 public:
   /////////////////////////////////////////////

lib/karto_sdk/src/Mapper.cpp

@@ -2700,8 +2700,7 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance)
       pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose()));
     }
 
-    // test if scan is outside minimum boundary or if heading is larger then minimum heading
-    if (!HasMovedEnough(pScan, pLastScan)) {
+    if(!ContinueProcessingScan(pScan, pLastScan)){
       return false;
     }
 
@@ -2860,9 +2859,7 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covari
         pScan->GetOdometricPose()));
   }
 
-  // test if scan is outside minimum boundary
-  // or if heading is larger then minimum heading
-  if (!HasMovedEnough(pScan, pLastScan)) {
+  if(!ContinueProcessingScan(pScan, pLastScan)){
     return false;
   }
 
@@ -3102,27 +3099,47 @@ kt_bool Mapper::ProcessAtDock(LocalizedRangeScan * pScan, Matrix3 * covariance)
 }
 
 /**
- * Is the scan sufficiently far from the last scan?
- * @param pScan
- * @param pLastScan
- * @return true if the scans are sufficiently far
- */
-kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * pLastScan) const
+* See if we should continue processing this scan.
+* @param pScan scan to be checked
+* @param pLastScan last scan added to mapper
+* @return true if it is the first scan, if enough time has passed since the last one or 
+* the scan pose has moved enough.
+*/
+kt_bool Mapper::ContinueProcessingScan(LocalizedRangeScan * pScan, LocalizedRangeScan * pLastScan) const
+{
+  if(pLastScan == NULL) return true;
+  if(
+    EnoughTimeHasPassed(pScan->GetTime(), pLastScan->GetTime()) ||
+    HasMovedEnough(pScan->GetOdometricPose(), pLastScan->GetOdometricPose())
+    ){
+      return true;
+    }
+  return false;
+}
+/**
+* Test if the enough time has passed since the last processed scan.
+* @param pScan scan to be checked
+* @param pLastScan last scan added to mapper
+* @return true if the minimum alloted time has passed
+*/
+kt_bool Mapper::EnoughTimeHasPassed(const kt_double& scanTime, const kt_double& lastScanTime) const
 {
-  // test if first scan
-  if (pLastScan == NULL) {
-    return true;
-  }
-
   // test if enough time has passed
-  kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime();
+  kt_double timeInterval = scanTime - lastScanTime;
   if (timeInterval >= m_pMinimumTimeInterval->GetValue()) {
     return true;
   }
+  return false;
+}
 
-  Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose());
-  Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose());
-
+/**
+* Test if the scan is "sufficiently far" from the last scan added.
+* @param scannerPose scanner pose to be checked
+* @param lastScannerPose pose of the last scan added to mapper
+* @return true if the scan is "sufficiently far" from the last scan added
+*/
+kt_bool Mapper::HasMovedEnough(const Pose2& scannerPose, const Pose2& lastScannerPose) const
+{
   // test if we have turned enough
   kt_double deltaHeading = math::NormalizeAngle(
     scannerPose.GetHeading() - lastScannerPose.GetHeading());
@@ -3140,6 +3157,7 @@ kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan *
   return false;
 }
 
+
 /**
  * Gets all the processed scans
  * @return all scans

src/slam_mapper.cpp

@@ -110,6 +110,13 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node)
   node->get_parameter("use_scan_barycenter", use_scan_barycenter);
   mapper_->setParamUseScanBarycenter(use_scan_barycenter);
 
+  double minimum_time_interval = 3600;
+  if (!node->has_parameter("minimum_time_interval")) {
+    node->declare_parameter("minimum_time_interval", minimum_time_interval);
+  }
+  node->get_parameter("minimum_time_interval", minimum_time_interval);
+  mapper_->setParamMinimumTimeInterval(minimum_time_interval);
+
   double minimum_travel_distance = 0.5;
   if (!node->has_parameter("minimum_travel_distance")) {
     node->declare_parameter("minimum_travel_distance", minimum_travel_distance);

src/slam_toolbox_common.cpp

@@ -20,6 +20,7 @@
 #include <vector>
 #include <string>
 #include <chrono>
+#include <math.h>
 #include "slam_toolbox/slam_toolbox_common.hpp"
 #include "slam_toolbox/serialization.hpp"
 
@@ -40,8 +41,7 @@ SlamToolbox::SlamToolbox(rclcpp::NodeOptions options)
   processor_type_(PROCESS),
   first_measurement_(true),
   process_near_pose_(nullptr),
-  transform_timeout_(rclcpp::Duration::from_seconds(0.5)),
-  minimum_time_interval_(std::chrono::nanoseconds(0))
+  transform_timeout_(rclcpp::Duration::from_seconds(0.5))
 /*****************************************************************************/
 {
   smapper_ = std::make_unique<mapper_utils::SMapper>();
@@ -176,8 +176,6 @@ void SlamToolbox::setParams()
   double tmp_val = 0.5;
   tmp_val = this->declare_parameter("transform_timeout", tmp_val);
   transform_timeout_ = rclcpp::Duration::from_seconds(tmp_val);
-  tmp_val = this->declare_parameter("minimum_time_interval", tmp_val);
-  minimum_time_interval_ = rclcpp::Duration::from_seconds(tmp_val);
 
   bool debug = false;
   debug = this->declare_parameter("debug_logging", debug);
@@ -506,9 +504,9 @@ bool SlamToolbox::shouldProcessScan(
 {
   static Pose2 last_pose;
   static rclcpp::Time last_scan_time = rclcpp::Time(0.);
-  static double min_dist2 =
-    smapper_->getMapper()->getParamMinimumTravelDistance() *
-    smapper_->getMapper()->getParamMinimumTravelDistance();
+  static double min_dist = smapper_->getMapper()->getParamMinimumTravelDistance();
+  static double min_angle =
+    smapper_->getMapper()->getParamMinimumTravelHeading() /*in degrees, needs conversion to rad: */ * M_PI/180;
   static int scan_ctr = 0;
   scan_ctr++;
 
@@ -530,21 +528,17 @@ bool SlamToolbox::shouldProcessScan(
     return false;
   }
 
-  // not enough time
-  if (rclcpp::Time(scan->header.stamp) - last_scan_time < minimum_time_interval_) {
-    return false;
+  if(
+    smapper_->getMapper()->EnoughTimeHasPassed(rclcpp::Time(scan->header.stamp).seconds(), last_scan_time.seconds()) ||
+    smapper_->getMapper()->HasMovedEnough(pose, last_pose)
+  ){
+    last_pose = pose;
+    last_scan_time = scan->header.stamp;
+    return true;
   }
-
-  // check moved enough, within 10% for correction error
-  const double dist2 = last_pose.SquaredDistance(pose);
-  if (dist2 < 0.8 * min_dist2 || scan_ctr < 5) {
+  else{
     return false;
   }
-
-  last_pose = pose;
-  last_scan_time = scan->header.stamp;
-
-  return true;
 }
 
 /*****************************************************************************/