Midigate spamming for 50Hz lidar (#526)

* Increase scan filter queue_size

* Add scan_queue_size as parameter

README.md

@@ -222,6 +222,8 @@ The following settings and options are exposed to you. My default configuration
 
 `scan_topic` - scan topic, *absolute* path, ei `/scan` not `scan`
 
+`scan_queue_size` - The number of scan messages to queue up before throwing away old ones. Should always be set to 1 in async mode
+
 `map_file_name` - Name of the pose-graph file to load on startup if available
 
 `map_start_pose` - Pose to start pose-graph mapping/localization in, if available

include/slam_toolbox/slam_toolbox_common.hpp

@@ -144,7 +144,7 @@ class SlamToolbox : public rclcpp::Node
   std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_;
   rclcpp::Duration transform_timeout_, minimum_time_interval_;
   std_msgs::msg::Header scan_header;
-  int throttle_scans_;
+  int throttle_scans_, scan_queue_size_;
 
   double resolution_;
   double position_covariance_scale_;

src/slam_toolbox_common.cpp

@@ -142,6 +142,9 @@ void SlamToolbox::setParams()
   scan_topic_ = std::string("/scan");
   scan_topic_ = this->declare_parameter("scan_topic", scan_topic_);
 
+  scan_queue_size_ = 1.0;
+  scan_queue_size_ = this->declare_parameter("scan_queue_size", scan_queue_size_);
+
   throttle_scans_ = 1;
   throttle_scans_ = this->declare_parameter("throttle_scans", throttle_scans_);
 
@@ -217,7 +220,7 @@ void SlamToolbox::setROSInterfaces()
     shared_from_this().get(), scan_topic_, rmw_qos_profile_sensor_data);
   scan_filter_ =
     std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
-    *scan_filter_sub_, *tf_, odom_frame_, 1, shared_from_this(),
+    *scan_filter_sub_, *tf_, odom_frame_, scan_queue_size_, shared_from_this(),
     tf2::durationFromSec(transform_timeout_.seconds()));
   scan_filter_->registerCallback(
     std::bind(&SlamToolbox::laserCallback, this, std::placeholders::_1));