From ae1bdd32463a71bcee24e3717018f64816b75dcc Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 1 Mar 2021 16:47:26 -0800 Subject: [PATCH] adding reset can buffer on relocalization update --- include/slam_toolbox/slam_mapper.hpp | 2 + .../slam_toolbox_localization.hpp | 6 +++ lib/karto_sdk/include/karto_sdk/Mapper.h | 13 +++++ lib/karto_sdk/src/Mapper.cpp | 54 +++++++++++++++++++ src/slam_mapper.cpp | 7 +++ src/slam_toolbox_localization.cpp | 19 +++++++ 6 files changed, 101 insertions(+) diff --git a/include/slam_toolbox/slam_mapper.hpp b/include/slam_toolbox/slam_mapper.hpp index 7499de4c7..869a632ef 100644 --- a/include/slam_toolbox/slam_mapper.hpp +++ b/include/slam_toolbox/slam_mapper.hpp @@ -56,6 +56,8 @@ class SMapper void setMapper(karto::Mapper * mapper); karto::Mapper * getMapper(); + void clearLocalizationBuffer(); + protected: std::unique_ptr mapper_; }; diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index 711627282..36452a430 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -21,6 +21,7 @@ #include #include "slam_toolbox/slam_toolbox_common.hpp" +#include "std_srvs/srv/empty.hpp" namespace slam_toolbox { @@ -37,6 +38,10 @@ class LocalizationSlamToolbox : public SlamToolbox sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; void localizePoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + bool clearLocalizationBuffer( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr resp); bool serializePoseGraphCallback( const std::shared_ptr request_header, @@ -54,6 +59,7 @@ class LocalizationSlamToolbox : public SlamToolbox std::shared_ptr> localization_pose_sub_; + std::shared_ptr > clear_localization_; }; } // namespace slam_toolbox diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 3ef47a996..28e341d44 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1603,6 +1603,18 @@ class KARTO_EXPORT MapperSensorManager */ void SetLastScan(LocalizedRangeScan * pScan); + /** + * Clears the laser scan of device + * @param pScan + */ + void ClearLastScan(LocalizedRangeScan* pScan); + + /** + * Clears the laser scan of device name + * @param pScan + */ + void ClearLastScan(const Name& name); + /** * Gets the scan with the given unique id * @param id @@ -1965,6 +1977,7 @@ class KARTO_EXPORT Mapper : public Module kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan); kt_bool ProcessLocalization(LocalizedRangeScan * pScan); kt_bool RemoveNodeFromGraph(Vertex *); + void ClearLocalizationBuffer(); /** * Returns all processed scans added to the mapper. diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 4efcd77b1..78a9c57d1 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -112,6 +112,15 @@ class ScanManager return m_pLastScan; } + /** + * Clears last scan + * @param deviceId + */ + inline void ClearLastScan() + { + m_pLastScan = NULL; + } + /** * Sets the last scan * @param pScan @@ -287,6 +296,24 @@ void MapperSensorManager::SetLastScan(LocalizedRangeScan * pScan) GetScanManager(pScan)->SetLastScan(pScan); } +/** + * Clears the last scan of device of given scan + * @param pScan + */ +void MapperSensorManager::ClearLastScan(LocalizedRangeScan* pScan) +{ + GetScanManager(pScan)->ClearLastScan(); +} + +/** + * Clears the last scan of device name + * @param pScan + */ +void MapperSensorManager::ClearLastScan(const Name& name) +{ + GetScanManager(name)->ClearLastScan(); +} + /** * Adds scan to scan vector of device that recorded scan * @param pScan @@ -2819,6 +2846,33 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan) return true; } +void Mapper::ClearLocalizationBuffer() +{ + while (!m_LocalizationScanVertices.empty()) + { + LocalizationScanVertex& oldLSV = m_LocalizationScanVertices.front(); + RemoveNodeFromGraph(oldLSV.vertex); + oldLSV.vertex->RemoveObject(); + m_pMapperSensorManager->RemoveScan(oldLSV.scan); + if (oldLSV.scan) + { + delete oldLSV.scan; + oldLSV.scan = NULL; + } + + m_LocalizationScanVertices.pop(); + } + + std::vector names = m_pMapperSensorManager->GetSensorNames(); + for (uint i = 0; i != names.size(); i++) + { + m_pMapperSensorManager->ClearRunningScans(names[i]); + m_pMapperSensorManager->ClearLastScan(names[i]); + } + + return; +} + kt_bool Mapper::RemoveNodeFromGraph(Vertex * vertex_to_remove) { // 1) delete edges in adjacent vertices, graph, and optimizer diff --git a/src/slam_mapper.cpp b/src/slam_mapper.cpp index a315fd342..bcc5c5edf 100644 --- a/src/slam_mapper.cpp +++ b/src/slam_mapper.cpp @@ -52,6 +52,13 @@ void SMapper::setMapper(karto::Mapper * mapper) mapper_.reset(mapper); } +/*****************************************************************************/ +void SMapper::clearLocalizationBuffer() +/*****************************************************************************/ +{ + mapper_->ClearLocalizationBuffer(); +} + /*****************************************************************************/ karto::OccupancyGrid * SMapper::getOccupancyGrid(const double & resolution) /*****************************************************************************/ diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index f577a30d6..9c8947544 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -34,6 +34,10 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) "/initialpose", 1, std::bind(&LocalizationSlamToolbox::localizePoseCallback, this, std::placeholders::_1)); + clear_localization_ = this->create_service( + "clear_localization_buffer", + std::bind(&LocalizationSlamToolbox::clearLocalizationBuffer, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // in localization mode, we cannot allow for interactive mode enable_interactive_mode_ = false; @@ -68,6 +72,19 @@ void LocalizationSlamToolbox::loadPoseGraphByParams() } } +/*****************************************************************************/ +bool LocalizationSlamToolbox::clearLocalizationBuffer( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr resp) +/*****************************************************************************/ +{ + RCLCPP_INFO(get_logger(), + "LocalizationSlamToolbox: Clearing localization buffer."); + smapper_->clearLocalizationBuffer(); + return true; +} + /*****************************************************************************/ bool LocalizationSlamToolbox::serializePoseGraphCallback( const std::shared_ptr request_header, @@ -203,6 +220,8 @@ void LocalizationSlamToolbox::localizePoseCallback( first_measurement_ = true; + smapper_->clearLocalizationBuffer(); + RCLCPP_INFO(get_logger(), "LocalizePoseCallback: Localizing to: (%0.2f %0.2f), theta=%0.2f", msg->pose.pose.position.x, msg->pose.pose.position.y,