Add option to perform scan matching at a minimum rate without needing to update map

README.md

@@ -248,21 +248,23 @@ The following settings and options are exposed to you. My default configuration
 
 `minimum_time_interval` - The minimum duration of time between scans to be processed in synchronous mode
 
+`maximum_match_interval` - Max interval in seconds between scan matches in async mode.  Note: Scan matches triggered by this condition won't update the map or scan buffer.  Default: -1 (infinity).
+
 `transform_timeout` - TF timeout for looking up transforms
 
 `tf_buffer_duration` - Duration to store TF messages for lookup. Set high if running offline at multiple times speed in synchronous mode. 
 
 `stack_size_to_use` - The number of bytes to reset the stack size to, to enable serialization/deserialization of files. A liberal default is 40000000, but less is fine.
 
-`minimum_travel_distance` - Minimum distance of travel before processing a new scan
+`minimum_travel_distance` - Minimum distance of travel before processing a new scan into the map, or adding a scan to the queue for processing in sync or localization modes.
 
 ## Matcher Params
 
 `use_scan_matching` - whether to use scan matching to refine odometric pose (uh, why would you not?)
 
 `use_scan_barycenter` - Whether to use the barycenter or scan pose
 
-`minimum_travel_heading` - Minimum changing in heading to justify an update
+`minimum_travel_heading` - Minimum changing in heading before processing a new scan into the map
 
 `scan_buffer_size` - The number of scans to buffer into a chain, also used as the number of scans in the circular buffer of localization mode
 

include/slam_toolbox/slam_toolbox_common.hpp

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

lib/karto_sdk/include/karto_sdk/Mapper.h

@@ -1994,9 +1994,15 @@ class KARTO_EXPORT Mapper : public Module
    * is that of the range device originating the scan.  Note that the mapper will set corrected pose
    * information in the scan object.
    *
+   * @param force_match_only Flag to force a scan match regardless of distance or angle traveled and to only return
+   * the resulting pose instead of adding the scan to the map.
+   *
    * @return true if the scan was added successfully, false otherwise
    */
-  virtual kt_bool Process(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
+  virtual kt_bool Process(
+    LocalizedRangeScan * pScan,
+    Matrix3 * covariance = nullptr,
+    bool force_match_only = false);
 
   /**
    * Process an Object

lib/karto_sdk/src/Mapper.cpp

@@ -2676,7 +2676,10 @@ kt_bool Mapper::Process(Object *  /*pObject*/)  // NOLINT
   return true;
 }
 
-kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance)
+kt_bool Mapper::Process(
+  LocalizedRangeScan * pScan,
+  Matrix3 * covariance,
+  bool force_match_only)
 {
   if (pScan != NULL) {
     karto::LaserRangeFinder * pLaserRangeFinder = pScan->GetLaserRangeFinder();
@@ -2701,7 +2704,7 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance)
     }
 
     // test if scan is outside minimum boundary or if heading is larger then minimum heading
-    if (!HasMovedEnough(pScan, pLastScan)) {
+    if (!force_match_only && !HasMovedEnough(pScan, pLastScan)) {
       return false;
     }
 
@@ -2721,26 +2724,28 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance)
       }
     }
 
-    // add scan to buffer and assign id
-    m_pMapperSensorManager->AddScan(pScan);
+    if (!force_match_only) {
+      // add scan to buffer and assign id
+      m_pMapperSensorManager->AddScan(pScan);
 
-    if (m_pUseScanMatching->GetValue()) {
-      // add to graph
-      m_pGraph->AddVertex(pScan);
-      m_pGraph->AddEdges(pScan, cov);
+      if (m_pUseScanMatching->GetValue()) {
+        // add to graph
+        m_pGraph->AddVertex(pScan);
+        m_pGraph->AddEdges(pScan, cov);
 
-      m_pMapperSensorManager->AddRunningScan(pScan);
+        m_pMapperSensorManager->AddRunningScan(pScan);
 
-      if (m_pDoLoopClosing->GetValue()) {
-        std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
-        const_forEach(std::vector<Name>, &deviceNames)
-        {
-          m_pGraph->TryCloseLoop(pScan, *iter);
+        if (m_pDoLoopClosing->GetValue()) {
+          std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
+          const_forEach(std::vector<Name>, &deviceNames)
+          {
+            m_pGraph->TryCloseLoop(pScan, *iter);
+          }
         }
       }
-    }
 
-    m_pMapperSensorManager->SetLastScan(pScan);
+      m_pMapperSensorManager->SetLastScan(pScan);
+    }
 
     return true;
   }

src/slam_toolbox_common.cpp

@@ -41,7 +41,8 @@ SlamToolbox::SlamToolbox(rclcpp::NodeOptions options)
   first_measurement_(true),
   process_near_pose_(nullptr),
   transform_timeout_(rclcpp::Duration::from_seconds(0.5)),
-  minimum_time_interval_(std::chrono::nanoseconds(0))
+  minimum_time_interval_(std::chrono::nanoseconds(0)),
+  maximum_match_interval_(rclcpp::Duration::from_seconds(-1.0))
 /*****************************************************************************/
 {
   smapper_ = std::make_unique<mapper_utils::SMapper>();
@@ -155,11 +156,14 @@ void SlamToolbox::setParams()
   enable_interactive_mode_ = this->declare_parameter("enable_interactive_mode",
       enable_interactive_mode_);
 
+
   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);
+  tmp_val = this->declare_parameter("maximum_match_interval", -1.0);
+  maximum_match_interval_ = rclcpp::Duration::from_seconds(tmp_val);
 
   bool debug = false;
   debug = this->declare_parameter("debug_logging", debug);
@@ -543,6 +547,8 @@ LocalizedRangeScan * SlamToolbox::addScan(
   Pose2 & odom_pose)
 /*****************************************************************************/
 {
+  static rclcpp::Time last_match_time = rclcpp::Time(0.);
+
   // get our localized range scan
   LocalizedRangeScan * range_scan = getLocalizedRangeScan(
     laser, scan, odom_pose);
@@ -551,11 +557,26 @@ LocalizedRangeScan * SlamToolbox::addScan(
   boost::mutex::scoped_lock lock(smapper_mutex_);
   bool processed = false, update_reprocessing_transform = false;
 
+  // whether or not the scan was processed as only a scan match without updating
+  // the graph and scan buffer
+  bool match_only = false;
+
   Matrix3 covariance;
   covariance.SetToIdentity();
 
   if (processor_type_ == PROCESS) {
     processed = smapper_->getMapper()->Process(range_scan, &covariance);
+
+    // if the scan was not processed into the map because of insuffcient travel
+    // distance, then check if enough time as passed to just perform a scan
+    // match without updating the graph or scan buffer
+    rclcpp::Time stamp = scan->header.stamp;
+    bool match_only = !processed
+      && maximum_match_interval_ >= rclcpp::Duration(0.)
+      && stamp - last_match_time > maximum_match_interval_;
+    if (match_only) {
+      processed = smapper_->getMapper()->Process(range_scan, &covariance, true);
+    }
   } else if (processor_type_ == PROCESS_FIRST_NODE) {
     processed = smapper_->getMapper()->ProcessAtDock(range_scan, &covariance);
     processor_type_ = PROCESS;
@@ -583,7 +604,8 @@ LocalizedRangeScan * SlamToolbox::addScan(
   // if successfully processed, create odom to map transformation
   // and add our scan to storage
   if (processed) {
-    if (enable_interactive_mode_) {
+    last_match_time = scan->header.stamp;
+    if (enable_interactive_mode_ && !match_only) {
       scan_holder_->addScan(*scan);
     }