From eedab07c353e78690c2fe3b64c1acd17dfd2ba2f Mon Sep 17 00:00:00 2001 From: Samuel Lindgren Date: Tue, 23 Aug 2022 14:56:01 +0000 Subject: [PATCH 1/2] Increase scan filter queue_size --- src/slam_toolbox_common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 989c9114..9b9405c6 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -217,7 +217,7 @@ void SlamToolbox::setROSInterfaces() shared_from_this().get(), scan_topic_, rmw_qos_profile_sensor_data); scan_filter_ = std::make_unique>( - *scan_filter_sub_, *tf_, odom_frame_, 1, shared_from_this(), + *scan_filter_sub_, *tf_, odom_frame_, 5, shared_from_this(), tf2::durationFromSec(transform_timeout_.seconds())); scan_filter_->registerCallback( std::bind(&SlamToolbox::laserCallback, this, std::placeholders::_1)); From 0609660975dd49c09874b6830eafac5e7225ca57 Mon Sep 17 00:00:00 2001 From: Samuel Lindgren Date: Wed, 24 Aug 2022 15:57:46 +0000 Subject: [PATCH 2/2] Add scan_queue_size as parameter --- README.md | 2 ++ include/slam_toolbox/slam_toolbox_common.hpp | 2 +- src/slam_toolbox_common.cpp | 5 ++++- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index d2bb5ffc..a48f8c40 100644 --- a/README.md +++ b/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 diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 67a6c33e..9cca586a 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/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_; diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 9b9405c6..654a3625 100644 --- a/src/slam_toolbox_common.cpp +++ b/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>( - *scan_filter_sub_, *tf_, odom_frame_, 5, 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));