From 8d6a35f54c5a713cdc700bc64c57e333240de3f3 Mon Sep 17 00:00:00 2001 From: Marc Alban Date: Tue, 21 Jun 2022 08:53:08 -0500 Subject: [PATCH 1/3] Add option to perform continuous scan matching to update the transform and pose even when the minimum travel distance or heading have not been reached for adding the scan into the map. --- README.md | 8 +++-- include/slam_toolbox/slam_toolbox_common.hpp | 1 + lib/karto_sdk/include/karto_sdk/Mapper.h | 5 ++- lib/karto_sdk/src/Mapper.cpp | 34 +++++++++++--------- src/slam_toolbox_common.cpp | 13 +++++++- 5 files changed, 40 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 3f403bfb..2e22c2b9 100644 --- a/README.md +++ b/README.md @@ -248,7 +248,11 @@ The following settings and options are exposed to you. My default configuration `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 + +`minimum_travel_heading` - Minimum changing in heading before processing a new scan into the map + +`enable_continuous_matching` - Perform scan matching to update the transform and pose even when the minimum travel distance or heading have not been reached for adding the scan into the map ## Matcher Params @@ -256,8 +260,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 diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 1f98e78f..2ab52903 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -143,6 +143,7 @@ class SlamToolbox : public rclcpp::Node double resolution_; bool first_measurement_, enable_interactive_mode_; + bool enable_continuous_matching_; // Book keeping std::unique_ptr smapper_; diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index ae6c8cad..1fb85d79 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1994,9 +1994,12 @@ 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); + virtual kt_bool Process(LocalizedRangeScan * pScan, bool force_match_only); /** * Process an Object diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 27c02930..a1864daa 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -2676,7 +2676,7 @@ kt_bool Mapper::Process(Object * /*pObject*/) // NOLINT return true; } -kt_bool Mapper::Process(LocalizedRangeScan * pScan) +kt_bool Mapper::Process(LocalizedRangeScan * pScan, bool force_match_only) { if (pScan != NULL) { karto::LaserRangeFinder * pLaserRangeFinder = pScan->GetLaserRangeFinder(); @@ -2701,7 +2701,7 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan) } // 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; } @@ -2718,26 +2718,28 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan) pScan->SetSensorPose(bestPose); } - // 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, covariance); + if (m_pUseScanMatching->GetValue()) { + // add to graph + m_pGraph->AddVertex(pScan); + m_pGraph->AddEdges(pScan, covariance); - m_pMapperSensorManager->AddRunningScan(pScan); + m_pMapperSensorManager->AddRunningScan(pScan); - if (m_pDoLoopClosing->GetValue()) { - std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); - const_forEach(std::vector, &deviceNames) - { - m_pGraph->TryCloseLoop(pScan, *iter); + if (m_pDoLoopClosing->GetValue()) { + std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); + const_forEach(std::vector, &deviceNames) + { + m_pGraph->TryCloseLoop(pScan, *iter); + } } } - } - m_pMapperSensorManager->SetLastScan(pScan); + m_pMapperSensorManager->SetLastScan(pScan); + } return true; } diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 6d6095e0..0015b9bb 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -149,6 +149,9 @@ void SlamToolbox::setParams() enable_interactive_mode_ = this->declare_parameter("enable_interactive_mode", enable_interactive_mode_); + enable_continuous_matching_ = false; + enable_continuous_matching_ = this->declare_parameter("enable_continuous_matching", enable_continuous_matching_); + double tmp_val = 0.5; tmp_val = this->declare_parameter("transform_timeout", tmp_val); transform_timeout_ = rclcpp::Duration::from_seconds(tmp_val); @@ -542,9 +545,17 @@ LocalizedRangeScan * SlamToolbox::addScan( // Add the localized range scan to the smapper boost::mutex::scoped_lock lock(smapper_mutex_); bool processed = false, update_reprocessing_transform = false; + bool match_only = false; if (processor_type_ == PROCESS) { processed = smapper_->getMapper()->Process(range_scan); + + // if continuos mapping is enabled, force a scan match without adding it to + // the graph or scan buffer + if (!processed && enable_continuous_matching_) { + match_only = true; + processed = smapper_->getMapper()->Process(range_scan, true); + } } else if (processor_type_ == PROCESS_FIRST_NODE) { processed = smapper_->getMapper()->ProcessAtDock(range_scan); processor_type_ = PROCESS; @@ -571,7 +582,7 @@ LocalizedRangeScan * SlamToolbox::addScan( // if successfully processed, create odom to map transformation // and add our scan to storage if (processed) { - if (enable_interactive_mode_) { + if (enable_interactive_mode_ && !match_only) { scan_holder_->addScan(*scan); } From 719eb9886fe07f9f1d6a077e395fa06d300a3526 Mon Sep 17 00:00:00 2001 From: Marc Alban Date: Wed, 22 Jun 2022 14:46:42 -0500 Subject: [PATCH 2/3] Tweaks to parameter documentation. --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 2e22c2b9..07ea6c87 100644 --- a/README.md +++ b/README.md @@ -248,11 +248,7 @@ The following settings and options are exposed to you. My default configuration `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 into the map - -`minimum_travel_heading` - Minimum changing in heading before processing a new scan into the map - -`enable_continuous_matching` - Perform scan matching to update the transform and pose even when the minimum travel distance or heading have not been reached for adding the scan into the map +`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 @@ -260,6 +256,10 @@ 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 before processing a new scan into the map + +`enable_continuous_matching` - Perform scan matching to update the transform and pose even when the minimum travel distance or heading have not been reached for adding the 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 `scan_buffer_maximum_scan_distance` - Maximum distance of a scan from the pose before removing the scan from the buffer From dca9c10274b8973ecd31ba2bc0df832053e9fbe9 Mon Sep 17 00:00:00 2001 From: Marc Alban Date: Wed, 22 Jun 2022 16:52:23 -0500 Subject: [PATCH 3/3] Refactor scan match only logic to be based on a time interval. --- README.md | 4 ++-- include/slam_toolbox/slam_toolbox_common.hpp | 2 +- src/slam_toolbox_common.cpp | 25 ++++++++++++++------ 3 files changed, 21 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index a0f89679..712cb650 100644 --- a/README.md +++ b/README.md @@ -248,6 +248,8 @@ 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. @@ -264,8 +266,6 @@ The following settings and options are exposed to you. My default configuration `minimum_travel_heading` - Minimum changing in heading before processing a new scan into the map -`enable_continuous_matching` - Perform scan matching to update the transform and pose even when the minimum travel distance or heading have not been reached for adding the 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 `scan_buffer_maximum_scan_distance` - Maximum distance of a scan from the pose before removing the scan from the buffer diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 0941548d..97480c7b 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/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_; @@ -150,7 +151,6 @@ class SlamToolbox : public rclcpp::Node double position_covariance_scale_; double yaw_covariance_scale_; bool first_measurement_, enable_interactive_mode_; - bool enable_continuous_matching_; // Book keeping std::unique_ptr smapper_; diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 82052991..97c1a446 100644 --- a/src/slam_toolbox_common.cpp +++ b/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(); @@ -155,14 +156,14 @@ void SlamToolbox::setParams() enable_interactive_mode_ = this->declare_parameter("enable_interactive_mode", enable_interactive_mode_); - enable_continuous_matching_ = false; - enable_continuous_matching_ = this->declare_parameter("enable_continuous_matching", enable_continuous_matching_); 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); @@ -546,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); @@ -553,6 +556,9 @@ LocalizedRangeScan * SlamToolbox::addScan( // Add the localized range scan to the smapper 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; @@ -561,10 +567,14 @@ LocalizedRangeScan * SlamToolbox::addScan( if (processor_type_ == PROCESS) { processed = smapper_->getMapper()->Process(range_scan, &covariance); - // if continuos mapping is enabled, force a scan match without adding it to - // the graph or scan buffer - if (!processed && enable_continuous_matching_) { - match_only = true; + // 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) { @@ -594,6 +604,7 @@ LocalizedRangeScan * SlamToolbox::addScan( // if successfully processed, create odom to map transformation // and add our scan to storage if (processed) { + last_match_time = scan->header.stamp; if (enable_interactive_mode_ && !match_only) { scan_holder_->addScan(*scan); }