diff --git a/README.md b/README.md index 8ad0af856..712cb650c 100644 --- a/README.md +++ b/README.md @@ -248,13 +248,15 @@ 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 @@ -262,7 +264,7 @@ 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 +`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 diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 7d352e8c5..97480c7bc 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_; diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 379fa8e23..969b197e6 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/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 diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 4686efd05..735840fc0 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/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 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 c65e9d716..97c1a4464 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,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); }