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} 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/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_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 77ac6b11..56fb9592 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" @@ -40,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(); @@ -176,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); @@ -506,9 +504,9 @@ 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; static int scan_ctr = 0; scan_ctr++; @@ -530,21 +528,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; } - - // check moved enough, within 10% for correction error - const double dist2 = last_pose.SquaredDistance(pose); - if (dist2 < 0.8 * min_dist2 || scan_ctr < 5) { + else{ return false; } - - last_pose = pose; - last_scan_time = scan->header.stamp; - - return true; } /*****************************************************************************/