From 949872da89464f8c134d5d62f74156e563be2f80 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Tue, 18 Jun 2024 09:07:28 +0200 Subject: [PATCH 1/4] fixed laser scan not updating on heading change --- src/slam_toolbox_common.cpp | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 77ac6b11..0dc49240 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "slam_toolbox/slam_toolbox_common.hpp" #include "slam_toolbox/serialization.hpp" @@ -506,9 +507,11 @@ bool SlamToolbox::shouldProcessScan( { static Pose2 last_pose; static rclcpp::Time last_scan_time = rclcpp::Time(0.); - static double min_dist2 = - smapper_->getMapper()->getParamMinimumTravelDistance() * - smapper_->getMapper()->getParamMinimumTravelDistance(); + static double min_dist = smapper_->getMapper()->getParamMinimumTravelDistance(); + static double min_angle = + smapper_->getMapper()->getParamMinimumTravelHeading() /*IN DEGREES! needs conversion to rad: */ * M_PI/180; + + //double min_angle = 0.05; static int scan_ctr = 0; scan_ctr++; @@ -535,12 +538,20 @@ bool SlamToolbox::shouldProcessScan( return false; } - // check moved enough, within 10% for correction error - const double dist2 = last_pose.SquaredDistance(pose); - if (dist2 < 0.8 * min_dist2 || scan_ctr < 5) { + const double deltaHeading = math::NormalizeAngle( + pose.GetHeading() - last_pose.GetHeading()); + const double squaredTravelDistance = last_pose.GetPosition().SquaredDistance( + pose.GetPosition()); + // return only if we both havent turned enough or havent moved enough + if (squaredTravelDistance <= math::Square(min_dist) - KT_TOLERANCE && + fabs(deltaHeading) <= min_angle) { return false; } + std::cout << "---" << scan_ctr << "---" << std::endl; + std::cout << "deltaHeading: " << deltaHeading << std::endl; + std::cout << "squaredTravelDistance:" << squaredTravelDistance << std::endl; + last_pose = pose; last_scan_time = scan->header.stamp; From abab8aa5204c7de52a06bcdfff01c77059f2a9ec Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Tue, 18 Jun 2024 11:42:09 +0200 Subject: [PATCH 2/4] refactor laserCallback so it uses pose and time checks implemented in Mapper.h --- lib/karto_sdk/include/karto_sdk/Mapper.h | 26 +++++++++-- lib/karto_sdk/src/Mapper.cpp | 58 ++++++++++++++++-------- src/slam_toolbox_common.cpp | 32 ++++--------- 3 files changed, 68 insertions(+), 48 deletions(-) diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index d8256b2a..b746c7ee 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -2091,17 +2091,33 @@ class KARTO_EXPORT Mapper : public Module return m_LocalizationScanVertices; } -protected: - void InitializeParameters(); + /** + * Test if the enough time has passed since the last processed scan. + * @param scanTime scan to be checked + * @param lastScanTime last scan added to mapper + * @return true if the minimum alloted time has passed + */ + kt_bool EnoughTimeHasPassed(const kt_double& scanTime, const kt_double& lastScanTime) const; /** * Test if the scan is "sufficiently far" from the last scan added. + * @param scannerPose scanner pose to be checked + * @param lastScannerPose pose of the last scan added to mapper + * @return true if the scan is "sufficiently far" from the last scan added + */ + kt_bool HasMovedEnough(const Pose2& scannerPose, const Pose2& lastScannerPose) const; + + /** + * See if we should continue processing this scan. * @param pScan scan to be checked * @param pLastScan last scan added to mapper - * @return true if the scan is "sufficiently far" from the last scan added or - * the scan is the first scan to be added + * @return true if it is the first scan, if enough time has passed since the last one or + * the scan pose has moved enough. */ - kt_bool HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * pLastScan) const; + kt_bool ContinueProcessingScan(LocalizedRangeScan * pScan, LocalizedRangeScan * pLastScan) const; + +protected: + void InitializeParameters(); public: ///////////////////////////////////////////// diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 83e2af2c..e7fae714 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -2700,8 +2700,7 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose())); } - // test if scan is outside minimum boundary or if heading is larger then minimum heading - if (!HasMovedEnough(pScan, pLastScan)) { + if(!ContinueProcessingScan(pScan, pLastScan)){ return false; } @@ -2860,9 +2859,7 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covari pScan->GetOdometricPose())); } - // test if scan is outside minimum boundary - // or if heading is larger then minimum heading - if (!HasMovedEnough(pScan, pLastScan)) { + if(!ContinueProcessingScan(pScan, pLastScan)){ return false; } @@ -3102,27 +3099,47 @@ kt_bool Mapper::ProcessAtDock(LocalizedRangeScan * pScan, Matrix3 * covariance) } /** - * Is the scan sufficiently far from the last scan? - * @param pScan - * @param pLastScan - * @return true if the scans are sufficiently far - */ -kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * pLastScan) const +* See if we should continue processing this scan. +* @param pScan scan to be checked +* @param pLastScan last scan added to mapper +* @return true if it is the first scan, if enough time has passed since the last one or +* the scan pose has moved enough. +*/ +kt_bool Mapper::ContinueProcessingScan(LocalizedRangeScan * pScan, LocalizedRangeScan * pLastScan) const +{ + if(pLastScan == NULL) return true; + if( + EnoughTimeHasPassed(pScan->GetTime(), pLastScan->GetTime()) || + HasMovedEnough(pScan->GetOdometricPose(), pLastScan->GetOdometricPose()) + ){ + return true; + } + return false; +} +/** +* Test if the enough time has passed since the last processed scan. +* @param pScan scan to be checked +* @param pLastScan last scan added to mapper +* @return true if the minimum alloted time has passed +*/ +kt_bool Mapper::EnoughTimeHasPassed(const kt_double& scanTime, const kt_double& lastScanTime) const { - // test if first scan - if (pLastScan == NULL) { - return true; - } - // test if enough time has passed - kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime(); + kt_double timeInterval = scanTime - lastScanTime; if (timeInterval >= m_pMinimumTimeInterval->GetValue()) { return true; } + return false; +} - Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose()); - Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose()); - +/** +* Test if the scan is "sufficiently far" from the last scan added. +* @param scannerPose scanner pose to be checked +* @param lastScannerPose pose of the last scan added to mapper +* @return true if the scan is "sufficiently far" from the last scan added +*/ +kt_bool Mapper::HasMovedEnough(const Pose2& scannerPose, const Pose2& lastScannerPose) const +{ // test if we have turned enough kt_double deltaHeading = math::NormalizeAngle( scannerPose.GetHeading() - lastScannerPose.GetHeading()); @@ -3140,6 +3157,7 @@ kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * return false; } + /** * Gets all the processed scans * @return all scans diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 0dc49240..3f62b993 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -509,9 +509,7 @@ bool SlamToolbox::shouldProcessScan( static rclcpp::Time last_scan_time = rclcpp::Time(0.); static double min_dist = smapper_->getMapper()->getParamMinimumTravelDistance(); static double min_angle = - smapper_->getMapper()->getParamMinimumTravelHeading() /*IN DEGREES! needs conversion to rad: */ * M_PI/180; - - //double min_angle = 0.05; + smapper_->getMapper()->getParamMinimumTravelHeading() /*in degrees, needs conversion to rad: */ * M_PI/180; static int scan_ctr = 0; scan_ctr++; @@ -533,29 +531,17 @@ bool SlamToolbox::shouldProcessScan( return false; } - // not enough time - if (rclcpp::Time(scan->header.stamp) - last_scan_time < minimum_time_interval_) { - return false; + if( + smapper_->getMapper()->EnoughTimeHasPassed(rclcpp::Time(scan->header.stamp).seconds(), last_scan_time.seconds()) || + smapper_->getMapper()->HasMovedEnough(pose, last_pose) + ){ + last_pose = pose; + last_scan_time = scan->header.stamp; + return true; } - - const double deltaHeading = math::NormalizeAngle( - pose.GetHeading() - last_pose.GetHeading()); - const double squaredTravelDistance = last_pose.GetPosition().SquaredDistance( - pose.GetPosition()); - // return only if we both havent turned enough or havent moved enough - if (squaredTravelDistance <= math::Square(min_dist) - KT_TOLERANCE && - fabs(deltaHeading) <= min_angle) { + else{ return false; } - - std::cout << "---" << scan_ctr << "---" << std::endl; - std::cout << "deltaHeading: " << deltaHeading << std::endl; - std::cout << "squaredTravelDistance:" << squaredTravelDistance << std::endl; - - last_pose = pose; - last_scan_time = scan->header.stamp; - - return true; } /*****************************************************************************/ From f074eb0e3fc1636ac59cfeb2cdfeb1c324822ec2 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Tue, 18 Jun 2024 11:54:06 +0200 Subject: [PATCH 3/4] enable setting of minimum_time_interval parameter --- include/slam_toolbox/slam_toolbox_common.hpp | 2 +- src/slam_mapper.cpp | 7 +++++++ src/slam_toolbox_common.cpp | 5 +---- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 4e3a5b85..3885d367 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -143,7 +143,7 @@ class SlamToolbox : public rclcpp::Node // Storage for ROS parameters std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_; bool use_map_saver_; - rclcpp::Duration transform_timeout_, minimum_time_interval_; + rclcpp::Duration transform_timeout_; std_msgs::msg::Header scan_header; int throttle_scans_, scan_queue_size_; diff --git a/src/slam_mapper.cpp b/src/slam_mapper.cpp index 79dfa2ff..de18399a 100644 --- a/src/slam_mapper.cpp +++ b/src/slam_mapper.cpp @@ -110,6 +110,13 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node) node->get_parameter("use_scan_barycenter", use_scan_barycenter); mapper_->setParamUseScanBarycenter(use_scan_barycenter); + double minimum_time_interval = 3600; + if (!node->has_parameter("minimum_time_interval")) { + node->declare_parameter("minimum_time_interval", minimum_time_interval); + } + node->get_parameter("minimum_time_interval", minimum_time_interval); + mapper_->setParamMinimumTimeInterval(minimum_time_interval); + double minimum_travel_distance = 0.5; if (!node->has_parameter("minimum_travel_distance")) { node->declare_parameter("minimum_travel_distance", minimum_travel_distance); diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 3f62b993..56fb9592 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -41,8 +41,7 @@ SlamToolbox::SlamToolbox(rclcpp::NodeOptions options) processor_type_(PROCESS), first_measurement_(true), process_near_pose_(nullptr), - transform_timeout_(rclcpp::Duration::from_seconds(0.5)), - minimum_time_interval_(std::chrono::nanoseconds(0)) + transform_timeout_(rclcpp::Duration::from_seconds(0.5)) /*****************************************************************************/ { smapper_ = std::make_unique(); @@ -177,8 +176,6 @@ void SlamToolbox::setParams() 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); bool debug = false; debug = this->declare_parameter("debug_logging", debug); From e976ca44faa50f74d6eac459bcc64f56dbbb6a78 Mon Sep 17 00:00:00 2001 From: MAHA Maia Date: Wed, 17 Jul 2024 13:32:31 +0200 Subject: [PATCH 4/4] Install 'lifelong' node. --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index aab4ab8b..7bb27edf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -199,6 +199,7 @@ rclcpp_components_register_nodes(map_and_localization_slam_toolbox "slam_toolbox install(TARGETS async_slam_toolbox_node sync_slam_toolbox_node localization_slam_toolbox_node + lifelong_slam_toolbox_node map_and_localization_slam_toolbox_node merge_maps_kinematic ${libraries}