Added parameter check_min_dist_and_heading_precisely

README.md

@@ -282,6 +282,10 @@ The following settings and options are exposed to you. My default configuration
 
 `minimum_travel_distance` - Minimum distance of travel before processing a new scan
 
+`minimum_travel_heading` - Minimum changing in heading to justify an update.
+
+`check_min_dist_and_heading_precisely` - Whether to always check if either *`minimum_travel_distance`* or *`minimum_travel_heading`* is satisfied. With the default value *`false`*, the behavior suits most cases where, for example, rotational odometry is poor.
+
 `localization_on_configure` - Set to true to set the localization mode to localization during node on_configure transition. Set to false to set the localization mode to mapping instead. Only applies to `map_and_localization_slam_toolbox` node.
 
 ## Matcher Params
@@ -290,8 +294,6 @@ The following settings and options are exposed to you. My default configuration
 
 `use_scan_barycenter` - Whether to use the barycenter or scan pose
 
-`minimum_travel_heading` - Minimum changing in heading to justify an update
-
 `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
 
 `scan_buffer_maximum_scan_distance` - Maximum distance of a scan from the pose before removing the scan from the buffer

config/mapper_params_lifelong.yaml

@@ -53,6 +53,7 @@ slam_toolbox:
     use_scan_barycenter: true
     minimum_travel_distance: 0.5
     minimum_travel_heading: 0.5
+    check_min_dist_and_heading_precisely: false
     scan_buffer_size: 10
     scan_buffer_maximum_scan_distance: 10.0
     link_match_minimum_response_fine: 0.1  

config/mapper_params_localization.yaml

@@ -36,6 +36,7 @@ slam_toolbox:
     use_scan_barycenter: true
     minimum_travel_distance: 0.5
     minimum_travel_heading: 0.5
+    check_min_dist_and_heading_precisely: false
     scan_buffer_size: 3
     scan_buffer_maximum_scan_distance: 10.0
     link_match_minimum_response_fine: 0.1  

config/mapper_params_offline.yaml

@@ -35,6 +35,7 @@ slam_toolbox:
     use_scan_barycenter: true
     minimum_travel_distance: 0.5
     minimum_travel_heading: 0.5
+    check_min_dist_and_heading_precisely: false
     scan_buffer_size: 10
     scan_buffer_maximum_scan_distance: 10.0
     link_match_minimum_response_fine: 0.1  

config/mapper_params_online_async.yaml

@@ -43,6 +43,7 @@ slam_toolbox:
     use_scan_barycenter: true
     minimum_travel_distance: 0.5
     minimum_travel_heading: 0.5
+    check_min_dist_and_heading_precisely: false
     scan_buffer_size: 10
     scan_buffer_maximum_scan_distance: 10.0
     link_match_minimum_response_fine: 0.1  

config/mapper_params_online_multi_async.yaml

@@ -40,6 +40,7 @@ $(var namespace)/slam_toolbox:
     use_scan_barycenter: true
     minimum_travel_distance: 0.5
     minimum_travel_heading: 0.5
+    check_min_dist_and_heading_precisely: false
     scan_buffer_size: 10
     scan_buffer_maximum_scan_distance: 10.0
     link_match_minimum_response_fine: 0.1  
@@ -70,4 +71,4 @@ $(var namespace)/slam_toolbox:
     coarse_angle_resolution: 0.0349        
     minimum_angle_penalty: 0.9
     minimum_distance_penalty: 0.5
-    use_response_expansion: true
+    use_response_expansion: true

config/mapper_params_online_sync.yaml

@@ -43,6 +43,7 @@ slam_toolbox:
     use_scan_barycenter: true
     minimum_travel_distance: 0.5
     minimum_travel_heading: 0.5
+    check_min_dist_and_heading_precisely: false
     scan_buffer_size: 10
     scan_buffer_maximum_scan_distance: 10.0
     link_match_minimum_response_fine: 0.1  

include/slam_toolbox/slam_toolbox_common.hpp

@@ -174,6 +174,7 @@ class SlamToolbox : public rclcpp_lifecycle::LifecycleNode
   double yaw_covariance_scale_;
   bool first_measurement_, enable_interactive_mode_;
   bool restamp_tf_;
+  bool check_min_dist_and_heading_precisely_;
 
   // Book keeping
   std::unique_ptr<mapper_utils::SMapper> smapper_;

lib/karto_sdk/include/karto_sdk/Mapper.h

@@ -2424,6 +2424,7 @@ class KARTO_EXPORT Mapper : public Module
   double getParamMinimumTimeInterval();
   double getParamMinimumTravelDistance();
   double getParamMinimumTravelHeading();
+  double getParamMinimumTravelHeadingInRadians();
   int getParamScanBufferSize();
   double getParamScanBufferMaximumScanDistance();
   double getParamLinkMatchMinimumResponseFine();

lib/karto_sdk/src/Mapper.cpp

@@ -2333,6 +2333,11 @@ double Mapper::getParamMinimumTravelHeading()
   return math::RadiansToDegrees(static_cast<double>(m_pMinimumTravelHeading->GetValue()));
 }
 
+double Mapper::getParamMinimumTravelHeadingInRadians()
+{
+  return static_cast<double>(m_pMinimumTravelHeading->GetValue());
+}
+
 int Mapper::getParamScanBufferSize()
 {
   return static_cast<int>(m_pScanBufferSize->GetValue());

src/slam_toolbox_common.cpp

@@ -411,6 +411,13 @@ void SlamToolbox::setParams()
   tmp_val = this->get_parameter("minimum_time_interval").as_double();
   minimum_time_interval_ = rclcpp::Duration::from_seconds(tmp_val);
 
+  check_min_dist_and_heading_precisely_ = false;
+  if (!this->has_parameter("check_min_dist_and_heading_precisely")) {
+    this->declare_parameter("check_min_dist_and_heading_precisely", check_min_dist_and_heading_precisely_);
+  }
+  check_min_dist_and_heading_precisely_ =
+    this->get_parameter("check_min_dist_and_heading_precisely").as_bool();
+
   bool debug = false;
   if (!this->has_parameter("debug_logging")) {
     this->declare_parameter("debug_logging", debug);
@@ -758,6 +765,8 @@ bool SlamToolbox::shouldProcessScan(
   static double min_dist2 =
     smapper_->getMapper()->getParamMinimumTravelDistance() *
     smapper_->getMapper()->getParamMinimumTravelDistance();
+  static double min_rotation =
+    smapper_->getMapper()->getParamMinimumTravelHeadingInRadians();
   static int scan_ctr = 0;
   scan_ctr++;
 
@@ -784,9 +793,21 @@ bool SlamToolbox::shouldProcessScan(
     return false;
   }
 
-  // check moved enough, within 10% for correction error
+  // for initial stabilization
+  if (scan_ctr < 5) {
+    return false;
+  }
+
+  // check if the movement is enough
   const double dist2 = last_pose.SquaredDistance(pose);
-  if (dist2 < 0.8 * min_dist2 || scan_ctr < 5) {
+  if (check_min_dist_and_heading_precisely_) {
+    const double heading_diff =
+      fabs(math::NormalizeAngle(pose.GetHeading() - last_pose.GetHeading()));
+    if (dist2 < min_dist2 && heading_diff < min_rotation) {
+      return false;
+    }
+  } else if (dist2 < 0.8 * min_dist2) {
+    // within 10% for correction error, min heading is occasionally checked in the mapper
     return false;
   }