From 23794aea23e2466bd8c24d37e3b9c0c59e877f88 Mon Sep 17 00:00:00 2001 From: baha Date: Wed, 10 Jul 2024 09:59:24 +0300 Subject: [PATCH 01/79] feat: Add publish_map_once_ and update_map_once_ flags for conditional map update --- include/slam_toolbox/slam_toolbox_common.hpp | 1 + src/slam_toolbox_common.cpp | 12 +++++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 4e3a5b85f..15dd0e517 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_; bool use_map_saver_; + bool publish_map_once_, update_map_once_; rclcpp::Duration transform_timeout_, minimum_time_interval_; std_msgs::msg::Header scan_header; int throttle_scans_, scan_queue_size_; diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 77ac6b113..363d38408 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -287,10 +287,20 @@ void SlamToolbox::publishVisualizations() double map_update_interval = 10; map_update_interval = this->declare_parameter("map_update_interval", map_update_interval); + publish_map_once_ = this->declare_parameter("publish_map_once", + publish_map_once_); rclcpp::Rate r(1.0 / map_update_interval); while (rclcpp::ok()) { - updateMap(); + if(publish_map_once_){ + if(update_map_once_){ + updateMap(); + update_map_once_ = false; + } + } + else{ + updateMap(); + } if (!isPaused(VISUALIZING_GRAPH)) { boost::mutex::scoped_lock lock(smapper_mutex_); closure_assistant_->publishGraph(); From 0c406e9e02e08907830b2e37e2a902bf2f55dbd1 Mon Sep 17 00:00:00 2001 From: baha Date: Thu, 11 Jul 2024 10:35:35 +0300 Subject: [PATCH 02/79] Refactor slam_toolbox_common.hpp to initialize update_map_once_ flag to true --- include/slam_toolbox/slam_toolbox_common.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 15dd0e517..a1c1fbf39 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_; - bool publish_map_once_, update_map_once_; + bool publish_map_once_, update_map_once_ = true; rclcpp::Duration transform_timeout_, minimum_time_interval_; std_msgs::msg::Header scan_header; int throttle_scans_, scan_queue_size_; From dc93d6f6bac9aba4b47afebb7a1a7425804df8bd Mon Sep 17 00:00:00 2001 From: baha Date: Tue, 16 Jul 2024 08:52:17 +0300 Subject: [PATCH 03/79] debugging version --- include/slam_toolbox/slam_toolbox_common.hpp | 2 +- lib/karto_sdk/src/Mapper.cpp | 108 ++++++++++++++++++- src/slam_toolbox_common.cpp | 12 +++ 3 files changed, 117 insertions(+), 5 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index a1c1fbf39..089f8ebf1 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_; - bool publish_map_once_, update_map_once_ = true; + bool update_map_once_, publish_map_once_= true; rclcpp::Duration transform_timeout_, minimum_time_interval_; std_msgs::msg::Header scan_header; int throttle_scans_, scan_queue_size_; diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 83e2af2ca..2c1c0e25d 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -47,7 +47,7 @@ namespace karto { // enable this for verbose debug information -// #define KARTO_DEBUG +#define KARTO_DEBUG #define MAX_VARIANCE 500.0 #define DISTANCE_PENALTY_GAIN 0.2 @@ -536,6 +536,11 @@ kt_double ScanMatcher::MatchScan( LocalizedRangeScan * pScan, const T & rBaseScans, Pose2 & rMean, Matrix3 & rCovariance, kt_bool doPenalize, kt_bool doRefineMatch) { + + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); + + std::cout << "Doing MatchScan" << std::endl; /////////////////////////////////////// // set scan pose to be center of grid @@ -558,7 +563,7 @@ kt_double ScanMatcher::MatchScan( // 2. get size of grid Rectangle2 roi = m_pCorrelationGrid->GetROI(); - + // 3. compute offset (in meters - lower left corner) Vector2 offset; offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * m_pCorrelationGrid->GetResolution())); @@ -635,6 +640,14 @@ kt_double ScanMatcher::MatchScan( #endif assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI)); + + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::time_t end_time = std::chrono::system_clock::to_time_t(end); + + std::cout << "-------finished computation of MATCHSCAN at " << std::ctime(&end_time) + << "--------elapsed time: " << elapsed_seconds.count() << "s\n"; + return bestResponse; } @@ -706,7 +719,7 @@ void ScanMatcher::operator()(const kt_double & y) const * @param doPenalize whether to penalize matches further from the search center * @param rMean output parameter of mean (best pose) of match * @param rCovariance output parameter of covariance of match - * @param doingFineMatch whether to do a finer search after coarse search + * @param do-ingFineMatch whether to do a finer search after coarse search * @return strength of response */ kt_double ScanMatcher::CorrelateScan( @@ -1499,15 +1512,21 @@ void MapperGraph::AddEdges(LocalizedRangeScan * pScan, const Matrix3 & rCovarian kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSensorName) { + std::cout << "Inside of TryCloseLoop: " << std::endl; kt_bool loopClosed = false; kt_int32u scanIndex = 0; LocalizedRangeScanVector candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); + std::cout << "TryCloseLoop candidateChain size: " << candidateChain.size() << std::endl; + while (!candidateChain.empty()) { + std::cout << "Checking of the candidatecChainsize inside of while: " << candidateChain.size() << std::endl; + Pose2 bestPose; Matrix3 covariance; + std::cout << "-----------------the next scan match is from trycloseloop------------" << std::endl; kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false, false); @@ -1518,6 +1537,9 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1) << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")"; + std::cout << "Coarse response: " << coarseResponse << std::endl; + std::cout << "Coarse var: " << covariance(0, 0) << ", " << covariance(1, 1) << std::endl; + m_pMapper->FireLoopClosureCheck(stream.str()); if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) && @@ -1553,8 +1575,9 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens loopClosed = true; } } - + std::cout << "-----------------TryCloseLoop loopClosed: " << loopClosed << std::endl; candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); + std::cout << "----------candidateChain" << candidateChain.size() << std::endl; } return loopClosed; @@ -1641,6 +1664,7 @@ void MapperGraph::LinkNearChains( std::vector & rCovariances) { const std::vector nearChains = FindNearChains(pScan); + const_forEach(std::vector, &nearChains) { if (iter->size() < m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) { @@ -1650,8 +1674,22 @@ void MapperGraph::LinkNearChains( Pose2 mean; Matrix3 covariance; // match scan against "near" chain + using namespace std::chrono; + + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); + kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false); + + + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::time_t end_time = std::chrono::system_clock::to_time_t(end); + + std::cout << "----------finished computation at LinkNearCHains " << std::ctime(&end_time) + << "---------elapsed time: " << elapsed_seconds.count() << "s\n"; + if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE) { rMeans.push_back(mean); rCovariances.push_back(covariance); @@ -1962,6 +2000,7 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( const Name & rSensorName, kt_int32u & rStartNum) { + LocalizedRangeScanVector chain; // return value Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); @@ -1971,8 +2010,18 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue()); + std::cout << "FindPossibleLoopClosure nearLinkedScans size: " << nearLinkedScans.size() << std::endl; + std::cout << "Near Linked Scans: " << nearLinkedScans.size() << std::endl; + + kt_int32u nScans = static_cast(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size()); + + std::cout << "nScans value " << nScans << std::endl; + + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); + for (; rStartNum < nScans; rStartNum++) { LocalizedRangeScan * pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum); @@ -2002,10 +2051,18 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( return chain; } else { chain.clear(); + } } } + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::time_t end_time = std::chrono::system_clock::to_time_t(end); + + std::cout << "finished computation of nscans at " << std::ctime(&end_time) + << "--------elapsed time: " << elapsed_seconds.count() << "s\n"; + std::cout << "chain" << chain.size() << std::endl; return chain; } @@ -2711,10 +2768,19 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) // correct scan (if not first scan) if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { Pose2 bestPose; + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); + m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::time_t end_time = std::chrono::system_clock::to_time_t(end); + + std::cout << "----------finished computation at HAsMovedEnough " << std::ctime(&end_time) + << "---------elapsed time: " << elapsed_seconds.count() << "s\n"; pScan->SetSensorPose(bestPose); if (covariance) { *covariance = cov; @@ -2726,6 +2792,7 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) if (m_pUseScanMatching->GetValue()) { // add to graph + m_pGraph->AddVertex(pScan); m_pGraph->AddEdges(pScan, cov); @@ -2782,10 +2849,19 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad // correct scan (if not first scan) if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { Pose2 bestPose; + + std::chrono::time_point start3, end3; + start3 = std::chrono::system_clock::now(); m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); + end3 = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds3 = end3 - start3; + std::time_t end_time3 = std::chrono::system_clock::to_time_t(end3); + + std::cout << "----------finished computation at ProcessAgainstNodesNearBy " << std::ctime(&end_time3) + << "---------elapsed time: " << elapsed_seconds3.count() << "s\n"; pScan->SetSensorPose(bestPose); } @@ -2872,10 +2948,21 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covari // correct scan (if not first scan) if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { Pose2 bestPose; + + std::chrono::time_point start2, end2; + start2 = std::chrono::system_clock::now(); + m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); + + end2 = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds2 = end2 - start2; + std::time_t end_time2 = std::chrono::system_clock::to_time_t(end2); + + std::cout << "----------finished computation at ProcessLocalization " << std::ctime(&end_time2) + << "---------elapsed time: " << elapsed_seconds2.count() << "s\n"; pScan->SetSensorPose(bestPose); if (covariance) { *covariance = cov; @@ -2888,6 +2975,8 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covari Vertex * scan_vertex = NULL; if (m_pUseScanMatching->GetValue()) { // add to graph + // TODO: Testing here baha + scan_vertex = m_pGraph->AddVertex(pScan); m_pGraph->AddEdges(pScan, cov); @@ -3055,10 +3144,21 @@ kt_bool Mapper::ProcessAgainstNode( // correct scan (if not first scan) if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { Pose2 bestPose; + + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); + m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); + + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::time_t end_time = std::chrono::system_clock::to_time_t(end); + std::cout << "----------finished computation at ProcessAgainstNode " << std::ctime(&end_time) + << "---------elapsed time: " << elapsed_seconds.count() << "s\n"; + pScan->SetSensorPose(bestPose); } diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 363d38408..b44572983 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -255,6 +255,12 @@ void SlamToolbox::publishTransformLoop( { boost::mutex::scoped_lock lock(map_to_odom_mutex_); rclcpp::Time scan_timestamp = scan_header.stamp; + + auto current_time = this->now(); + auto last_scan_time = scan_header.stamp; + + // RCLCPP_INFO(get_logger(), "pasted_time_last_recieved_scan_data = %f", (current_time - last_scan_time).seconds() ); + // std::cout << "pasted_time_last_recieved_scan_data" << (current_time - last_scan_time).seconds()<< std::endl; // Avoid publishing tf with initial 0.0 scan timestamp if (scan_timestamp.seconds() > 0.0 && !scan_header.frame_id.empty()) { geometry_msgs::msg::TransformStamped msg; @@ -292,6 +298,8 @@ void SlamToolbox::publishVisualizations() rclcpp::Rate r(1.0 / map_update_interval); while (rclcpp::ok()) { + auto before_update_map = this->now(); + if(publish_map_once_){ if(update_map_once_){ updateMap(); @@ -301,6 +309,10 @@ void SlamToolbox::publishVisualizations() else{ updateMap(); } + auto after_update_map = this->now(); + auto time_diff = (after_update_map - before_update_map).seconds(); + // RCLCPP_ERROR(get_logger(),"Pasted time in updateMap function inside PublishVisualization %f",time_diff); + // std::cout << "finished computation in updateMap " << time_diff<< std::endl; if (!isPaused(VISUALIZING_GRAPH)) { boost::mutex::scoped_lock lock(smapper_mutex_); closure_assistant_->publishGraph(); From fcfb891313dc49c420f65460389a7233f6544624 Mon Sep 17 00:00:00 2001 From: baha Date: Mon, 22 Jul 2024 08:44:35 +0300 Subject: [PATCH 04/79] testing for localization health --- lib/karto_sdk/include/karto_sdk/Mapper.h | 5 +++ lib/karto_sdk/src/Mapper.cpp | 40 ++++++++++++++++++++++-- 2 files changed, 43 insertions(+), 2 deletions(-) diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index d8256b2a1..82c50e03f 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1961,6 +1961,11 @@ class KARTO_EXPORT Mapper : public Module virtual ~Mapper(); public: + + double * m_pbestResponse; + double * GetBestResponse(); + void SetBestResponse(double * pBestResponse); + /** * Allocate memory needed for mapping * @param rangeThreshold diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 2c1c0e25d..03c1c0399 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -719,7 +719,7 @@ void ScanMatcher::operator()(const kt_double & y) const * @param doPenalize whether to penalize matches further from the search center * @param rMean output parameter of mean (best pose) of match * @param rCovariance output parameter of covariance of match - * @param do-ingFineMatch whether to do a finer search after coarse search + * @param doingFineMatch whether to do a finer search after coarse search * @return strength of response */ kt_double ScanMatcher::CorrelateScan( @@ -1539,9 +1539,20 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens std::cout << "Coarse response: " << coarseResponse << std::endl; std::cout << "Coarse var: " << covariance(0, 0) << ", " << covariance(1, 1) << std::endl; - + m_pMapper->FireLoopClosureCheck(stream.str()); + double coarsedResponse = coarseResponse; + try + { + std::cout << "---------------- OKKAYYYY-----------"<< std::endl; + m_pMapper->SetBestResponse(&coarsedResponse); + std::cout << "---------------- OKKAYYYY-----------"<< std::endl; + } + catch (std::exception & e) { + std::cout << "Exception caught: " << e.what() << std::endl; + } + if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) && (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) && (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())) @@ -1556,6 +1567,21 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens candidateChain, bestPose, covariance, false); + std::cout << "--------------fineResponseeeeeeeeeeeee----------" << fineResponse << std::endl; + + double finedResponse = fineResponse; + std::cout << "--------------finedResponse----------" << finedResponse << std::endl; + + try + { + std::cout << "---------------- OKKAYYYY-----------"<< std::endl; + m_pMapper->SetBestResponse(&finedResponse); + std::cout << "---------------- OKKAYYYY-----------"<< std::endl; + } + catch (std::exception & e) { + std::cout << "Exception caught: " << e.what() << std::endl; + } + std::stringstream stream1; stream1 << "FINE RESPONSE: " << fineResponse << " (>" << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl; @@ -3333,6 +3359,16 @@ void Mapper::FireEndLoopClosure(const std::string & rInfo) const } } +double * Mapper::GetBestResponse() +{ + return m_pbestResponse; +} + +void Mapper::SetBestResponse(double * pBestResponse) +{ + m_pbestResponse = pBestResponse; +} + void Mapper::SetScanSolver(ScanSolver * pScanOptimizer) { m_pScanOptimizer = pScanOptimizer; From 226c861e2699c9af155b00b1368af384b7d2ee95 Mon Sep 17 00:00:00 2001 From: baha Date: Wed, 24 Jul 2024 09:31:49 +0300 Subject: [PATCH 05/79] set and get the best response from matchscan output for localization health information --- lib/karto_sdk/include/karto_sdk/Mapper.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index d8256b2a1..d53d6416d 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1961,6 +1961,14 @@ class KARTO_EXPORT Mapper : public Module virtual ~Mapper(); public: + + /** + * Purpose of this function is set the best response to localization health information + */ + double * m_pbestResponse; + double * GetBestResponse(); + void SetBestResponse(double * pBestResponse); + /** * Allocate memory needed for mapping * @param rangeThreshold From 2411f91990321046d4010b5fb3b8caddbd8aedde Mon Sep 17 00:00:00 2001 From: baha Date: Wed, 24 Jul 2024 09:33:09 +0300 Subject: [PATCH 06/79] Refactor Mapper.cpp to initialize m_pbestResponse in the constructor --- lib/karto_sdk/src/Mapper.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 83e2af2ca..06ca65bc8 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -2055,7 +2055,8 @@ Mapper::Mapper() m_pSequentialScanMatcher(NULL), m_pMapperSensorManager(NULL), m_pGraph(NULL), - m_pScanOptimizer(NULL) + m_pScanOptimizer(NULL), + m_pbestResponse(new double(0.0)) { InitializeParameters(); } @@ -2070,7 +2071,8 @@ Mapper::Mapper(const std::string & rName) m_pSequentialScanMatcher(NULL), m_pMapperSensorManager(NULL), m_pGraph(NULL), - m_pScanOptimizer(NULL) + m_pScanOptimizer(NULL), + m_pbestResponse(new double(0.0)) { InitializeParameters(); } @@ -2664,6 +2666,10 @@ void Mapper::Reset() delete m_pMapperSensorManager; m_pMapperSensorManager = NULL; } + if (m_pbestResponse) { + delete m_pbestResponse; + m_pbestResponse = NULL; + } m_Initialized = false; m_Deserialized = false; while (!m_LocalizationScanVertices.empty()) { From 346f78fec1198b18144bc2bcef2ec07ca818ac86 Mon Sep 17 00:00:00 2001 From: baha Date: Wed, 24 Jul 2024 09:34:07 +0300 Subject: [PATCH 07/79] Refactor Mapper.cpp to initialize m_pbestResponse in the constructor --- lib/karto_sdk/src/Mapper.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 06ca65bc8..2f6767efc 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -3239,6 +3239,18 @@ void Mapper::FireEndLoopClosure(const std::string & rInfo) const } } +double* Mapper::GetBestResponse() +{ + return m_pbestResponse; +} + +void Mapper::SetBestResponse(double* pBestResponse) +{ + if (pBestResponse != nullptr) { + *m_pbestResponse = *pBestResponse; + } +} + void Mapper::SetScanSolver(ScanSolver * pScanOptimizer) { m_pScanOptimizer = pScanOptimizer; From 30957789220290a992d4823717dbd1b476626d3c Mon Sep 17 00:00:00 2001 From: baha Date: Wed, 24 Jul 2024 09:40:09 +0300 Subject: [PATCH 08/79] feat: Add localization health check feature The code changes in `slam_toolbox_common.cpp` add a new feature for performing a localization health check. The `SlamToolbox::publishTransformLoop` function now checks the `best_response` value and publishes a topic based on the result. --- src/slam_toolbox_common.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 363d38408..da07eb188 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -255,6 +255,18 @@ void SlamToolbox::publishTransformLoop( { boost::mutex::scoped_lock lock(map_to_odom_mutex_); rclcpp::Time scan_timestamp = scan_header.stamp; + + //TODO: In future, we need to remove from there and create new function named as localizationHealthCheck. + double * best_response =smapper_->getMapper()->GetBestResponse(); + if (best_response != nullptr && *best_response > 0.02) { + try { + //TODO: Write publisher topic for localization health check. + } catch (std::exception & e) { + //TODO: Write publisher topic when cannot acces the result of health check . + } + } + // + // Avoid publishing tf with initial 0.0 scan timestamp if (scan_timestamp.seconds() > 0.0 && !scan_header.frame_id.empty()) { geometry_msgs::msg::TransformStamped msg; From 0cb09ffb14dcae0deb5a23292e16bfc830754322 Mon Sep 17 00:00:00 2001 From: baha Date: Wed, 24 Jul 2024 10:23:50 +0300 Subject: [PATCH 09/79] Refactor Mapper.cpp to use kt_double instead of double for best_response in MatchScan function --- lib/karto_sdk/src/Mapper.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 2f6767efc..6579f74ea 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -634,6 +634,19 @@ kt_double ScanMatcher::MatchScan( rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl; #endif assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI)); + /* + * purpose of equal to best_response is; Instead of {double} type {kt_double} doesn't mean anything inside the slam_toolbox_common + */ + double best_response = bestResponse; + + try + { + m_pMapper->SetBestResponse(&best_response); + } + catch (std::exception & e) { + throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " + "unable to publish set best response!"); + } return bestResponse; } From baab128fa653d35a2e6959075217b54d6ff233a4 Mon Sep 17 00:00:00 2001 From: baha Date: Wed, 24 Jul 2024 11:28:15 +0300 Subject: [PATCH 10/79] for better debugging: For enable the KARTO_DEBUG: just oncomment 50th line. --- lib/karto_sdk/src/Mapper.cpp | 103 ++++++++++++++++++++++++++++++++++- 1 file changed, 101 insertions(+), 2 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 6579f74ea..105ce1d4b 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -536,6 +536,12 @@ kt_double ScanMatcher::MatchScan( LocalizedRangeScan * pScan, const T & rBaseScans, Pose2 & rMean, Matrix3 & rCovariance, kt_bool doPenalize, kt_bool doRefineMatch) { +#ifdef KARTO_DEBUG + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); + + std::cout << "\nDoing MatchScan\n" << std::endl; +#endif /////////////////////////////////////// // set scan pose to be center of grid @@ -634,6 +640,14 @@ kt_double ScanMatcher::MatchScan( rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl; #endif assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI)); + +#ifdef KARTO_DEBUG + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::cout << "-------finished computation of MATCHSCAN at\n" + << "-------elapsed time: " << elapsed_seconds.count() << "s\n"; +#endif + /* * purpose of equal to best_response is; Instead of {double} type {kt_double} doesn't mean anything inside the slam_toolbox_common */ @@ -1518,7 +1532,14 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens LocalizedRangeScanVector candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); +#ifdef KARTO_DEBUG + std::cout << "TryCloseLoop candidateChain size: " << candidateChain.size() << std::endl; +#endif + while (!candidateChain.empty()) { +#ifdef KARTO_DEBUG + std::cout << "Checking of the candidatecChainsize inside of Loop CLosure: " << candidateChain.size() << std::endl; +#endif Pose2 bestPose; Matrix3 covariance; kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, @@ -1566,7 +1587,9 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens loopClosed = true; } } - +#ifdef KARTO_DEBUG + std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; +#endif candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); } @@ -1662,9 +1685,25 @@ void MapperGraph::LinkNearChains( Pose2 mean; Matrix3 covariance; +#ifdef KARTO_DEBUG + using namespace std::chrono; + + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); +#endif + // match scan against "near" chain kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false); + +#ifdef KARTO_DEBUG + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::cout << "----------Calculating time of Matchscan after triggered\n" + << "----------LinkNearCHains function\n" + << "----------elapsed time: " << elapsed_seconds.count() << "s\n"; +#endif + if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE) { rMeans.push_back(mean); rCovariances.push_back(covariance); @@ -1986,6 +2025,13 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( kt_int32u nScans = static_cast(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size()); + +#ifdef KARTO_DEBUG + std::cout << "\nnScans size " << nScans << std::endl; + + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); +#endif for (; rStartNum < nScans; rStartNum++) { LocalizedRangeScan * pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum); @@ -2018,7 +2064,13 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( } } } - +#ifdef KARTO_DEBUG + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::cout <<"----------Finded chain size is" << chain.size() << std::endl; + std::cout <<"----------Calculating time of Matchscan inside nscans function\n" + <<"----------elapsed time: " << elapsed_seconds.count() << "s\n"; +#endif return chain; } @@ -2730,10 +2782,21 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) // correct scan (if not first scan) if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { Pose2 bestPose; +#ifdef KARTO_DEBUG + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); +#endif m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); +#ifdef KARTO_DEBUG + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::cout <<"----------Calculating time of Matchscan after triggered\n" + <<"----------HasMovedEnough function\n" + <<"----------elapsed time: " << elapsed_seconds.count() << "s\n"; +#endif pScan->SetSensorPose(bestPose); if (covariance) { *covariance = cov; @@ -2801,10 +2864,22 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad // correct scan (if not first scan) if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { Pose2 bestPose; +#ifdef KARTO_DEBUG + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); +#endif m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); +#ifdef KARTO_DEBUG + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + + std::cout << "----------Calculating time of Matchscan after triggered\n" + << "----------ProcessAgainstNodesNearBy function\n" + << "----------elapsed time: " << elapsed_seconds.count() << "s\n"; +#endif pScan->SetSensorPose(bestPose); } @@ -2891,10 +2966,22 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covari // correct scan (if not first scan) if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { Pose2 bestPose; +#ifdef KARTO_DEBUG + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); +#endif m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); +#ifdef KARTO_DEBUG + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + + std::cout << "----------Calculating time of Matchscan after triggered\n" + << "----------ProcessLocalization function\n" + << "----------elapsed time: " << elapsed_seconds.count() << "s\n"; +#endif pScan->SetSensorPose(bestPose); if (covariance) { *covariance = cov; @@ -3074,10 +3161,22 @@ kt_bool Mapper::ProcessAgainstNode( // correct scan (if not first scan) if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { Pose2 bestPose; +#ifdef KARTO_DEBUG + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); +#endif m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, cov); +#ifdef KARTO_DEBUG + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + + std::cout << "----------Calculating time of Matchscan after triggered\n" + << "----------ProcessAgainstNode function\n" + << "----------elapsed time: " << elapsed_seconds.count() << "s\n"; +#endif pScan->SetSensorPose(bestPose); } From 22bd4d8fdacdb6d3d3518d904b45fb1cd4f71a5c Mon Sep 17 00:00:00 2001 From: baha Date: Wed, 24 Jul 2024 13:59:17 +0300 Subject: [PATCH 11/79] feat: Add publisher for localization health information msg type needed to be change. --- include/slam_toolbox/slam_toolbox_common.hpp | 3 +++ src/slam_toolbox_common.cpp | 16 ++++++++++++++-- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index a1c1fbf39..09e916ac8 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -49,6 +49,8 @@ #include "slam_toolbox/map_saver.hpp" #include "slam_toolbox/loop_closure_assistant.hpp" +#include // TODO: change here idk + namespace slam_toolbox { @@ -135,6 +137,7 @@ class SlamToolbox : public rclcpp::Node std::shared_ptr> sst_; std::shared_ptr> sstm_; std::shared_ptr> pose_pub_; + std::shared_ptr> localization_health_pub_; // TODO: Change here std::shared_ptr> ssMap_; std::shared_ptr> ssPauseMeasurements_; std::shared_ptr> ssSerialize_; diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index da07eb188..4b831a536 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -208,6 +208,9 @@ void SlamToolbox::setROSInterfaces() pose_pub_ = this->create_publisher( "pose", 10); + localization_health_pub_ = this->create_publisher( + "localization_health_baha", 10); + sst_ = this->create_publisher( map_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); sstm_ = this->create_publisher( @@ -258,11 +261,20 @@ void SlamToolbox::publishTransformLoop( //TODO: In future, we need to remove from there and create new function named as localizationHealthCheck. double * best_response =smapper_->getMapper()->GetBestResponse(); + static double previous_best_response = 0.0; + if (best_response != nullptr && *best_response > 0.02) { try { - //TODO: Write publisher topic for localization health check. - } catch (std::exception & e) { + if (*best_response != previous_best_response) { + std_msgs::msg::Float32 msg; + msg.data = static_cast(*best_response); // TODO: Change here + localization_health_pub_->publish(msg); + previous_best_response = *best_response; + } + } + catch (std::exception & e) { //TODO: Write publisher topic when cannot acces the result of health check . + RCLCPP_ERROR(this->get_logger(), "Exception caught while dereferencing best_response: %s", e.what()); } } // From 44bb715312df7b0ed805c2e65c306567fa1ee5a2 Mon Sep 17 00:00:00 2001 From: baha Date: Fri, 26 Jul 2024 16:39:16 +0300 Subject: [PATCH 12/79] replaced the function for localizationHealth --- lib/karto_sdk/src/Mapper.cpp | 72 +++++++++++++++++++++++++----------- 1 file changed, 50 insertions(+), 22 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 105ce1d4b..f1ccfe96b 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -648,20 +648,6 @@ kt_double ScanMatcher::MatchScan( << "-------elapsed time: " << elapsed_seconds.count() << "s\n"; #endif - /* - * purpose of equal to best_response is; Instead of {double} type {kt_double} doesn't mean anything inside the slam_toolbox_common - */ - double best_response = bestResponse; - - try - { - m_pMapper->SetBestResponse(&best_response); - } - catch (std::exception & e) { - throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " - "unable to publish set best response!"); - } - return bestResponse; } @@ -1539,12 +1525,33 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens while (!candidateChain.empty()) { #ifdef KARTO_DEBUG std::cout << "Checking of the candidatecChainsize inside of Loop CLosure: " << candidateChain.size() << std::endl; + using namespace std::chrono; + + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); + #endif Pose2 bestPose; Matrix3 covariance; kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false, false); + /* + * purpose of equal to best_response is; Instead of {double} type {kt_double} doesn't mean anything inside the slam_toolbox_common. + * The reason for getting the bestresponse here instead of the Matchscan function is that the other functions calling Matchscan always >>> + * receive outputs greater than 0.5 from bestresponse, regardless of whether the robot's position is correct or not. + */ + double best_response = coarseResponse; + + try + { + m_pMapper->SetBestResponse(&best_response); + } + catch (std::exception & e) { + throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " + "unable to publish set best response!"); + } + std::stringstream stream; stream << "COARSE RESPONSE: " << coarseResponse << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")" << @@ -1557,6 +1564,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) && (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) && (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())) + { LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector()); tmpScan.SetUniqueId(pScan->GetUniqueId()); @@ -1564,7 +1572,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens tmpScan.SetStateId(pScan->GetStateId()); tmpScan.SetCorrectedPose(pScan->GetCorrectedPose()); tmpScan.SetSensorPose(bestPose); // This also updates OdometricPose. - kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, + kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, candidateChain, bestPose, covariance, false); @@ -1587,8 +1595,14 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens loopClosed = true; } } + #ifdef KARTO_DEBUG std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::cout << "----------Calculated time of Matchscan after triggered\n" + << "----------INSIDE OF WHILE LOOPCLOSURE function\n" + << "----------elapsed time: " << elapsed_seconds.count() << "s\n"; #endif candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); } @@ -1699,11 +1713,25 @@ void MapperGraph::LinkNearChains( #ifdef KARTO_DEBUG end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; - std::cout << "----------Calculating time of Matchscan after triggered\n" + std::cout << "----------Calculated time of Matchscan after triggered\n" << "----------LinkNearCHains function\n" << "----------elapsed time: " << elapsed_seconds.count() << "s\n"; #endif - + /* + * purpose of equal to best_response is; Instead of {double} type {kt_double} doesn't mean anything inside the slam_toolbox_common. + * The reason for getting the bestresponse here instead of the Matchscan function is that the other functions calling Matchscan always >>> + * receive outputs greater than 0.5 from bestresponse, regardless of whether the robot's position is correct or not. + */ + double best_response = response; + + try + { + m_pMapper->SetBestResponse(&best_response); + } + catch (std::exception & e) { + throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " + "unable to publish set best response!"); + } if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE) { rMeans.push_back(mean); rCovariances.push_back(covariance); @@ -2068,7 +2096,7 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; std::cout <<"----------Finded chain size is" << chain.size() << std::endl; - std::cout <<"----------Calculating time of Matchscan inside nscans function\n" + std::cout <<"----------Calculated time of Matchscan inside nscans function\n" <<"----------elapsed time: " << elapsed_seconds.count() << "s\n"; #endif return chain; @@ -2793,7 +2821,7 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) #ifdef KARTO_DEBUG end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; - std::cout <<"----------Calculating time of Matchscan after triggered\n" + std::cout <<"----------Calculated time of Matchscan after triggered\n" <<"----------HasMovedEnough function\n" <<"----------elapsed time: " << elapsed_seconds.count() << "s\n"; #endif @@ -2876,7 +2904,7 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; - std::cout << "----------Calculating time of Matchscan after triggered\n" + std::cout << "----------Calculated time of Matchscan after triggered\n" << "----------ProcessAgainstNodesNearBy function\n" << "----------elapsed time: " << elapsed_seconds.count() << "s\n"; #endif @@ -2978,7 +3006,7 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covari end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; - std::cout << "----------Calculating time of Matchscan after triggered\n" + std::cout << "----------Calculated time of Matchscan after triggered\n" << "----------ProcessLocalization function\n" << "----------elapsed time: " << elapsed_seconds.count() << "s\n"; #endif @@ -3173,7 +3201,7 @@ kt_bool Mapper::ProcessAgainstNode( end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; - std::cout << "----------Calculating time of Matchscan after triggered\n" + std::cout << "----------Calculated time of Matchscan after triggered\n" << "----------ProcessAgainstNode function\n" << "----------elapsed time: " << elapsed_seconds.count() << "s\n"; #endif From 986e0472211db00d3758f0932f3b0a83b86daa89 Mon Sep 17 00:00:00 2001 From: baha Date: Wed, 31 Jul 2024 14:17:16 +0300 Subject: [PATCH 13/79] rename the localization_health publisher topic --- src/slam_toolbox_common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 4b831a536..6345613bf 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -209,7 +209,7 @@ void SlamToolbox::setROSInterfaces() pose_pub_ = this->create_publisher( "pose", 10); localization_health_pub_ = this->create_publisher( - "localization_health_baha", 10); + "slam_toolbox/best_response", 10); sst_ = this->create_publisher( map_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); From cbd12acd85ad1929d4635cb6333f231f7af51c97 Mon Sep 17 00:00:00 2001 From: Zarqu0n Date: Mon, 19 Aug 2024 11:20:42 +0300 Subject: [PATCH 14/79] publish map once can be set --- src/experimental/slam_toolbox_map_and_localization.cpp | 3 ++- src/slam_toolbox_common.cpp | 9 ++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/experimental/slam_toolbox_map_and_localization.cpp b/src/experimental/slam_toolbox_map_and_localization.cpp index c5552020c..15c206e8f 100644 --- a/src/experimental/slam_toolbox_map_and_localization.cpp +++ b/src/experimental/slam_toolbox_map_and_localization.cpp @@ -67,7 +67,7 @@ void MapAndLocalizationSlamToolbox::toggleMode(bool enable_localization) { if (enable_localization) { RCLCPP_INFO(get_logger(), "Enabling localization ..."); processor_type_ = PROCESS_LOCALIZATION; - + publish_map_once_ = this->get_parameter("publish_map_once").as_bool(); localization_pose_sub_ = create_subscription( "initialpose", 1, std::bind(&MapAndLocalizationSlamToolbox::localizePoseCallback, this, std::placeholders::_1)); @@ -87,6 +87,7 @@ void MapAndLocalizationSlamToolbox::toggleMode(bool enable_localization) { map_saver_ = std::make_unique(shared_from_this(), map_name_); boost::mutex::scoped_lock lock(smapper_mutex_); + publish_map_once_ = false; if (smapper_ && !smapper_->getMapper()->GetLocalizationVertices().empty()) { smapper_->clearLocalizationBuffer(); } diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 6345613bf..028534ca0 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -75,6 +75,9 @@ void SlamToolbox::configure() transform_publish_period = this->declare_parameter("transform_publish_period", transform_publish_period); + publish_map_once_ = this->declare_parameter("publish_map_once", + publish_map_once_); + this->declare_parameter("map_update_interval",10.0); threads_.push_back(std::make_unique( boost::bind(&SlamToolbox::publishTransformLoop, this, transform_publish_period))); @@ -308,11 +311,7 @@ void SlamToolbox::publishVisualizations() og.info.origin.orientation.w = 1.0; og.header.frame_id = map_frame_; - double map_update_interval = 10; - map_update_interval = this->declare_parameter("map_update_interval", - map_update_interval); - publish_map_once_ = this->declare_parameter("publish_map_once", - publish_map_once_); + auto map_update_interval = this->get_parameter("map_update_interval").as_double(); rclcpp::Rate r(1.0 / map_update_interval); while (rclcpp::ok()) { From c4246c84e1af48c28b9c254ceb53034c0c0aa559 Mon Sep 17 00:00:00 2001 From: Zarqu0n Date: Tue, 20 Aug 2024 08:41:14 +0300 Subject: [PATCH 15/79] set tf broadcaster to static broadcaster --- include/slam_toolbox/merge_maps_kinematic.hpp | 4 ++-- src/merge_maps_kinematic.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/slam_toolbox/merge_maps_kinematic.hpp b/include/slam_toolbox/merge_maps_kinematic.hpp index 92102e499..b21d791ad 100644 --- a/include/slam_toolbox/merge_maps_kinematic.hpp +++ b/include/slam_toolbox/merge_maps_kinematic.hpp @@ -31,7 +31,7 @@ #include "rclcpp/rclcpp.hpp" #include "interactive_markers/interactive_marker_server.hpp" #include "interactive_markers/menu_handler.hpp" -#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/static_transform_broadcaster.h" #include "tf2_ros/transform_listener.h" #include "tf2_ros/message_filter.h" #include "tf2/LinearMath/Matrix3x3.h" @@ -92,7 +92,7 @@ class MergeMapsKinematic : public rclcpp::Node std::vector> dataset_vec_; // TF - std::unique_ptr tfB_; + std::unique_ptr tfB_; // visualization std::unique_ptr interactive_server_; diff --git a/src/merge_maps_kinematic.cpp b/src/merge_maps_kinematic.cpp index c546aac8d..8fedd4d8c 100644 --- a/src/merge_maps_kinematic.cpp +++ b/src/merge_maps_kinematic.cpp @@ -49,7 +49,7 @@ void MergeMapsKinematic::configure() std::bind(&MergeMapsKinematic::addSubmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - tfB_ = std::make_unique(shared_from_this()); + tfB_ = std::make_unique(shared_from_this()); interactive_server_ = std::make_unique( From f29bc17527e5028ebc805531fc1465e0e03e5377 Mon Sep 17 00:00:00 2001 From: Zarqu0n Date: Wed, 21 Aug 2024 14:18:09 +0300 Subject: [PATCH 16/79] fix map position --- src/merge_maps_kinematic.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/merge_maps_kinematic.cpp b/src/merge_maps_kinematic.cpp index 8fedd4d8c..b00e5173a 100644 --- a/src/merge_maps_kinematic.cpp +++ b/src/merge_maps_kinematic.cpp @@ -118,8 +118,6 @@ bool MergeMapsKinematic::addSubmapCallback( og.info.width * og.info.resolution / 2.0, og.info.origin.position.y + og.info.height * og.info.resolution / 2.0, 0.)); - og.info.origin.position.x = -(og.info.width * og.info.resolution / 2.0); - og.info.origin.position.y = -(og.info.height * og.info.resolution / 2.0); og.header.stamp = this->now(); og.header.frame_id = "map_" + std::to_string(num_submaps_); sstS_[num_submaps_]->publish(og); From 2ae0adb11b5d204d5407e1250980af7e79129f16 Mon Sep 17 00:00:00 2001 From: Zarqu0n Date: Wed, 21 Aug 2024 14:18:23 +0300 Subject: [PATCH 17/79] change maps qos --- src/merge_maps_kinematic.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/merge_maps_kinematic.cpp b/src/merge_maps_kinematic.cpp index b00e5173a..ddee6b4cc 100644 --- a/src/merge_maps_kinematic.cpp +++ b/src/merge_maps_kinematic.cpp @@ -95,11 +95,12 @@ bool MergeMapsKinematic::addSubmapCallback( scans_vec_.push_back(scans); num_submaps_++; + auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(); // create and publish map with marker that will move the map around sstS_.push_back(this->create_publisher( - "/map_" + std::to_string(num_submaps_), rclcpp::QoS(1))); + "/map_" + std::to_string(num_submaps_), qos)); sstmS_.push_back(this->create_publisher( - "/map_metadata_" + std::to_string(num_submaps_), rclcpp::QoS(1))); + "/map_metadata_" + std::to_string(num_submaps_), qos)); sleep(1.0); nav_msgs::srv::GetMap::Response map; From b63cee653306be45af280528ca1d176ca186fc5f Mon Sep 17 00:00:00 2001 From: baha Date: Thu, 22 Aug 2024 15:48:32 +0300 Subject: [PATCH 18/79] added functions for service --- lib/karto_sdk/include/karto_sdk/Mapper.h | 9 +++++++- lib/karto_sdk/src/Mapper.cpp | 26 +++++++++++++++++++++--- 2 files changed, 31 insertions(+), 4 deletions(-) diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index d53d6416d..66acac07a 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1968,7 +1968,14 @@ class KARTO_EXPORT Mapper : public Module double * m_pbestResponse; double * GetBestResponse(); void SetBestResponse(double * pBestResponse); - + + /** + * After service called if bestResponse calculated, it will publish the service + */ + mutable bool m_bestResponseServiceFlag; //For acces const member function + bool GetBestResponseServiceFlag(); + void SetBestResponseServiceFlag(bool checkBestResponse); + /** * Allocate memory needed for mapping * @param rangeThreshold diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index f1ccfe96b..1af9bb439 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1545,7 +1545,8 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens try { - m_pMapper->SetBestResponse(&best_response); + m_pMapper->SetBestResponse(&best_response); + m_pMapper->SetBestResponseServiceFlag(false); } catch (std::exception & e) { throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " @@ -2149,7 +2150,8 @@ Mapper::Mapper() m_pMapperSensorManager(NULL), m_pGraph(NULL), m_pScanOptimizer(NULL), - m_pbestResponse(new double(0.0)) + m_pbestResponse(new double(0.0)), + m_bestResponseServiceFlag(false) { InitializeParameters(); } @@ -2165,7 +2167,8 @@ Mapper::Mapper(const std::string & rName) m_pMapperSensorManager(NULL), m_pGraph(NULL), m_pScanOptimizer(NULL), - m_pbestResponse(new double(0.0)) + m_pbestResponse(new double(0.0)), + m_bestResponseServiceFlag(false) { InitializeParameters(); } @@ -2763,6 +2766,7 @@ void Mapper::Reset() delete m_pbestResponse; m_pbestResponse = NULL; } + m_bestResponseServiceFlag = false; m_Initialized = false; m_Deserialized = false; while (!m_LocalizationScanVertices.empty()) { @@ -3260,6 +3264,11 @@ kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * return true; } + if (m_bestResponseServiceFlag) { + m_bestResponseServiceFlag = false; // Reset the flag + return true; + } + // test if enough time has passed kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime(); if (timeInterval >= m_pMinimumTimeInterval->GetValue()) { @@ -3379,6 +3388,17 @@ void Mapper::FireEndLoopClosure(const std::string & rInfo) const } } +void Mapper::SetBestResponseServiceFlag(bool checkBestResponse) +{ + std::cout<<"Setting Best Response Service Flag to true"< Date: Thu, 22 Aug 2024 15:50:02 +0300 Subject: [PATCH 19/79] added getBestResponseCallback for service data --- include/slam_toolbox/slam_toolbox_common.hpp | 6 ++++ src/slam_toolbox_common.cpp | 30 +++++++++++++++++++- 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 09e916ac8..c12f4ce82 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -48,6 +48,7 @@ #include "slam_toolbox/get_pose_helper.hpp" #include "slam_toolbox/map_saver.hpp" #include "slam_toolbox/loop_closure_assistant.hpp" +#include "std_srvs/srv/trigger.hpp" #include // TODO: change here idk @@ -128,6 +129,10 @@ class SlamToolbox : public rclcpp::Node const std::shared_ptr req, std::shared_ptr resp); + bool getBestResponseCallback( + const std::shared_ptr req, + std::shared_ptr res + ); // ROS-y-ness std::unique_ptr tf_; std::unique_ptr tfL_; @@ -142,6 +147,7 @@ class SlamToolbox : public rclcpp::Node std::shared_ptr> ssPauseMeasurements_; std::shared_ptr> ssSerialize_; std::shared_ptr> ssDesserialize_; + std::shared_ptr> ssGetBestResponse_; // TODO: check here // Storage for ROS parameters std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_; diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 6345613bf..1455d7e07 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -196,6 +196,8 @@ void SlamToolbox::setROSInterfaces() /*****************************************************************************/ { double tmp_val = 30.; + auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(); + tmp_val = this->declare_parameter("tf_buffer_duration", tmp_val); tf_ = std::make_unique(this->get_clock(), tf2::durationFromSec(tmp_val)); @@ -209,7 +211,7 @@ void SlamToolbox::setROSInterfaces() pose_pub_ = this->create_publisher( "pose", 10); localization_health_pub_ = this->create_publisher( - "slam_toolbox/best_response", 10); + "slam_toolbox/best_response", qos); sst_ = this->create_publisher( map_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -233,6 +235,11 @@ void SlamToolbox::setROSInterfaces() std::bind(&SlamToolbox::deserializePoseGraphCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + ssGetBestResponse_ = this->create_service( + "slam_toolbox/get_best_response", + std::bind(&SlamToolbox::getBestResponseCallback, this, + std::placeholders::_1,std::placeholders::_2)); + scan_filter_sub_ = std::make_unique>( shared_from_this().get(), scan_topic_, rmw_qos_profile_sensor_data); @@ -812,6 +819,27 @@ void SlamToolbox::loadSerializedPoseGraph( solver_->Compute(); } +/*****************************************************************************/ +bool SlamToolbox::getBestResponseCallback( + const std::shared_ptr req, + std::shared_ptr res) +/*****************************************************************************/ +{ + smapper_->getMapper()->SetBestResponseServiceFlag(true); + + double * best_response = smapper_->getMapper()->GetBestResponse(); + + if (best_response != nullptr) { + res->message = std::to_string(*best_response); + res->success = true; + return true; / + } else { + res->message = "Couldn't find bestResponse"; + res->success = false; + return false; + } +} + /*****************************************************************************/ bool SlamToolbox::deserializePoseGraphCallback( const std::shared_ptr request_header, From b7eb615c9eb2bcef8d8df22babe9af848ccf46d6 Mon Sep 17 00:00:00 2001 From: baha Date: Thu, 22 Aug 2024 16:42:58 +0300 Subject: [PATCH 20/79] nothing here to look --- src/slam_toolbox_common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 1455d7e07..51ccd2626 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -832,7 +832,7 @@ bool SlamToolbox::getBestResponseCallback( if (best_response != nullptr) { res->message = std::to_string(*best_response); res->success = true; - return true; / + return true; } else { res->message = "Couldn't find bestResponse"; res->success = false; From 01ea761aa575a43be232d83a351925cd2fb85740 Mon Sep 17 00:00:00 2001 From: baha Date: Fri, 23 Aug 2024 09:28:23 +0300 Subject: [PATCH 21/79] deleted health service and only add qos settings --- include/slam_toolbox/slam_toolbox_common.hpp | 6 ----- lib/karto_sdk/include/karto_sdk/Mapper.h | 7 ------ lib/karto_sdk/src/Mapper.cpp | 24 ++---------------- src/slam_toolbox_common.cpp | 26 -------------------- 4 files changed, 2 insertions(+), 61 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index c12f4ce82..09e916ac8 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -48,7 +48,6 @@ #include "slam_toolbox/get_pose_helper.hpp" #include "slam_toolbox/map_saver.hpp" #include "slam_toolbox/loop_closure_assistant.hpp" -#include "std_srvs/srv/trigger.hpp" #include // TODO: change here idk @@ -129,10 +128,6 @@ class SlamToolbox : public rclcpp::Node const std::shared_ptr req, std::shared_ptr resp); - bool getBestResponseCallback( - const std::shared_ptr req, - std::shared_ptr res - ); // ROS-y-ness std::unique_ptr tf_; std::unique_ptr tfL_; @@ -147,7 +142,6 @@ class SlamToolbox : public rclcpp::Node std::shared_ptr> ssPauseMeasurements_; std::shared_ptr> ssSerialize_; std::shared_ptr> ssDesserialize_; - std::shared_ptr> ssGetBestResponse_; // TODO: check here // Storage for ROS parameters std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_; diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 66acac07a..298ff9170 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1969,13 +1969,6 @@ class KARTO_EXPORT Mapper : public Module double * GetBestResponse(); void SetBestResponse(double * pBestResponse); - /** - * After service called if bestResponse calculated, it will publish the service - */ - mutable bool m_bestResponseServiceFlag; //For acces const member function - bool GetBestResponseServiceFlag(); - void SetBestResponseServiceFlag(bool checkBestResponse); - /** * Allocate memory needed for mapping * @param rangeThreshold diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 1af9bb439..6ce84ce91 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1546,7 +1546,6 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens try { m_pMapper->SetBestResponse(&best_response); - m_pMapper->SetBestResponseServiceFlag(false); } catch (std::exception & e) { throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " @@ -2150,8 +2149,7 @@ Mapper::Mapper() m_pMapperSensorManager(NULL), m_pGraph(NULL), m_pScanOptimizer(NULL), - m_pbestResponse(new double(0.0)), - m_bestResponseServiceFlag(false) + m_pbestResponse(new double(0.0)) { InitializeParameters(); } @@ -2167,8 +2165,7 @@ Mapper::Mapper(const std::string & rName) m_pMapperSensorManager(NULL), m_pGraph(NULL), m_pScanOptimizer(NULL), - m_pbestResponse(new double(0.0)), - m_bestResponseServiceFlag(false) + m_pbestResponse(new double(0.0)) { InitializeParameters(); } @@ -2766,7 +2763,6 @@ void Mapper::Reset() delete m_pbestResponse; m_pbestResponse = NULL; } - m_bestResponseServiceFlag = false; m_Initialized = false; m_Deserialized = false; while (!m_LocalizationScanVertices.empty()) { @@ -3264,11 +3260,6 @@ kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * return true; } - if (m_bestResponseServiceFlag) { - m_bestResponseServiceFlag = false; // Reset the flag - return true; - } - // test if enough time has passed kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime(); if (timeInterval >= m_pMinimumTimeInterval->GetValue()) { @@ -3388,17 +3379,6 @@ void Mapper::FireEndLoopClosure(const std::string & rInfo) const } } -void Mapper::SetBestResponseServiceFlag(bool checkBestResponse) -{ - std::cout<<"Setting Best Response Service Flag to true"<create_service( - "slam_toolbox/get_best_response", - std::bind(&SlamToolbox::getBestResponseCallback, this, - std::placeholders::_1,std::placeholders::_2)); - scan_filter_sub_ = std::make_unique>( shared_from_this().get(), scan_topic_, rmw_qos_profile_sensor_data); @@ -819,27 +814,6 @@ void SlamToolbox::loadSerializedPoseGraph( solver_->Compute(); } -/*****************************************************************************/ -bool SlamToolbox::getBestResponseCallback( - const std::shared_ptr req, - std::shared_ptr res) -/*****************************************************************************/ -{ - smapper_->getMapper()->SetBestResponseServiceFlag(true); - - double * best_response = smapper_->getMapper()->GetBestResponse(); - - if (best_response != nullptr) { - res->message = std::to_string(*best_response); - res->success = true; - return true; - } else { - res->message = "Couldn't find bestResponse"; - res->success = false; - return false; - } -} - /*****************************************************************************/ bool SlamToolbox::deserializePoseGraphCallback( const std::shared_ptr request_header, From f8cd9607571202bbb8f2845e61c0743e492b5161 Mon Sep 17 00:00:00 2001 From: Renbago Date: Tue, 10 Sep 2024 09:51:38 +0300 Subject: [PATCH 22/79] added service --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index aab4ab8bb..dc7b8bcea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,6 +105,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} srvs/MergeMaps.srv srvs/AddSubmap.srv srvs/DeserializePoseGraph.srv + srvs/DesiredPoseChecker.srv srvs/SerializePoseGraph.srv DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs ) From 001171cf9fc7d7e26406b90945f20b89c59d1a3b Mon Sep 17 00:00:00 2001 From: Renbago Date: Tue, 10 Sep 2024 09:57:57 +0300 Subject: [PATCH 23/79] removed getBestResponseCallback from common to -> localization --- include/slam_toolbox/slam_toolbox_common.hpp | 6 +---- .../slam_toolbox_localization.hpp | 12 ++++++++- src/slam_toolbox_common.cpp | 26 ------------------- 3 files changed, 12 insertions(+), 32 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index c12f4ce82..653619d32 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -129,10 +129,6 @@ class SlamToolbox : public rclcpp::Node const std::shared_ptr req, std::shared_ptr resp); - bool getBestResponseCallback( - const std::shared_ptr req, - std::shared_ptr res - ); // ROS-y-ness std::unique_ptr tf_; std::unique_ptr tfL_; @@ -147,7 +143,6 @@ class SlamToolbox : public rclcpp::Node std::shared_ptr> ssPauseMeasurements_; std::shared_ptr> ssSerialize_; std::shared_ptr> ssDesserialize_; - std::shared_ptr> ssGetBestResponse_; // TODO: check here // Storage for ROS parameters std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_; @@ -182,6 +177,7 @@ class SlamToolbox : public rclcpp::Node nav_msgs::srv::GetMap::Response map_; ProcessType processor_type_; std::unique_ptr process_near_pose_; + std::unique_ptr process_desired_pose_; tf2::Transform reprocessing_transform_; // pluginlib diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index a99f56e9e..96034c6f3 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -43,6 +43,10 @@ class LocalizationSlamToolbox : public SlamToolbox const std::shared_ptr req, std::shared_ptr resp); + bool getBestResponseCallback( + const std::shared_ptr req, + std::shared_ptr res); + virtual bool serializePoseGraphCallback( const std::shared_ptr request_header, const std::shared_ptr req, @@ -59,7 +63,13 @@ class LocalizationSlamToolbox : public SlamToolbox std::shared_ptr> localization_pose_sub_; - std::shared_ptr > clear_localization_; + std::shared_ptr> clear_localization_; + std::shared_ptr> ssGetBestResponse_; // TODO: check here + + sensor_msgs::msg::LaserScan::ConstSharedPtr last_scan_stored_; + Pose2 last_odom_pose_stored_; + LaserRangeFinder * last_laser_stored_; + bool have_scan_values_ = false; }; } // namespace slam_toolbox diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 51ccd2626..c31970530 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -235,11 +235,6 @@ void SlamToolbox::setROSInterfaces() std::bind(&SlamToolbox::deserializePoseGraphCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - ssGetBestResponse_ = this->create_service( - "slam_toolbox/get_best_response", - std::bind(&SlamToolbox::getBestResponseCallback, this, - std::placeholders::_1,std::placeholders::_2)); - scan_filter_sub_ = std::make_unique>( shared_from_this().get(), scan_topic_, rmw_qos_profile_sensor_data); @@ -819,27 +814,6 @@ void SlamToolbox::loadSerializedPoseGraph( solver_->Compute(); } -/*****************************************************************************/ -bool SlamToolbox::getBestResponseCallback( - const std::shared_ptr req, - std::shared_ptr res) -/*****************************************************************************/ -{ - smapper_->getMapper()->SetBestResponseServiceFlag(true); - - double * best_response = smapper_->getMapper()->GetBestResponse(); - - if (best_response != nullptr) { - res->message = std::to_string(*best_response); - res->success = true; - return true; - } else { - res->message = "Couldn't find bestResponse"; - res->success = false; - return false; - } -} - /*****************************************************************************/ bool SlamToolbox::deserializePoseGraphCallback( const std::shared_ptr request_header, From 7e9670f6edcb82067c26fadd24fc0d2c56d51aa1 Mon Sep 17 00:00:00 2001 From: Renbago Date: Tue, 10 Sep 2024 09:58:24 +0300 Subject: [PATCH 24/79] added msg depends and new enum type --- include/slam_toolbox/toolbox_msgs.hpp | 1 + include/slam_toolbox/toolbox_types.hpp | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/include/slam_toolbox/toolbox_msgs.hpp b/include/slam_toolbox/toolbox_msgs.hpp index 1300962d4..5ef8597c9 100644 --- a/include/slam_toolbox/toolbox_msgs.hpp +++ b/include/slam_toolbox/toolbox_msgs.hpp @@ -39,5 +39,6 @@ #include "slam_toolbox/srv/deserialize_pose_graph.hpp" #include "slam_toolbox/srv/merge_maps.hpp" #include "slam_toolbox/srv/add_submap.hpp" +#include "slam_toolbox/srv/desired_pose_checker.hpp" #endif // SLAM_TOOLBOX__TOOLBOX_MSGS_HPP_ diff --git a/include/slam_toolbox/toolbox_types.hpp b/include/slam_toolbox/toolbox_types.hpp index dcf6a7273..97f527570 100644 --- a/include/slam_toolbox/toolbox_types.hpp +++ b/include/slam_toolbox/toolbox_types.hpp @@ -86,7 +86,8 @@ enum ProcessType PROCESS = 0, PROCESS_FIRST_NODE = 1, PROCESS_NEAR_REGION = 2, - PROCESS_LOCALIZATION = 3 + PROCESS_LOCALIZATION = 3, + PROCESS_DESIRED_POSE = 4 }; // Pause state From a503c81e6105a7ccbcdb0d9a152cf2615a28b09a Mon Sep 17 00:00:00 2001 From: Renbago Date: Tue, 10 Sep 2024 09:58:38 +0300 Subject: [PATCH 25/79] DesiredPoseChecker srv --- srvs/DesiredPoseChecker.srv | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 srvs/DesiredPoseChecker.srv diff --git a/srvs/DesiredPoseChecker.srv b/srvs/DesiredPoseChecker.srv new file mode 100644 index 000000000..8412eee57 --- /dev/null +++ b/srvs/DesiredPoseChecker.srv @@ -0,0 +1,15 @@ +float64 pose_x +float64 pose_y +bool do_relocalize +int32 process_timeout +float64 search_range +float64 minimum_best_response +float64 angle_resolution + +--- +float64 best_response +float64 relocated_x +float64 relocated_y +float64 relocated_yaw +string message +bool success \ No newline at end of file From d75dd5414ed89508d948d60c6627e9bfd4729c78 Mon Sep 17 00:00:00 2001 From: Renbago Date: Tue, 10 Sep 2024 09:59:28 +0300 Subject: [PATCH 26/79] relocalization service added --- src/slam_toolbox_localization.cpp | 158 +++++++++++++++++++++++++++++- 1 file changed, 153 insertions(+), 5 deletions(-) diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index fe4e066b2..87aebc7e7 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -39,6 +39,12 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) std::bind(&LocalizationSlamToolbox::clearLocalizationBuffer, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + ssGetBestResponse_ = this->create_service( + "slam_toolbox/get_best_response", + std::bind(&LocalizationSlamToolbox::getBestResponseCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + // in localization mode, we cannot allow for interactive mode enable_interactive_mode_ = false; @@ -122,6 +128,7 @@ void LocalizationSlamToolbox::laserCallback( scan_header = scan->header; // no odom info Pose2 pose; + RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Processing scans."); if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) { RCLCPP_WARN(get_logger(), "Failed to compute odom pose"); return; @@ -148,11 +155,20 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( Pose2 & odom_pose) /*****************************************************************************/ { + // dirty storage: + last_laser_stored_ = laser; + last_scan_stored_ = scan; + last_odom_pose_stored_ = odom_pose; + have_scan_values_ = true; + boost::mutex::scoped_lock l(pose_mutex_); if (processor_type_ == PROCESS_LOCALIZATION && process_near_pose_) { processor_type_ = PROCESS_NEAR_REGION; } + if (processor_type_ == PROCESS_LOCALIZATION && process_desired_pose_) { + processor_type_ = PROCESS_DESIRED_POSE; + } LocalizedRangeScan * range_scan = getLocalizedRangeScan( laser, scan, odom_pose); @@ -172,7 +188,7 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( return nullptr; } - // set our position to the requested pose and process + // set our position to the requested pose and proces range_scan->SetOdometricPose(*process_near_pose_); range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); process_near_pose_.reset(nullptr); @@ -181,20 +197,25 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( // reset to localization mode update_reprocessing_transform = true; processor_type_ = PROCESS_LOCALIZATION; - } else if (processor_type_ == PROCESS_LOCALIZATION) { + }else if (processor_type_ == PROCESS_DESIRED_POSE) { + // process_desired_pose_.reset(nullptr); + update_reprocessing_transform = false; + processor_type_ = PROCESS_LOCALIZATION; + }else if (processor_type_ == PROCESS_LOCALIZATION) { processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance); update_reprocessing_transform = false; - } else { + } + else { RCLCPP_FATAL(get_logger(), "LocalizationSlamToolbox: " "No valid processor type set! Exiting."); exit(-1); } - // if successfully processed, create odom to map transformation if (!processed) { delete range_scan; range_scan = nullptr; - } else { + } + else { // compute our new transform setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, scan->header.stamp, update_reprocessing_transform); @@ -205,6 +226,133 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( return range_scan; } +/*****************************************************************************/ +bool LocalizationSlamToolbox::getBestResponseCallback( + const std::shared_ptr req, + std::shared_ptr res) +/*****************************************************************************/ +{ + std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now(); + std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); + Matrix3 covariance; + LocalizedRangeScan * range_scan = nullptr; + covariance.SetToIdentity(); + { + boost::mutex::scoped_lock lock(smapper_mutex_); + smapper_->clearLocalizationBuffer(); + } + double loop_search_maximum_distance; + double link_scan_maximum_distance; + double minimum_best_response; + double angle_resolution; + double search_maximum_distance; + double search_maximum_loop_closure_distance; + double search_maximum_link_scan_distance; + int process_timeout; + this->get_parameter("loop_search_maximum_distance", loop_search_maximum_distance); + this->get_parameter("link_scan_maximum_distance", link_scan_maximum_distance); + + if (req->pose_x == 0.0 || req->pose_y == 0.0) { + RCLCPP_ERROR(get_logger(), "Error: pose_x or pose_y is not provided."); + res->message = "Error: pose_x or pose_y is missing. Please use it"; + res->success = false; + return false; + } + if (req->minimum_best_response == 0.0) { + RCLCPP_INFO(get_logger(), "minimum_best_response is not provided setting initial value to 0.5"); + minimum_best_response = (req->minimum_best_response != 0.0) ? req->minimum_best_response : 0.5; + } + if(req->angle_resolution == 0.0){ + RCLCPP_INFO(get_logger(), "angle_resolution is not provided setting initial value to 20.0"); + angle_resolution = (req->angle_resolution != 0.0) ? req->angle_resolution : 20.0; + } + if(req->process_timeout == 0){ + RCLCPP_INFO(get_logger(), "process_timeout is not provided setting initial value to 10"); + process_timeout = (req->process_timeout != 0) ? req->process_timeout : 10; + } + if(req->search_range == 0.0){ + RCLCPP_INFO(get_logger(), "search_maximum_distance is not provided setting initial value to yaml param"); + search_maximum_distance = req->search_range; + this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", search_maximum_distance)); + this->set_parameter(rclcpp::Parameter("link_scan_maximum_distance", search_maximum_distance)); + } + + std::cout << "last_odom_pose_stored_ " << last_odom_pose_stored_ << std::endl; + if (!have_scan_values_) { + res->message = "No scan values stored try later"; + res->success = false; + this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); + this->set_parameter(rclcpp::Parameter("link_scan_maximum_distance", link_scan_maximum_distance)); + return false; + } + else{ + for (double angle = 0.0; angle <= 360.0; angle += angle_resolution) { + bool processed = false; + { + boost::mutex::scoped_lock l(pose_mutex_); + process_desired_pose_ = std::make_unique(req->pose_x, req->pose_y, angle); + range_scan = getLocalizedRangeScan(last_laser_stored_, last_scan_stored_, last_odom_pose_stored_); + + boost::mutex::scoped_lock lock(smapper_mutex_); + range_scan->SetOdometricPose(*process_desired_pose_); + range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); + processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); + } + double * best_response = smapper_->getMapper()->GetBestResponse(); + + if (processed) { + std::cout << "best_response " << *best_response << std::endl; + std::cout << "req->minimum_best_response " << minimum_best_response << std::endl; + if (best_response != nullptr && *best_response > minimum_best_response) { + std::cout<< "finded best response" << std::endl; + res->message = std::to_string(*best_response); + res->success = true; + this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); + this->set_parameter(rclcpp::Parameter("link_scan_maximum_distance", link_scan_maximum_distance)); + if (req->do_relocalize) { + // compute our new transform + setTransformFromPoses(range_scan->GetCorrectedPose(), last_odom_pose_stored_, + last_scan_stored_->header.stamp, true); + + publishPose(range_scan->GetCorrectedPose(), covariance, last_scan_stored_->header.stamp); + } + return true; + } else { + res->message = "Couldn't find bestResponse"; + { + boost::mutex::scoped_lock lock(smapper_mutex_); + smapper_->clearLocalizationBuffer(); + } + } + } + else { + res->message = "Couldn't process scan will try again"; + end = std::chrono::high_resolution_clock::now(); + auto time_elapsed = std::chrono::duration_cast(end - start).count(); + if (time_elapsed > process_timeout) { + res->message = "Couldnt process scan in " + std::to_string(process_timeout) + " seconds"; + res->success = false; + this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); + this->set_parameter(rclcpp::Parameter("link_scan_maximum_distance", link_scan_maximum_distance)); + + return false; + } + } + + end = std::chrono::high_resolution_clock::now(); + auto time_elapsed = std::chrono::duration_cast(end - start).count(); + if (time_elapsed > process_timeout) { + res->message = "Couldnt find bestResponse in " + std::to_string(process_timeout) + " seconds"; + res->success = false; + this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); + this->set_parameter(rclcpp::Parameter("link_scan_maximum_distance", link_scan_maximum_distance)); + return false; + } + } + res->message = "Couldn't find with this resolution at desired time, halving the angle_resolution and decreasing best_response then search again."; + } +} + /*****************************************************************************/ void LocalizationSlamToolbox::localizePoseCallback( const From 9b4529af4fb006e284dd746ec556d05aebaa6488 Mon Sep 17 00:00:00 2001 From: Zarqu0n Date: Tue, 10 Sep 2024 14:20:27 +0300 Subject: [PATCH 27/79] add change map topic function --- .../slam_toolbox_map_and_localization.hpp | 1 + .../slam_toolbox_map_and_localization.cpp | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp b/include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp index a19db256d..965fc97b8 100644 --- a/include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp +++ b/include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp @@ -31,6 +31,7 @@ class MapAndLocalizationSlamToolbox : public LocalizationSlamToolbox virtual ~MapAndLocalizationSlamToolbox() {} void loadPoseGraphByParams() override; void configure() override; + void changeMapTopic(const std::string & map_topic); protected: void laserCallback( diff --git a/src/experimental/slam_toolbox_map_and_localization.cpp b/src/experimental/slam_toolbox_map_and_localization.cpp index 15c206e8f..c531626b7 100644 --- a/src/experimental/slam_toolbox_map_and_localization.cpp +++ b/src/experimental/slam_toolbox_map_and_localization.cpp @@ -66,6 +66,8 @@ void MapAndLocalizationSlamToolbox::toggleMode(bool enable_localization) { if (enable_localization) { RCLCPP_INFO(get_logger(), "Enabling localization ..."); + changeMapTopic(map_name_); + updateMap(); processor_type_ = PROCESS_LOCALIZATION; publish_map_once_ = this->get_parameter("publish_map_once").as_bool(); localization_pose_sub_ = @@ -78,9 +80,11 @@ void MapAndLocalizationSlamToolbox::toggleMode(bool enable_localization) { // in localization mode, disable map saver map_saver_.reset(); + } else { RCLCPP_INFO(get_logger(), "Enabling mapping ..."); + changeMapTopic("map"); processor_type_ = PROCESS; localization_pose_sub_.reset(); clear_localization_.reset(); @@ -102,6 +106,7 @@ void MapAndLocalizationSlamToolbox::loadPoseGraphByParams() LocalizationSlamToolbox::loadPoseGraphByParams(); } else { + changeMapTopic(map_name_); SlamToolbox::loadPoseGraphByParams(); } } @@ -132,6 +137,7 @@ bool MapAndLocalizationSlamToolbox::deserializePoseGraphCallback( return LocalizationSlamToolbox::deserializePoseGraphCallback(request_header, req, resp); } else { + changeMapTopic(map_name_); return SlamToolbox::deserializePoseGraphCallback(request_header, req, resp); } } @@ -177,6 +183,18 @@ LocalizedRangeScan * MapAndLocalizationSlamToolbox::addScan( } } +/*****************************************************************************/ +void MapAndLocalizationSlamToolbox::changeMapTopic(const std::string & map_topic) +/*****************************************************************************/ +{ + sst_.reset(); + sstm_.reset(); + sst_ = this->create_publisher( + map_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + sstm_ = this->create_publisher( + map_topic + "_metadata", + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); +} } // namespace slam_toolbox #include "rclcpp_components/register_node_macro.hpp" From 534373af8ef9ddfbf54ea28ac07dbcbc15d4be8a Mon Sep 17 00:00:00 2001 From: Zarqu0n Date: Tue, 10 Sep 2024 15:30:38 +0300 Subject: [PATCH 28/79] fix slam toolbox shutting down error when mapping --- src/experimental/slam_toolbox_map_and_localization.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/experimental/slam_toolbox_map_and_localization.cpp b/src/experimental/slam_toolbox_map_and_localization.cpp index c531626b7..0585cbb88 100644 --- a/src/experimental/slam_toolbox_map_and_localization.cpp +++ b/src/experimental/slam_toolbox_map_and_localization.cpp @@ -84,7 +84,7 @@ void MapAndLocalizationSlamToolbox::toggleMode(bool enable_localization) { } else { RCLCPP_INFO(get_logger(), "Enabling mapping ..."); - changeMapTopic("map"); + changeMapTopic(std::string("map")); processor_type_ = PROCESS; localization_pose_sub_.reset(); clear_localization_.reset(); From 54aa3ebb2932babf4c32a8476bcf7ecabe68a1e6 Mon Sep 17 00:00:00 2001 From: Zarqu0n Date: Tue, 10 Sep 2024 15:55:35 +0300 Subject: [PATCH 29/79] fix mapping error --- src/experimental/slam_toolbox_map_and_localization.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/experimental/slam_toolbox_map_and_localization.cpp b/src/experimental/slam_toolbox_map_and_localization.cpp index 0585cbb88..81d06fa5b 100644 --- a/src/experimental/slam_toolbox_map_and_localization.cpp +++ b/src/experimental/slam_toolbox_map_and_localization.cpp @@ -84,12 +84,11 @@ void MapAndLocalizationSlamToolbox::toggleMode(bool enable_localization) { } else { RCLCPP_INFO(get_logger(), "Enabling mapping ..."); - changeMapTopic(std::string("map")); processor_type_ = PROCESS; localization_pose_sub_.reset(); clear_localization_.reset(); map_saver_ = std::make_unique(shared_from_this(), map_name_); - + changeMapTopic(std::string("map")); boost::mutex::scoped_lock lock(smapper_mutex_); publish_map_once_ = false; if (smapper_ && !smapper_->getMapper()->GetLocalizationVertices().empty()) { @@ -106,8 +105,8 @@ void MapAndLocalizationSlamToolbox::loadPoseGraphByParams() LocalizationSlamToolbox::loadPoseGraphByParams(); } else { - changeMapTopic(map_name_); SlamToolbox::loadPoseGraphByParams(); + changeMapTopic(map_name_); } } @@ -187,6 +186,7 @@ LocalizedRangeScan * MapAndLocalizationSlamToolbox::addScan( void MapAndLocalizationSlamToolbox::changeMapTopic(const std::string & map_topic) /*****************************************************************************/ { + boost::mutex::scoped_lock lock(smapper_mutex_); sst_.reset(); sstm_.reset(); sst_ = this->create_publisher( From 9692da60e7986438b72568bbc42d1d3b2672c6ac Mon Sep 17 00:00:00 2001 From: Renbago Date: Tue, 10 Sep 2024 16:37:59 +0300 Subject: [PATCH 30/79] some redefinitions --- include/slam_toolbox/slam_toolbox_common.hpp | 10 ++ src/slam_toolbox_common.cpp | 35 +++++ src/slam_toolbox_localization.cpp | 157 ++++++++++++------- srvs/DesiredPoseChecker.srv | 11 +- 4 files changed, 147 insertions(+), 66 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 653619d32..79d0f72c6 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -157,6 +157,16 @@ class SlamToolbox : public rclcpp::Node double yaw_covariance_scale_; bool first_measurement_, enable_interactive_mode_; + // Position search service params + double position_search_distance_; + double position_search_resolution_; + double position_search_smear_deviation_; + double position_search_fine_angle_offset_; + double position_search_coarse_angle_offset_; + double position_search_coarse_angle_resolution_; + double position_search_minimum_best_response_; + bool position_search_do_relocalization_; + // Book keeping std::unique_ptr smapper_; std::unique_ptr dataset_; diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index c31970530..90be0a75f 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -189,6 +189,41 @@ void SlamToolbox::setParams() smapper_->configure(shared_from_this()); this->declare_parameter("paused_new_measurements",rclcpp::ParameterType::PARAMETER_BOOL); this->set_parameter({"paused_new_measurements", false}); + + /* + * Those parameters for pose_search service + */ + position_search_distance_ = 10.0; + position_search_distance_ = this->declare_parameter("position_search_distance", + position_search_distance_); + + position_search_resolution_ = 0.2; + position_search_resolution_ = this->declare_parameter("position_search_resolution", + position_search_resolution_); + + position_search_smear_deviation_ = 0.2; + position_search_smear_deviation_ = this->declare_parameter("position_search_smear_deviation", + position_search_smear_deviation_); + + position_search_fine_angle_offset_ = 0.0314; + position_search_fine_angle_offset_ = this->declare_parameter("position_search_fine_search_angle_offset", + position_search_fine_angle_offset_); + + position_search_coarse_angle_offset_ = 3.14; + position_search_coarse_angle_offset_ = this->declare_parameter("position_search_coarse_angle_offset", + position_search_coarse_angle_offset_); + + position_search_coarse_angle_resolution_ = 0.314; + position_search_coarse_angle_resolution_ = this->declare_parameter("position_search_coarse_angle_resolution", + position_search_coarse_angle_resolution_); + + position_search_do_relocalization_ = false; + position_search_do_relocalization_ = this->declare_parameter("position_search_do_relocalization", + position_search_do_relocalization_); + + position_search_minimum_best_response_ = 0.45; + position_search_minimum_best_response_ = this->declare_parameter("position_search_minimum_best_response", + position_search_minimum_best_response_); } /*****************************************************************************/ diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 87aebc7e7..788a02dfb 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -241,16 +241,37 @@ bool LocalizationSlamToolbox::getBestResponseCallback( boost::mutex::scoped_lock lock(smapper_mutex_); smapper_->clearLocalizationBuffer(); } - double loop_search_maximum_distance; - double link_scan_maximum_distance; - double minimum_best_response; - double angle_resolution; - double search_maximum_distance; - double search_maximum_loop_closure_distance; - double search_maximum_link_scan_distance; - int process_timeout; - this->get_parameter("loop_search_maximum_distance", loop_search_maximum_distance); - this->get_parameter("link_scan_maximum_distance", link_scan_maximum_distance); + + /* + Yaml parameters definitions for setting the parameters after process finished + */ + double initial_correlation_search_space_dimension = this->get_parameter("correlation_search_space_dimension").as_double(); + double initial_correlation_search_space_resolution = this->get_parameter("correlation_search_space_resolution").as_double(); + double initial_correlation_search_space_smear_deviation = this->get_parameter("correlation_search_space_smear_deviation").as_double(); + + double initial_loop_search_space_dimension = this->get_parameter("loop_search_space_dimension").as_double(); + // double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); + // double initial_loop_search_space_smear_deviation = this->get_parameter("loop_search_space_smear_deviation").as_double(); + + double initial_fine_search_angle_offset = this->get_parameter("fine_search_angle_offset").as_double(); + double initial_coarse_search_angle_offset = this->get_parameter("coarse_search_angle_offset").as_double(); + double initial_coarse_angle_resolution = this->get_parameter("coarse_angle_resolution").as_double(); + + double initial_do_relocalization = this->get_parameter("position_search_do_relocalization").as_bool(); + double initial_minimum_best_response = this->get_parameter("position_search_minimum_best_response").as_double(); + + /* + * Setting initial_definitions. + */ + + smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(position_search_distance_); + smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(position_search_resolution_); + smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(position_search_smear_deviation_); + smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance_); + smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset_); + smapper_->getMapper()->setParamCoarseSearchAngleOffset(position_search_coarse_angle_offset_); + smapper_->getMapper()->setParamCoarseAngleResolution(position_search_coarse_angle_resolution_); + if (req->pose_x == 0.0 || req->pose_y == 0.0) { RCLCPP_ERROR(get_logger(), "Error: pose_x or pose_y is not provided."); @@ -258,39 +279,36 @@ bool LocalizationSlamToolbox::getBestResponseCallback( res->success = false; return false; } - if (req->minimum_best_response == 0.0) { - RCLCPP_INFO(get_logger(), "minimum_best_response is not provided setting initial value to 0.5"); - minimum_best_response = (req->minimum_best_response != 0.0) ? req->minimum_best_response : 0.5; - } - if(req->angle_resolution == 0.0){ - RCLCPP_INFO(get_logger(), "angle_resolution is not provided setting initial value to 20.0"); - angle_resolution = (req->angle_resolution != 0.0) ? req->angle_resolution : 20.0; - } - if(req->process_timeout == 0){ - RCLCPP_INFO(get_logger(), "process_timeout is not provided setting initial value to 10"); - process_timeout = (req->process_timeout != 0) ? req->process_timeout : 10; + + if (req->do_relocalization){ + RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Searching for best response with relocalize"); + initial_do_relocalization = true; + } else { + RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Searching for best response without relocalize"); + initial_do_relocalization = false; } - if(req->search_range == 0.0){ - RCLCPP_INFO(get_logger(), "search_maximum_distance is not provided setting initial value to yaml param"); - search_maximum_distance = req->search_range; - this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", search_maximum_distance)); - this->set_parameter(rclcpp::Parameter("link_scan_maximum_distance", search_maximum_distance)); + + if (req->search_distance != 0.0) { + position_search_distance_ = req->search_distance; + this->set_parameter(rclcpp::Parameter("correlation_search_space_dimension", position_search_distance_)); + } + + if (req->minimum_best_response != 0.0) { + initial_minimum_best_response = req->minimum_best_response; + this->set_parameter(rclcpp::Parameter("position_search_minimum_best_response", initial_minimum_best_response)); } - std::cout << "last_odom_pose_stored_ " << last_odom_pose_stored_ << std::endl; if (!have_scan_values_) { res->message = "No scan values stored try later"; res->success = false; - this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); - this->set_parameter(rclcpp::Parameter("link_scan_maximum_distance", link_scan_maximum_distance)); return false; } else{ - for (double angle = 0.0; angle <= 360.0; angle += angle_resolution) { + // for (double angle = 0.0; angle <= 360.0; angle += angle_resolution) { bool processed = false; { boost::mutex::scoped_lock l(pose_mutex_); - process_desired_pose_ = std::make_unique(req->pose_x, req->pose_y, angle); + process_desired_pose_ = std::make_unique(req->pose_x, req->pose_y, 0.0); range_scan = getLocalizedRangeScan(last_laser_stored_, last_scan_stored_, last_odom_pose_stored_); boost::mutex::scoped_lock lock(smapper_mutex_); @@ -298,58 +316,75 @@ bool LocalizationSlamToolbox::getBestResponseCallback( range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); } - double * best_response = smapper_->getMapper()->GetBestResponse(); if (processed) { + double * best_response = smapper_->getMapper()->GetBestResponse(); std::cout << "best_response " << *best_response << std::endl; - std::cout << "req->minimum_best_response " << minimum_best_response << std::endl; - if (best_response != nullptr && *best_response > minimum_best_response) { + std::cout << "req->minimum_best_response " << initial_minimum_best_response << std::endl; + if (best_response != nullptr && *best_response > initial_minimum_best_response) { std::cout<< "finded best response" << std::endl; res->message = std::to_string(*best_response); res->success = true; - this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); - this->set_parameter(rclcpp::Parameter("link_scan_maximum_distance", link_scan_maximum_distance)); - if (req->do_relocalize) { - // compute our new transform + if (initial_do_relocalization) { + setTransformFromPoses(range_scan->GetCorrectedPose(), last_odom_pose_stored_, last_scan_stored_->header.stamp, true); publishPose(range_scan->GetCorrectedPose(), covariance, last_scan_stored_->header.stamp); } + + smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); + smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); + smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); + smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); + smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); + smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); + smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); + + return true; } else { res->message = "Couldn't find bestResponse"; + res->success = false; { boost::mutex::scoped_lock lock(smapper_mutex_); smapper_->clearLocalizationBuffer(); } - } - } - else { - res->message = "Couldn't process scan will try again"; - end = std::chrono::high_resolution_clock::now(); - auto time_elapsed = std::chrono::duration_cast(end - start).count(); - if (time_elapsed > process_timeout) { - res->message = "Couldnt process scan in " + std::to_string(process_timeout) + " seconds"; - res->success = false; - this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); - this->set_parameter(rclcpp::Parameter("link_scan_maximum_distance", link_scan_maximum_distance)); - + smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); + smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); + smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); + smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); + smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); + smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); + smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); return false; } } - - end = std::chrono::high_resolution_clock::now(); - auto time_elapsed = std::chrono::duration_cast(end - start).count(); - if (time_elapsed > process_timeout) { - res->message = "Couldnt find bestResponse in " + std::to_string(process_timeout) + " seconds"; - res->success = false; - this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); - this->set_parameter(rclcpp::Parameter("link_scan_maximum_distance", link_scan_maximum_distance)); - return false; - } - } + // else { + // res->message = "Couldn't process scan will try again"; + // end = std::chrono::high_resolution_clock::now(); + // auto time_elapsed = std::chrono::duration_cast(end - start).count(); + // if (time_elapsed > process_timeout) { + // res->message = "Couldnt process scan in " + std::to_string(process_timeout) + " seconds"; + // res->success = false; + // this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); + + // return false; + // } + // } + + // end = std::chrono::high_resolution_clock::now(); + // auto time_elapsed = std::chrono::duration_cast(end - start).count(); + // if (time_elapsed > process_timeout) { + // res->message = "Couldnt find bestResponse in " + std::to_string(process_timeout) + " seconds"; + // res->success = false; + // this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); + // return false; + // } + // } res->message = "Couldn't find with this resolution at desired time, halving the angle_resolution and decreasing best_response then search again."; + res->success = false; + return false; } } diff --git a/srvs/DesiredPoseChecker.srv b/srvs/DesiredPoseChecker.srv index 8412eee57..94c2bc0d2 100644 --- a/srvs/DesiredPoseChecker.srv +++ b/srvs/DesiredPoseChecker.srv @@ -1,10 +1,8 @@ float64 pose_x float64 pose_y -bool do_relocalize -int32 process_timeout -float64 search_range +float64 search_distance float64 minimum_best_response -float64 angle_resolution +bool do_relocalization --- float64 best_response @@ -12,4 +10,7 @@ float64 relocated_x float64 relocated_y float64 relocated_yaw string message -bool success \ No newline at end of file +bool success + + + From b8a2926b94ca474473623a38595da0313de38bde Mon Sep 17 00:00:00 2001 From: Oguzhan Kose Date: Wed, 11 Sep 2024 15:29:54 +0300 Subject: [PATCH 31/79] remove unnecessary changes --- lib/karto_sdk/src/Mapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 6ce84ce91..f1ccfe96b 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1545,7 +1545,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens try { - m_pMapper->SetBestResponse(&best_response); + m_pMapper->SetBestResponse(&best_response); } catch (std::exception & e) { throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " From 0b1cd28e48fbea2fb77cedfe8a58138bd7aa2a6e Mon Sep 17 00:00:00 2001 From: Oguzhan Kose Date: Wed, 11 Sep 2024 15:30:33 +0300 Subject: [PATCH 32/79] remove unnecessary changes --- lib/karto_sdk/include/karto_sdk/Mapper.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 298ff9170..d53d6416d 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1968,7 +1968,7 @@ class KARTO_EXPORT Mapper : public Module double * m_pbestResponse; double * GetBestResponse(); void SetBestResponse(double * pBestResponse); - + /** * Allocate memory needed for mapping * @param rangeThreshold From 6746c3d0ed7a3638667ffd294ee7abd1e8aa0e51 Mon Sep 17 00:00:00 2001 From: Renbago Date: Thu, 12 Sep 2024 09:22:06 +0300 Subject: [PATCH 33/79] Added setParams and renamed service will be cleaned later --- .../slam_toolbox_localization.hpp | 2 +- lib/karto_sdk/include/karto_sdk/Mapper.h | 4 +- src/slam_toolbox_localization.cpp | 70 +++++++++++-------- 3 files changed, 44 insertions(+), 32 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index 96034c6f3..9ce7af58b 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -43,7 +43,7 @@ class LocalizationSlamToolbox : public SlamToolbox const std::shared_ptr req, std::shared_ptr resp); - bool getBestResponseCallback( + bool desiredPoseCheck( const std::shared_ptr req, std::shared_ptr res); diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index d53d6416d..142823a3b 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -2164,9 +2164,9 @@ class KARTO_EXPORT Mapper : public Module public: void SetUseScanMatching(kt_bool val) {m_pUseScanMatching->SetValue(val);} - -protected: kt_bool m_Initialized; +protected: + kt_bool m_Deserialized; ScanMatcher * m_pSequentialScanMatcher; diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 788a02dfb..651319bc1 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -41,7 +41,7 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) ssGetBestResponse_ = this->create_service( "slam_toolbox/get_best_response", - std::bind(&LocalizationSlamToolbox::getBestResponseCallback, this, + std::bind(&LocalizationSlamToolbox::desiredPoseCheck, this, std::placeholders::_1, std::placeholders::_2)); @@ -227,7 +227,7 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( } /*****************************************************************************/ -bool LocalizationSlamToolbox::getBestResponseCallback( +bool LocalizationSlamToolbox::desiredPoseCheck( const std::shared_ptr req, std::shared_ptr res) /*****************************************************************************/ @@ -250,6 +250,7 @@ bool LocalizationSlamToolbox::getBestResponseCallback( double initial_correlation_search_space_smear_deviation = this->get_parameter("correlation_search_space_smear_deviation").as_double(); double initial_loop_search_space_dimension = this->get_parameter("loop_search_space_dimension").as_double(); + double initial_loop_search_maximum_distance = this->get_parameter("loop_search_maximum_distance").as_double(); // double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); // double initial_loop_search_space_smear_deviation = this->get_parameter("loop_search_space_smear_deviation").as_double(); @@ -259,19 +260,30 @@ bool LocalizationSlamToolbox::getBestResponseCallback( double initial_do_relocalization = this->get_parameter("position_search_do_relocalization").as_bool(); double initial_minimum_best_response = this->get_parameter("position_search_minimum_best_response").as_double(); + double initial_minimum_link_best_response = this->get_parameter("link_match_minimum_response_fine").as_double(); /* * Setting initial_definitions. */ - smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(position_search_distance_); - smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(position_search_resolution_); - smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(position_search_smear_deviation_); - smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance_); - smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset_); - smapper_->getMapper()->setParamCoarseSearchAngleOffset(position_search_coarse_angle_offset_); - smapper_->getMapper()->setParamCoarseAngleResolution(position_search_coarse_angle_resolution_); - + { + boost::mutex::scoped_lock lock(smapper_mutex_); + smapper_->getMapper()->setParamLoopSearchSpaceDimension(2*position_search_distance_); + smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_); + smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset_); + smapper_->getMapper()->setParamCoarseSearchAngleOffset(position_search_coarse_angle_offset_); + smapper_->getMapper()->setParamCoarseAngleResolution(position_search_coarse_angle_resolution_); + smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(position_search_minimum_best_response_); + + if (req->search_distance != 0.0) { + position_search_distance_ = req->search_distance; + smapper_->getMapper()->setParamLoopSearchSpaceDimension(2*position_search_distance_); + smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_); + } + + smapper_->getMapper()->m_Initialized = false; + std::cout << "m_Initialized: " << smapper_->getMapper()->m_Initialized << std::endl; + } if (req->pose_x == 0.0 || req->pose_y == 0.0) { RCLCPP_ERROR(get_logger(), "Error: pose_x or pose_y is not provided."); @@ -288,10 +300,6 @@ bool LocalizationSlamToolbox::getBestResponseCallback( initial_do_relocalization = false; } - if (req->search_distance != 0.0) { - position_search_distance_ = req->search_distance; - this->set_parameter(rclcpp::Parameter("correlation_search_space_dimension", position_search_distance_)); - } if (req->minimum_best_response != 0.0) { initial_minimum_best_response = req->minimum_best_response; @@ -333,14 +341,15 @@ bool LocalizationSlamToolbox::getBestResponseCallback( publishPose(range_scan->GetCorrectedPose(), covariance, last_scan_stored_->header.stamp); } - smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); - smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); - smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); - smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); - smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); - smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); - smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); - + // skartomapper_->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); + // skartomapper_->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); + // skartomapper_->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); + // skartomapper_->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); + // skartomapper_->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); + // skartomapper_->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); + // skartomapper_->setParamCoarseAngleResolution(initial_coarse_angle_resolution); + // skartomapper_->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); + // smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); return true; } else { @@ -350,13 +359,16 @@ bool LocalizationSlamToolbox::getBestResponseCallback( boost::mutex::scoped_lock lock(smapper_mutex_); smapper_->clearLocalizationBuffer(); } - smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); - smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); - smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); - smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); - smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); - smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); - smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); + // skartomapper_->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); + // skartomapper_->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); + // skartomapper_->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); + // skartomapper_->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); + // skartomapper_->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); + // skartomapper_->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); + // skartomapper_->setParamCoarseAngleResolution(initial_coarse_angle_resolution); + // skartomapper_->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); + // smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); + return false; } } From c8c6cd7388928999878dd48150df3c93ed421d0b Mon Sep 17 00:00:00 2001 From: Renbago Date: Thu, 12 Sep 2024 09:24:24 +0300 Subject: [PATCH 34/79] renamed service topic --- src/slam_toolbox_localization.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 651319bc1..d3ff638c7 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -40,7 +40,7 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); ssGetBestResponse_ = this->create_service( - "slam_toolbox/get_best_response", + "slam_toolbox/desired_pose_check", std::bind(&LocalizationSlamToolbox::desiredPoseCheck, this, std::placeholders::_1, std::placeholders::_2)); From e12ac488dcb06915cc0ba651c9cba8f372df64f2 Mon Sep 17 00:00:00 2001 From: Renbago Date: Thu, 19 Sep 2024 08:46:24 +0300 Subject: [PATCH 35/79] implemented desired_pose inside of addscan, will be rethink it its demo version --- lib/karto_sdk/src/Mapper.cpp | 209 ++++++++++++++++-- src/slam_toolbox_localization.cpp | 338 ++++++++++++++++-------------- 2 files changed, 372 insertions(+), 175 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index f1ccfe96b..049583224 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -47,7 +47,7 @@ namespace karto { // enable this for verbose debug information -// #define KARTO_DEBUG +#define KARTO_DEBUG #define MAX_VARIANCE 500.0 #define DISTANCE_PENALTY_GAIN 0.2 @@ -478,6 +478,8 @@ ScanMatcher * ScanMatcher::Create( Mapper * pMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold) { + // TODO: Baha Check Here + // invalid parameters if (resolution <= 0) { return NULL; @@ -576,9 +578,15 @@ kt_double ScanMatcher::MatchScan( /////////////////////////////////////// - // set up correlation grid + //TODO: Baha LOOK HERE + + // set up correlation grid + + std::cout << "scanPose.GetPosition() values " << scanPose.GetPosition() << std::endl; AddScans(rBaseScans, scanPose.GetPosition()); + std::cout << "m_pSearchSpaceProbs width " << m_pSearchSpaceProbs->GetWidth() << std::endl; + std::cout << "m_pSearchSpaceProbs height " << m_pSearchSpaceProbs->GetHeight() << std::endl; // compute how far to search in each direction Vector2 searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight()); @@ -586,6 +594,10 @@ kt_double ScanMatcher::MatchScan( m_pCorrelationGrid->GetResolution(), 0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution()); + std::cout << "m_pCorrelationGrid " << m_pCorrelationGrid->GetResolution() << std::endl; + std::cout << "searchDimensions " << searchDimensions << std::endl; + std::cout << "coarseSearchOffset " << coarseSearchOffset << std::endl; + // a coarse search only checks half the cells in each dimension Vector2 coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(), 2 * m_pCorrelationGrid->GetResolution()); @@ -625,6 +637,7 @@ kt_double ScanMatcher::MatchScan( } if (doRefineMatch) { + std::cout << "\ndoing refine match" << std::endl; Vector2 fineSearchOffset(coarseSearchResolution * 0.5); Vector2 fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution()); @@ -650,7 +663,7 @@ kt_double ScanMatcher::MatchScan( return bestResponse; } - +//TODO the scanmatch algoritm running is here check it BAHA void ScanMatcher::operator()(const kt_double & y) const { kt_int32u poseResponseCounter; @@ -729,6 +742,14 @@ kt_double ScanMatcher::CorrelateScan( kt_double searchAngleOffset, kt_double searchAngleResolution, kt_bool doPenalize, Pose2 & rMean, Matrix3 & rCovariance, kt_bool doingFineMatch) { + + std::cout << "\n\ninside of CorrelateScan" << std::endl; + std::cout << "rSearchCenter: " << rSearchCenter << std::endl; + std::cout << "rSearchSpaceOffset: " << rSearchSpaceOffset << std::endl; + std::cout << "rSearchSpaceResolution: " << rSearchSpaceResolution << std::endl; + std::cout << "searchAngleOffset: " << searchAngleOffset << std::endl; + std::cout << "searchAngleResolution: " << searchAngleResolution << std::endl; + assert(searchAngleResolution != 0.0); // setup lookup arrays @@ -764,6 +785,21 @@ kt_double ScanMatcher::CorrelateScan( } assert(math::DoubleEqual(m_yPoses.back(), -startY)); + // mx ve my poses benim grid size ımdaki resamplelanmış pose ları tekabul ediyor + + // std::cout << "m_xPoses: "; + // for (const auto& x : m_xPoses) { + // std::cout << x << " "; + // } + // std::cout << std::endl; + + // std::cout << "m_yPoses: "; + // for (const auto& y : m_yPoses) { + // std::cout << y << " "; + // } + // std::cout << std::endl; + + // calculate pose response array size kt_int32u nAngles = static_cast(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1); @@ -777,6 +813,10 @@ kt_double ScanMatcher::CorrelateScan( m_pCorrelationGrid->WorldToGrid(Vector2(rSearchCenter.GetX() + startX, rSearchCenter.GetY() + startY)); + std::cout << "startGridPoint: (" << startGridPoint.GetX() << ", " + << startGridPoint.GetY() << ")" << std::endl; + std::cout << "doingFineMatch: " << doingFineMatch << std::endl; + // this isn't good but its the fastest way to iterate. Should clean up later. m_rSearchCenter = rSearchCenter; m_searchAngleOffset = searchAngleOffset; @@ -787,9 +827,10 @@ kt_double ScanMatcher::CorrelateScan( // find value of best response (in [0; 1]) kt_double bestResponse = -1; + for (kt_int32u i = 0; i < poseResponseSize; i++) { - bestResponse = math::Maximum(bestResponse, m_pPoseResponse[i].first); - + bestResponse = math::Maximum(bestResponse, m_pPoseResponse[i].first); + // std::cout << "bestresponse " << m_pPoseResponse[i].first << "pose" << m_pPoseResponse[i].second.GetPosition() << "heading" << m_pPoseResponse[i].second.GetHeading() << std::endl; // will compute positional covariance, save best relative probability for each cell if (!doingFineMatch) { const Pose2 & rPose = m_pPoseResponse[i].second; @@ -811,13 +852,14 @@ kt_double ScanMatcher::CorrelateScan( *ptr = math::Maximum(m_pPoseResponse[i].first, *ptr); } } - + //TODO CHECK HERE // average all poses with same highest response Vector2 averagePosition; kt_double thetaX = 0.0; kt_double thetaY = 0.0; kt_int32s averagePoseCount = 0; for (kt_int32u i = 0; i < poseResponseSize; i++) { + // std::cout<< "m_pPoseResponse[i].second.GetPosition();"<< m_pPoseResponse[i].second.GetPosition() << std::endl; if (math::DoubleEqual(m_pPoseResponse[i].first, bestResponse)) { averagePosition += m_pPoseResponse[i].second.GetPosition(); @@ -861,9 +903,11 @@ kt_double ScanMatcher::CorrelateScan( rMean = averagePose; #ifdef KARTO_DEBUG - std::cout << "bestPose: " << averagePose << std::endl; + std::cout << "averagePose: " << averagePose << std::endl; + std::cout << "bestResponse: " << bestResponse << std::endl; #endif + if (bestResponse > 1.0) { bestResponse = 1.0; } @@ -1089,6 +1133,13 @@ void ScanMatcher::AddScan( { PointVectorDouble validPoints = FindValidPoints(pScan, rViewPoint); + if (!validPoints.empty()) { + std::cout << "validPoints: (" << validPoints.begin()->GetX() << ", " + << validPoints.begin()->GetY() << ")" << std::endl; + } else { + std::cout << "validPoints is empty!" << std::endl; + } + // put in all valid points const_forEach(PointVectorDouble, &validPoints) { @@ -1109,7 +1160,6 @@ void ScanMatcher::AddScan( } m_pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied; - // smear grid if (doSmear == true) { m_pCorrelationGrid->SmearPoint(gridPoint); @@ -1149,6 +1199,7 @@ PointVectorDouble ScanMatcher::FindValidPoints( } Vector2 delta = firstPoint - currentPoint; + if (delta.SquaredLength() > minSquareDistance) { // This compute the Determinant (viewPoint FirstPoint, viewPoint currentPoint) // Which computes the direction of rotation, if the rotation is counterclock @@ -1533,6 +1584,9 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens #endif Pose2 bestPose; Matrix3 covariance; + + // std::cout <<"pScan->GetSensorPose() " << pScan->GetSensorPose() << std::endl; + kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false, false); @@ -1559,6 +1613,13 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1) << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")"; + std::cout << "Coarse Response: " << coarseResponse << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")" << std::endl; + + std::cout << "search_space_dimension " << m_pMapper->m_pCorrelationSearchSpaceDimension->GetValue() << std::endl; + std::cout << "loop_search_dimension " << m_pMapper->m_pLoopSearchSpaceDimension->GetValue() << std::endl; + std::cout << "search_space_resolution " << m_pMapper->m_pCorrelationSearchSpaceResolution->GetValue() << std::endl; + std::cout << "search_space_smear_deviation " << m_pMapper->m_pCorrelationSearchSpaceSmearDeviation->GetValue() << std::endl; + m_pMapper->FireLoopClosureCheck(stream.str()); if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) && @@ -1566,6 +1627,9 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())) { +#ifdef KARTO_DEBUG + std::cout << "\n\n\nLast founded pose and response are fine checking fineresponse\n\n\n " << std::endl; +#endif LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector()); tmpScan.SetUniqueId(pScan->GetUniqueId()); tmpScan.SetTime(pScan->GetTime()); @@ -1586,25 +1650,85 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens } else { m_pMapper->FireBeginLoopClosure("Closing loop..."); + double best_response = fineResponse; + + try + { + m_pMapper->SetBestResponse(&best_response); + } + catch (std::exception & e) { + throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - "kim + "unable to publish set best response!"); + } + pScan->SetSensorPose(bestPose); LinkChainToScan(candidateChain, pScan, bestPose, covariance); + +#ifdef KARTO_DEBUG + std::cout << "\n\n\nLoop Closed!, starting pose correction\n\n\n " << std::endl; + std::cout << "pscan sensor pose: " << pScan->GetSensorPose() << std::endl; + std::cout << "pscan corrected pose: " << pScan->GetCorrectedPose() << std::endl; + std::cout << "pscan odometric pose: " << pScan->GetOdometricPose() << std::endl; +#endif CorrectPoses(); +#ifdef KARTO_DEBUG + std::cout << "pscan sensor pose: " << pScan->GetSensorPose() << std::endl; + std::cout << "pscan corrected pose: " << pScan->GetCorrectedPose() << std::endl; + std::cout << "pscan odometric pose: " << pScan->GetOdometricPose() << std::endl; + std::cout << "\n\n\nLoop Closed!, finished pose correction\n\n\n " << std::endl; +#endif m_pMapper->FireEndLoopClosure("Loop closed!"); loopClosed = true; + // TODO baha added here EKleme sebebim uzunca şudur: + // İlk önce robot loopclosure paramereleri ile matchscan atıyor bu değer 0.35 ve cov düşük ise while döngüsü içerisine giriyor. + // bu while döngüsü içerisinde bulunduğu konumu referans alaraktan tekrarda bir matchscan atıyor bu değer 0.45 den yüksk ise + // loopclosure yapıyor ve TRUE diyor. devamında ise tekrardan findpossibleloopclosure a giriyor şimdi burada ne oluyor peki + // tekrardan girince yeni chain ler buluyor ve bu yeni bulduğu chainlere göre tekrardan işleme tabi tutuyor burada referans atıyorum. + // terminal referansı: + //Checking of the candidatecChainsize inside of Loop CLosure: 7 + // Doing MatchScan + + // scanPose.GetPosition() values 0.605067 4.825724 + // validPoints: (-3.218281, 4.160619) + // validPoints: (-inf, inf) + // validPoints: (-inf, inf) + // validPoints: (-1.570163, 3.980981) + // validPoints: (-inf, inf) + // validPoints: (-inf, inf) + // validPoints: (-inf, inf) + // kod fiinish + + // burada ise tekrarda amtch arayınca buradaki verilere göre + + // bestPose: -4.594933 0.625724 -0.244379 + // bestResponse: 0.250306 + // averagePose: -4.594933 0.625724 -0.244379 + // bestResponse: 0.250306 + // BEST POSE = -4.594933 0.625724 -0.244379 BEST RESPONSE = 0.250306, + + // inanılmaz saçma bir depğer atıyor.. fakat zaten beim konumum doğruı ve 0.65 vemrişti ilk healtte niye tekrardan hesapolamaya giriyor + // byunu çözmek için direkt loopClosed değerini returnledim burada. ve bu değer true ise döngüden çıkıyor ve işlemi bitiriyor. +#ifdef KARTO_DEBUG + std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; +#endif + + return loopClosed; } } + candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); #ifdef KARTO_DEBUG std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; + std::cout << "pscan sensor pose: " << pScan->GetSensorPose() << std::endl; + end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; std::cout << "----------Calculated time of Matchscan after triggered\n" - << "----------INSIDE OF WHILE LOOPCLOSURE function\n" + << "----------INSIDE OF WHILE LOOPCLOSURE but its false function\n" << "----------elapsed time: " << elapsed_seconds.count() << "s\n"; #endif - candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); } return loopClosed; @@ -2056,7 +2180,24 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( #ifdef KARTO_DEBUG std::cout << "\nnScans size " << nScans << std::endl; - + std::cout << "rStartNum size " << rStartNum << std::endl; + std::cout << "Calculating in this pose: " << pose.GetX() << " " << pose.GetY() << std::endl; + std::cout << "Nearlinkedscans size: " << nearLinkedScans.size() << std::endl; + // std::cout << "Near Linked Scans(Founded chain's):" << std::endl; + // for (const auto& scan : nearLinkedScans) { + // if (scan != nullptr) { // Null pointer kontrolü + // std::cout << "Scan Position: (" + // << scan->GetCorrectedPose().GetX() << ", " + // << scan->GetCorrectedPose().GetY() << ", " + // << scan->GetCorrectedPose().GetHeading() << ")" << std::endl; + + // std::cout << "Scan Timestamp: " << scan->GetTime() << std::endl; + // // Diğer istediğiniz bilgileri ekleyebilirsiniz. + // } + // else { + // std::cout << "Null scan found!" << std::endl; + // } + // } std::chrono::time_point start, end; start = std::chrono::system_clock::now(); #endif @@ -2067,7 +2208,6 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( if (pCandidateScan == NULL) { continue; } - Pose2 candidateScanPose = pCandidateScan->GetReferencePose( m_pMapper->m_pUseScanBarycenter->GetValue()); @@ -2075,6 +2215,12 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( if (squaredDistance < math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE) { + + std::cout << "Candidate Scan Position: (" + << pCandidateScan->GetCorrectedPose().GetX() << ", " + << pCandidateScan->GetCorrectedPose().GetY() << ", " + << pCandidateScan->GetCorrectedPose().GetHeading() << ")" << std::endl; + // a linked scan cannot be in the chain if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end()) @@ -2108,10 +2254,11 @@ void MapperGraph::CorrectPoses() ScanSolver * pSolver = m_pMapper->m_pScanOptimizer; if (pSolver != NULL) { pSolver->Compute(); - + std::cout << "\n\n\n\nCorrectPose executed\n\n\n\n" << std::endl; const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections()) { LocalizedRangeScan * scan = m_pMapper->m_pMapperSensorManager->GetScan(iter->first); + if (scan == NULL) { continue; } @@ -2127,6 +2274,10 @@ void MapperGraph::UpdateLoopScanMatcher(kt_double rangeThreshold) if (m_pLoopScanMatcher) { delete m_pLoopScanMatcher; } + std::cout << "m_pMapper->m_pLoopSearchSpaceDimension->GetValue() " << m_pMapper->m_pLoopSearchSpaceDimension->GetValue() << std::endl; + std::cout << "m_pMapper->m_pLoopSearchSpaceResolution->GetValue() " << m_pMapper->m_pLoopSearchSpaceResolution->GetValue() << std::endl; + std::cout << "m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue() " << m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue() << std::endl; + m_pLoopScanMatcher = ScanMatcher::Create(m_pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), @@ -2667,16 +2818,19 @@ void Mapper::setParamAngleVariancePenalty(double d) void Mapper::setParamFineSearchAngleOffset(double d) { m_pFineSearchAngleOffset->SetValue((kt_double)d); + std::cout << "m_pFineSearchAngleOffset: " << *m_pFineSearchAngleOffset << std::endl; } void Mapper::setParamCoarseSearchAngleOffset(double d) { m_pCoarseSearchAngleOffset->SetValue((kt_double)d); + std::cout << "m_pCoarseSearchAngleOffset: " << *m_pCoarseSearchAngleOffset << std::endl; } void Mapper::setParamCoarseAngleResolution(double d) { m_pCoarseAngleResolution->SetValue((kt_double)d); + std::cout << "m_pCoarseAngleResolution: " << *m_pCoarseAngleResolution << std::endl; } void Mapper::setParamMinimumAnglePenalty(double d) @@ -2701,6 +2855,11 @@ void Mapper::Initialize(kt_double rangeThreshold) return; } // create sequential scan and loop matcher, update if deserialized + + std::cout << "m_pCorrelationSearchSpaceDimension: " << *m_pCorrelationSearchSpaceDimension << std::endl; + std::cout << "m_pCorrelationSearchSpaceResolution: " << *m_pCorrelationSearchSpaceResolution << std::endl; + std::cout << "m_pCorrelationSearchSpaceSmearDeviation: " << *m_pCorrelationSearchSpaceSmearDeviation << std::endl; + if (m_pSequentialScanMatcher) { delete m_pSequentialScanMatcher; @@ -2870,6 +3029,7 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad return false; } + std::cout <<"m_Initialized: " << m_Initialized << std::endl; if (m_Initialized == false) { // initialize mapper with range threshold from device Initialize(pLaserRangeFinder->GetRangeThreshold()); @@ -2911,8 +3071,6 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad pScan->SetSensorPose(bestPose); } - pScan->SetOdometricPose(pScan->GetCorrectedPose()); - if (covariance) { *covariance = cov; } @@ -2920,6 +3078,8 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad // add scan to buffer and assign id m_pMapperSensorManager->AddScan(pScan); + std::cout <<"m_pUseScanMatching: " << m_pUseScanMatching->GetValue() << std::endl; + std::cout <<"DoLoopClosing: " << m_pDoLoopClosing->GetValue() << std::endl; Vertex * scan_vertex = NULL; if (m_pUseScanMatching->GetValue()) { // add to graph @@ -2937,21 +3097,24 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad } } } + // sebebi robotu bir konuma atadığımız zamnan + pScan->SetOdometricPose(pScan->GetCorrectedPose()); + std::cout <<"\n\n\n\n\n\n\nprocessagainsnotdeby trycloseloop has finished\n\n\n\n\n\n\n\n" << std::endl; m_pMapperSensorManager->SetLastScan(pScan); - + std::cout <<"the last scas's odometric pose "<< pScan->GetOdometricPose().GetX() << " " << pScan->GetOdometricPose().GetY() << std::endl; + std::cout <<"the last scas's corrected pose "<< pScan->GetCorrectedPose().GetX() << " " << pScan->GetCorrectedPose().GetY() << std::endl; if (addScanToLocalizationBuffer) { AddScanToLocalizationBuffer(pScan, scan_vertex); } - return true; } - return false; } kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance) { + if (pScan == NULL) { return false; } @@ -3257,22 +3420,27 @@ kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * { // test if first scan if (pLastScan == NULL) { + std::cout << "First scan" << std::endl; return true; } // test if enough time has passed kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime(); if (timeInterval >= m_pMinimumTimeInterval->GetValue()) { + std::cout << "Time interval: " << timeInterval << std::endl; return true; } Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose()); Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose()); + Pose2 lastCorrectedPose = pLastScan->GetCorrectedPose(); + Pose2 correctedPose = pScan->GetCorrectedPose(); // test if we have turned enough kt_double deltaHeading = math::NormalizeAngle( scannerPose.GetHeading() - lastScannerPose.GetHeading()); if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue()) { + std::cout << "Delta heading: " << deltaHeading << std::endl; return true; } @@ -3280,6 +3448,11 @@ kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance( scannerPose.GetPosition()); if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE) { + std::cout << "\nLast scanner pose: " << lastScannerPose << std::endl; + std::cout << "Scanner pose: " << scannerPose << std::endl; + std::cout << "Last corrected pose: " << lastCorrectedPose << std::endl; + std::cout << "Corrected pose: " << correctedPose << std::endl; + std::cout << "Squared travel distance: " << squaredTravelDistance << std::endl; return true; } diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index d3ff638c7..e2928bfa3 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -187,20 +187,62 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( "valid region request. Ignoring scan."); return nullptr; } - + 21 + std::cout << "LocalizationSlamToolbox: processing nearregion" << std::endl; + // smapper_->getMapper()->setParamDoLoopClosing(true); // set our position to the requested pose and proces range_scan->SetOdometricPose(*process_near_pose_); range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); process_near_pose_.reset(nullptr); processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); - + // smapper_->getMapper()->setParamDoLoopClosing(false); // reset to localization mode update_reprocessing_transform = true; processor_type_ = PROCESS_LOCALIZATION; }else if (processor_type_ == PROCESS_DESIRED_POSE) { - // process_desired_pose_.reset(nullptr); - update_reprocessing_transform = false; - processor_type_ = PROCESS_LOCALIZATION; + + range_scan->SetOdometricPose(*process_desired_pose_); + range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); + std::cout << "Starting desired ProcessAgainstNodesNearBy" << std::endl; + processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); + + if (processed) { + double * best_response = smapper_->getMapper()->GetBestResponse(); + std::cout << "best_response " << *best_response << std::endl; + std::cout << "req->minimum_best_response " << position_search_minimum_best_response_ << std::endl; + if (best_response != nullptr && *best_response > position_search_minimum_best_response_) { + std::cout<< "finded best response, processing localization." << std::endl; + if (position_search_do_relocalization_) { + std::cout << "range_scan->GetCorrectedPose() before " << range_scan->GetCorrectedPose().GetX() << " " << range_scan->GetCorrectedPose().GetY() << " " << range_scan->GetCorrectedPose().GetHeading() << std::endl; + std::cout << "range_scan->GetOdometricPose() before " << range_scan->GetOdometricPose().GetX() << " " << range_scan->GetOdometricPose().GetY() << " " << range_scan->GetOdometricPose().GetHeading() << std::endl; + setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, + scan->header.stamp, true); + + publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp); + + // processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance); + std::cout << "range_scan->GetCorrectedPose() after " << range_scan->GetCorrectedPose().GetX() << " " << range_scan->GetCorrectedPose().GetY() << " " << range_scan->GetCorrectedPose().GetHeading() << std::endl; + std::cout << "range_scan->GetOdometricPose() after " << range_scan->GetOdometricPose().GetX() << " " << range_scan->GetOdometricPose().GetY() << " " << range_scan->GetOdometricPose().GetHeading() << std::endl; + } + // todo add the last pose values and do setodometric and correctedpose again. + else { + range_scan->SetOdometricPose(odom_pose); + range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); + } + } + smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(this->get_parameter("correlation_search_space_dimension").as_double()); + smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(this->get_parameter("correlation_search_space_resolution").as_double()); + smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(this->get_parameter("correlation_search_space_smear_deviation").as_double()); + smapper_->getMapper()->setParamLoopSearchSpaceDimension(this->get_parameter("loop_search_space_dimension").as_double()); + smapper_->getMapper()->setParamFineSearchAngleOffset(this->get_parameter("fine_search_angle_offset").as_double()); + smapper_->getMapper()->setParamCoarseSearchAngleOffset(this->get_parameter("coarse_search_angle_offset").as_double()); + smapper_->getMapper()->setParamCoarseAngleResolution(this->get_parameter("coarse_angle_resolution").as_double()); + smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(this->get_parameter("link_match_minimum_response_fine").as_double()); + smapper_->getMapper()->setParamLoopSearchMaximumDistance(this->get_parameter("loop_search_maximum_distance").as_double()); + smapper_->getMapper()->setParamDoLoopClosing(this->get_parameter("do_loop_closing").as_bool()); + } + process_desired_pose_.reset(nullptr); + }else if (processor_type_ == PROCESS_LOCALIZATION) { processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance); update_reprocessing_transform = false; @@ -210,17 +252,21 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( "No valid processor type set! Exiting."); exit(-1); } - // if successfully processed, create odom to map transformation + if (!processed) { delete range_scan; range_scan = nullptr; } - else { - // compute our new transform - setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, - scan->header.stamp, update_reprocessing_transform); + else{ + if (processor_type_ != PROCESS_DESIRED_POSE) { + setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, + scan->header.stamp, update_reprocessing_transform); - publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp); + publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp); + } + } + if (processor_type_ == PROCESS_DESIRED_POSE){ + processor_type_ = PROCESS_LOCALIZATION; } return range_scan; @@ -234,170 +280,148 @@ bool LocalizationSlamToolbox::desiredPoseCheck( { std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now(); std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); - Matrix3 covariance; - LocalizedRangeScan * range_scan = nullptr; - covariance.SetToIdentity(); - { - boost::mutex::scoped_lock lock(smapper_mutex_); - smapper_->clearLocalizationBuffer(); - } - - /* - Yaml parameters definitions for setting the parameters after process finished - */ - double initial_correlation_search_space_dimension = this->get_parameter("correlation_search_space_dimension").as_double(); - double initial_correlation_search_space_resolution = this->get_parameter("correlation_search_space_resolution").as_double(); - double initial_correlation_search_space_smear_deviation = this->get_parameter("correlation_search_space_smear_deviation").as_double(); + - double initial_loop_search_space_dimension = this->get_parameter("loop_search_space_dimension").as_double(); - double initial_loop_search_maximum_distance = this->get_parameter("loop_search_maximum_distance").as_double(); - // double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); - // double initial_loop_search_space_smear_deviation = this->get_parameter("loop_search_space_smear_deviation").as_double(); + if (req->pose_x == 0.0 || req->pose_y == 0.0) { + RCLCPP_ERROR(get_logger(), "Error: pose_x or pose_y is not provided."); + res->message = "Error: pose_x or pose_y is missing. Please use it"; + res->success = false; + return false; + } - double initial_fine_search_angle_offset = this->get_parameter("fine_search_angle_offset").as_double(); - double initial_coarse_search_angle_offset = this->get_parameter("coarse_search_angle_offset").as_double(); - double initial_coarse_angle_resolution = this->get_parameter("coarse_angle_resolution").as_double(); + if (req->do_relocalization){ + RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Searching for best response with relocalize"); + position_search_do_relocalization_ = true; + } else { + RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Searching for best response without relocalize"); + position_search_do_relocalization_ = false; + } - double initial_do_relocalization = this->get_parameter("position_search_do_relocalization").as_bool(); - double initial_minimum_best_response = this->get_parameter("position_search_minimum_best_response").as_double(); - double initial_minimum_link_best_response = this->get_parameter("link_match_minimum_response_fine").as_double(); + { + boost::mutex::scoped_lock l(pose_mutex_); + process_desired_pose_ = std::make_unique(req->pose_x, req->pose_y, 0.0); + } - /* - * Setting initial_definitions. - */ + first_measurement_ = true; { boost::mutex::scoped_lock lock(smapper_mutex_); - smapper_->getMapper()->setParamLoopSearchSpaceDimension(2*position_search_distance_); - smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_); + smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance_); + smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_-1); smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset_); smapper_->getMapper()->setParamCoarseSearchAngleOffset(position_search_coarse_angle_offset_); smapper_->getMapper()->setParamCoarseAngleResolution(position_search_coarse_angle_resolution_); smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(position_search_minimum_best_response_); - + smapper_->getMapper()->setParamLoopSearchSpaceResolution(position_search_resolution_); + smapper_->getMapper()->setParamLoopSearchSpaceSmearDeviation(position_search_smear_deviation_); + // smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(position_search_distance_); + smapper_->getMapper()->setParamDoLoopClosing(true); + smapper_->getMapper()->m_Initialized = false; + if (req->search_distance != 0.0) { position_search_distance_ = req->search_distance; - smapper_->getMapper()->setParamLoopSearchSpaceDimension(2*position_search_distance_); - smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_); + // smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance_); + // smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_-1); } - - smapper_->getMapper()->m_Initialized = false; - std::cout << "m_Initialized: " << smapper_->getMapper()->m_Initialized << std::endl; - } - - if (req->pose_x == 0.0 || req->pose_y == 0.0) { - RCLCPP_ERROR(get_logger(), "Error: pose_x or pose_y is not provided."); - res->message = "Error: pose_x or pose_y is missing. Please use it"; - res->success = false; - return false; + + smapper_->clearLocalizationBuffer(); } - if (req->do_relocalization){ - RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Searching for best response with relocalize"); - initial_do_relocalization = true; - } else { - RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Searching for best response without relocalize"); - initial_do_relocalization = false; - } - - if (req->minimum_best_response != 0.0) { - initial_minimum_best_response = req->minimum_best_response; - this->set_parameter(rclcpp::Parameter("position_search_minimum_best_response", initial_minimum_best_response)); - } + res->message = "processor_type_ changed to DESIRED_POSE"; + res->success = true; + return false; - if (!have_scan_values_) { - res->message = "No scan values stored try later"; - res->success = false; - return false; - } - else{ - // for (double angle = 0.0; angle <= 360.0; angle += angle_resolution) { - bool processed = false; - { - boost::mutex::scoped_lock l(pose_mutex_); - process_desired_pose_ = std::make_unique(req->pose_x, req->pose_y, 0.0); - range_scan = getLocalizedRangeScan(last_laser_stored_, last_scan_stored_, last_odom_pose_stored_); - - boost::mutex::scoped_lock lock(smapper_mutex_); - range_scan->SetOdometricPose(*process_desired_pose_); - range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); - processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); - } - - if (processed) { - double * best_response = smapper_->getMapper()->GetBestResponse(); - std::cout << "best_response " << *best_response << std::endl; - std::cout << "req->minimum_best_response " << initial_minimum_best_response << std::endl; - if (best_response != nullptr && *best_response > initial_minimum_best_response) { - std::cout<< "finded best response" << std::endl; - res->message = std::to_string(*best_response); - res->success = true; - if (initial_do_relocalization) { - - setTransformFromPoses(range_scan->GetCorrectedPose(), last_odom_pose_stored_, - last_scan_stored_->header.stamp, true); - - publishPose(range_scan->GetCorrectedPose(), covariance, last_scan_stored_->header.stamp); - } - - // skartomapper_->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); - // skartomapper_->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); - // skartomapper_->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); - // skartomapper_->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); - // skartomapper_->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); - // skartomapper_->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); - // skartomapper_->setParamCoarseAngleResolution(initial_coarse_angle_resolution); - // skartomapper_->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); - // smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); - - return true; - } else { - res->message = "Couldn't find bestResponse"; - res->success = false; - { - boost::mutex::scoped_lock lock(smapper_mutex_); - smapper_->clearLocalizationBuffer(); - } - // skartomapper_->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); - // skartomapper_->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); - // skartomapper_->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); - // skartomapper_->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); - // skartomapper_->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); - // skartomapper_->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); - // skartomapper_->setParamCoarseAngleResolution(initial_coarse_angle_resolution); - // skartomapper_->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); - // smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); - - return false; - } - } - // else { - // res->message = "Couldn't process scan will try again"; - // end = std::chrono::high_resolution_clock::now(); - // auto time_elapsed = std::chrono::duration_cast(end - start).count(); - // if (time_elapsed > process_timeout) { - // res->message = "Couldnt process scan in " + std::to_string(process_timeout) + " seconds"; - // res->success = false; - // this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); - - // return false; - // } - // } - - // end = std::chrono::high_resolution_clock::now(); - // auto time_elapsed = std::chrono::duration_cast(end - start).count(); - // if (time_elapsed > process_timeout) { - // res->message = "Couldnt find bestResponse in " + std::to_string(process_timeout) + " seconds"; - // res->success = false; - // this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); - // return false; - // } - // } - res->message = "Couldn't find with this resolution at desired time, halving the angle_resolution and decreasing best_response then search again."; - res->success = false; - return false; - } + // RCLCPP_INFO(get_logger(), + // "LocalizePoseCallback: Localizing to: (%0.2f %0.2f), theta=%0.2f", + // msg->pose.pose.position.x, msg->pose.pose.position.y, + // tf2::getYaw(msg->pose.pose.orientation)); + + // if (!have_scan_values_) { + // res->message = "No scan values stored try later"; + // res->success = false; + // return false; + // } + // else{ + // // for (double angle = 0.0; angle <= 360.0; angle += angle_resolution) { + // bool processed = false; + // { + // boost::mutex::scoped_lock l(pose_mutex_); + // process_desired_pose_ = std::make_unique(req->pose_x, req->pose_y, 0.0); + // range_scan = getLocalizedRangeScan(last_laser_stored_, last_scan_stored_, last_odom_pose_stored_); + + // // first_measurement_ = true; + // boost::mutex::scoped_lock lock(smapper_mutex_); + // range_scan->SetOdometricPose(*process_desired_pose_); + // range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); + // std::cout << "Starting ProcessAgainstNodesNearBy" << std::endl; + // processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); + // std::cout << "Finished ProcessAgainstNodesNearBy\n\n\n\n\n\n\n" << std::endl; + + // if (processed) { + // double * best_response = smapper_->getMapper()->GetBestResponse(); + // std::cout << "best_response " << *best_response << std::endl; + // std::cout << "req->minimum_best_response " << initial_minimum_best_response << std::endl; + // if (best_response != nullptr && *best_response > initial_minimum_best_response) { + // std::cout<< "finded best response, processing localization." << std::endl; + + // if (initial_do_relocalization) { + // std::cout << "range_scan->GetCorrectedPose() before" << range_scan->GetCorrectedPose().GetX() << " " << range_scan->GetCorrectedPose().GetY() << " " << range_scan->GetCorrectedPose().GetHeading() << std::endl; + // std::cout << "range_scan->GetOdometricPose() before" << range_scan->GetOdometricPose().GetX() << " " << range_scan->GetOdometricPose().GetY() << " " << range_scan->GetOdometricPose().GetHeading() << std::endl; + // setTransformFromPoses(range_scan->GetCorrectedPose(), last_odom_pose_stored_, + // last_scan_stored_->header.stamp, true); + + // publishPose(range_scan->GetCorrectedPose(), covariance, last_scan_stored_->header.stamp); + // processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance); + // std::cout << "range_scan->GetCorrectedPose() after" << range_scan->GetCorrectedPose().GetX() << " " << range_scan->GetCorrectedPose().GetY() << " " << range_scan->GetCorrectedPose().GetHeading() << std::endl; + // std::cout << "range_scan->GetOdometricPose() after " << range_scan->GetOdometricPose().GetX() << " " << range_scan->GetOdometricPose().GetY() << " " << range_scan->GetOdometricPose().GetHeading() << std::endl; + // addScan(last_laser_stored_, last_scan_stored_, last_odom_pose_stored_); + // } + // // todo add the last pose values and do setodometric and correctedpose again. + // else { + // range_scan->SetOdometricPose(last_odom_pose_stored_); + // range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); + // } + // smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); + // smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); + // smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); + // smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); + // smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); + // smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); + // smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); + // smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); + // smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); + // smapper_->getMapper()->setParamDoLoopClosing(initial_do_loop_closing_value); + + // res->message = std::to_string(*best_response); + // res->success = true; + // return true; + // } else { + // // { + // // boost::mutex::scoped_lock lock(smapper_mutex_); + // smapper_->clearLocalizationBuffer(); + // // } + + // smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); + // smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); + // smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); + // smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); + // smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); + // smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); + // smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); + // smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); + // smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); + // smapper_->getMapper()->setParamDoLoopClosing(initial_do_loop_closing_value); + // res->message = "Couldn't find bestResponse"; + // res->success = false; + // return false; + // } + // } + // } + // res->message = "Couldn't find with this resolution at desired time, halving the angle_resolution and decreasing best_response then search again."; + // res->success = false; + // return false; + // } } /*****************************************************************************/ From f9142044a14f6f244bac2f502d5a936105d4c5dc Mon Sep 17 00:00:00 2001 From: Renbago Date: Thu, 19 Sep 2024 09:44:49 +0300 Subject: [PATCH 36/79] fix --- lib/karto_sdk/src/Mapper.cpp | 41 +++++++++++++++++++----------------- src/slam_toolbox_common.cpp | 21 ------------------ 2 files changed, 22 insertions(+), 40 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 049583224..fbb653649 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1584,9 +1584,6 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens #endif Pose2 bestPose; Matrix3 covariance; - - // std::cout <<"pScan->GetSensorPose() " << pScan->GetSensorPose() << std::endl; - kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false, false); @@ -1612,16 +1609,26 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens std::endl; stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1) << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")"; - +#ifdef KARTO_DEBUG std::cout << "Coarse Response: " << coarseResponse << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")" << std::endl; std::cout << "search_space_dimension " << m_pMapper->m_pCorrelationSearchSpaceDimension->GetValue() << std::endl; std::cout << "loop_search_dimension " << m_pMapper->m_pLoopSearchSpaceDimension->GetValue() << std::endl; std::cout << "search_space_resolution " << m_pMapper->m_pCorrelationSearchSpaceResolution->GetValue() << std::endl; std::cout << "search_space_smear_deviation " << m_pMapper->m_pCorrelationSearchSpaceSmearDeviation->GetValue() << std::endl; +#endif m_pMapper->FireLoopClosureCheck(stream.str()); + double coarsedResponse = coarseResponse; + try + { + m_pMapper->SetBestResponse(&coarsedResponse); + } + catch (std::exception & e) { + std::cout << "Exception caught: " << e.what() << std::endl; + } + if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) && (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) && (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())) @@ -1640,6 +1647,15 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens candidateChain, bestPose, covariance, false); + double finedResponse = fineResponse; + try + { + m_pMapper->SetBestResponse(&finedResponse); + } + catch (std::exception & e) { + std::cout << "Exception caught: " << e.what() << std::endl; + } + std::stringstream stream1; stream1 << "FINE RESPONSE: " << fineResponse << " (>" << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl; @@ -1666,6 +1682,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens #ifdef KARTO_DEBUG std::cout << "\n\n\nLoop Closed!, starting pose correction\n\n\n " << std::endl; + std::cout << "\n bestPose: " << bestPose << " bestResponse: " << best_response << std::endl; std::cout << "pscan sensor pose: " << pScan->GetSensorPose() << std::endl; std::cout << "pscan corrected pose: " << pScan->GetCorrectedPose() << std::endl; std::cout << "pscan odometric pose: " << pScan->GetOdometricPose() << std::endl; @@ -1722,6 +1739,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens #ifdef KARTO_DEBUG std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; std::cout << "pscan sensor pose: " << pScan->GetSensorPose() << std::endl; + std::cout << "pscan odometric pose: " << pScan->GetOdometricPose() << std::endl; end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; @@ -2183,21 +2201,6 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( std::cout << "rStartNum size " << rStartNum << std::endl; std::cout << "Calculating in this pose: " << pose.GetX() << " " << pose.GetY() << std::endl; std::cout << "Nearlinkedscans size: " << nearLinkedScans.size() << std::endl; - // std::cout << "Near Linked Scans(Founded chain's):" << std::endl; - // for (const auto& scan : nearLinkedScans) { - // if (scan != nullptr) { // Null pointer kontrolü - // std::cout << "Scan Position: (" - // << scan->GetCorrectedPose().GetX() << ", " - // << scan->GetCorrectedPose().GetY() << ", " - // << scan->GetCorrectedPose().GetHeading() << ")" << std::endl; - - // std::cout << "Scan Timestamp: " << scan->GetTime() << std::endl; - // // Diğer istediğiniz bilgileri ekleyebilirsiniz. - // } - // else { - // std::cout << "Null scan found!" << std::endl; - // } - // } std::chrono::time_point start, end; start = std::chrono::system_clock::now(); #endif diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 90be0a75f..366035529 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -295,27 +295,6 @@ void SlamToolbox::publishTransformLoop( { boost::mutex::scoped_lock lock(map_to_odom_mutex_); rclcpp::Time scan_timestamp = scan_header.stamp; - - //TODO: In future, we need to remove from there and create new function named as localizationHealthCheck. - double * best_response =smapper_->getMapper()->GetBestResponse(); - static double previous_best_response = 0.0; - - if (best_response != nullptr && *best_response > 0.02) { - try { - if (*best_response != previous_best_response) { - std_msgs::msg::Float32 msg; - msg.data = static_cast(*best_response); // TODO: Change here - localization_health_pub_->publish(msg); - previous_best_response = *best_response; - } - } - catch (std::exception & e) { - //TODO: Write publisher topic when cannot acces the result of health check . - RCLCPP_ERROR(this->get_logger(), "Exception caught while dereferencing best_response: %s", e.what()); - } - } - // - // Avoid publishing tf with initial 0.0 scan timestamp if (scan_timestamp.seconds() > 0.0 && !scan_header.frame_id.empty()) { geometry_msgs::msg::TransformStamped msg; From 278fa2e0939440866f2b713fbaaaad8746035638 Mon Sep 17 00:00:00 2001 From: Mehmet Baha Dursun Date: Thu, 19 Sep 2024 10:27:58 +0300 Subject: [PATCH 37/79] Update Mapper.cpp --- lib/karto_sdk/src/Mapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index f36cc69e1..df5394579 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1678,7 +1678,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens m_pMapper->SetBestResponse(&best_response); } catch (std::exception & e) { - throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - "kim + throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " "unable to publish set best response!"); } From d8846607eff1e370a01083044e14d677332bd436 Mon Sep 17 00:00:00 2001 From: Renbago Date: Thu, 19 Sep 2024 11:30:22 +0300 Subject: [PATCH 38/79] this version is for testing zorlu if works correctly i will edit all of it no necessery to look at here. --- lib/karto_sdk/src/Mapper.cpp | 212 ++++++++++++++++++++++--- src/slam_toolbox_common.cpp | 21 --- src/slam_toolbox_localization.cpp | 251 +++++++++++++++++------------- 3 files changed, 335 insertions(+), 149 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index f1ccfe96b..8874fc665 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -47,7 +47,7 @@ namespace karto { // enable this for verbose debug information -// #define KARTO_DEBUG +#define KARTO_DEBUG #define MAX_VARIANCE 500.0 #define DISTANCE_PENALTY_GAIN 0.2 @@ -478,6 +478,8 @@ ScanMatcher * ScanMatcher::Create( Mapper * pMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold) { + // TODO: Baha Check Here + // invalid parameters if (resolution <= 0) { return NULL; @@ -576,9 +578,15 @@ kt_double ScanMatcher::MatchScan( /////////////////////////////////////// - // set up correlation grid + //TODO: Baha LOOK HERE + + // set up correlation grid + + std::cout << "scanPose.GetPosition() values " << scanPose.GetPosition() << std::endl; AddScans(rBaseScans, scanPose.GetPosition()); + std::cout << "m_pSearchSpaceProbs width " << m_pSearchSpaceProbs->GetWidth() << std::endl; + std::cout << "m_pSearchSpaceProbs height " << m_pSearchSpaceProbs->GetHeight() << std::endl; // compute how far to search in each direction Vector2 searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight()); @@ -586,6 +594,10 @@ kt_double ScanMatcher::MatchScan( m_pCorrelationGrid->GetResolution(), 0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution()); + std::cout << "m_pCorrelationGrid " << m_pCorrelationGrid->GetResolution() << std::endl; + std::cout << "searchDimensions " << searchDimensions << std::endl; + std::cout << "coarseSearchOffset " << coarseSearchOffset << std::endl; + // a coarse search only checks half the cells in each dimension Vector2 coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(), 2 * m_pCorrelationGrid->GetResolution()); @@ -625,6 +637,7 @@ kt_double ScanMatcher::MatchScan( } if (doRefineMatch) { + std::cout << "\ndoing refine match" << std::endl; Vector2 fineSearchOffset(coarseSearchResolution * 0.5); Vector2 fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution()); @@ -650,7 +663,7 @@ kt_double ScanMatcher::MatchScan( return bestResponse; } - +//TODO the scanmatch algoritm running is here check it BAHA void ScanMatcher::operator()(const kt_double & y) const { kt_int32u poseResponseCounter; @@ -729,6 +742,14 @@ kt_double ScanMatcher::CorrelateScan( kt_double searchAngleOffset, kt_double searchAngleResolution, kt_bool doPenalize, Pose2 & rMean, Matrix3 & rCovariance, kt_bool doingFineMatch) { + + std::cout << "\n\ninside of CorrelateScan" << std::endl; + std::cout << "rSearchCenter: " << rSearchCenter << std::endl; + std::cout << "rSearchSpaceOffset: " << rSearchSpaceOffset << std::endl; + std::cout << "rSearchSpaceResolution: " << rSearchSpaceResolution << std::endl; + std::cout << "searchAngleOffset: " << searchAngleOffset << std::endl; + std::cout << "searchAngleResolution: " << searchAngleResolution << std::endl; + assert(searchAngleResolution != 0.0); // setup lookup arrays @@ -764,6 +785,21 @@ kt_double ScanMatcher::CorrelateScan( } assert(math::DoubleEqual(m_yPoses.back(), -startY)); + // mx ve my poses benim grid size ımdaki resamplelanmış pose ları tekabul ediyor + + // std::cout << "m_xPoses: "; + // for (const auto& x : m_xPoses) { + // std::cout << x << " "; + // } + // std::cout << std::endl; + + // std::cout << "m_yPoses: "; + // for (const auto& y : m_yPoses) { + // std::cout << y << " "; + // } + // std::cout << std::endl; + + // calculate pose response array size kt_int32u nAngles = static_cast(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1); @@ -777,6 +813,10 @@ kt_double ScanMatcher::CorrelateScan( m_pCorrelationGrid->WorldToGrid(Vector2(rSearchCenter.GetX() + startX, rSearchCenter.GetY() + startY)); + std::cout << "startGridPoint: (" << startGridPoint.GetX() << ", " + << startGridPoint.GetY() << ")" << std::endl; + std::cout << "doingFineMatch: " << doingFineMatch << std::endl; + // this isn't good but its the fastest way to iterate. Should clean up later. m_rSearchCenter = rSearchCenter; m_searchAngleOffset = searchAngleOffset; @@ -787,9 +827,10 @@ kt_double ScanMatcher::CorrelateScan( // find value of best response (in [0; 1]) kt_double bestResponse = -1; + for (kt_int32u i = 0; i < poseResponseSize; i++) { - bestResponse = math::Maximum(bestResponse, m_pPoseResponse[i].first); - + bestResponse = math::Maximum(bestResponse, m_pPoseResponse[i].first); + // std::cout << "bestresponse " << m_pPoseResponse[i].first << "pose" << m_pPoseResponse[i].second.GetPosition() << "heading" << m_pPoseResponse[i].second.GetHeading() << std::endl; // will compute positional covariance, save best relative probability for each cell if (!doingFineMatch) { const Pose2 & rPose = m_pPoseResponse[i].second; @@ -811,13 +852,14 @@ kt_double ScanMatcher::CorrelateScan( *ptr = math::Maximum(m_pPoseResponse[i].first, *ptr); } } - + //TODO CHECK HERE // average all poses with same highest response Vector2 averagePosition; kt_double thetaX = 0.0; kt_double thetaY = 0.0; kt_int32s averagePoseCount = 0; for (kt_int32u i = 0; i < poseResponseSize; i++) { + // std::cout<< "m_pPoseResponse[i].second.GetPosition();"<< m_pPoseResponse[i].second.GetPosition() << std::endl; if (math::DoubleEqual(m_pPoseResponse[i].first, bestResponse)) { averagePosition += m_pPoseResponse[i].second.GetPosition(); @@ -861,9 +903,11 @@ kt_double ScanMatcher::CorrelateScan( rMean = averagePose; #ifdef KARTO_DEBUG - std::cout << "bestPose: " << averagePose << std::endl; + std::cout << "averagePose: " << averagePose << std::endl; + std::cout << "bestResponse: " << bestResponse << std::endl; #endif + if (bestResponse > 1.0) { bestResponse = 1.0; } @@ -1089,6 +1133,13 @@ void ScanMatcher::AddScan( { PointVectorDouble validPoints = FindValidPoints(pScan, rViewPoint); + if (!validPoints.empty()) { + std::cout << "validPoints: (" << validPoints.begin()->GetX() << ", " + << validPoints.begin()->GetY() << ")" << std::endl; + } else { + std::cout << "validPoints is empty!" << std::endl; + } + // put in all valid points const_forEach(PointVectorDouble, &validPoints) { @@ -1109,7 +1160,6 @@ void ScanMatcher::AddScan( } m_pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied; - // smear grid if (doSmear == true) { m_pCorrelationGrid->SmearPoint(gridPoint); @@ -1149,6 +1199,7 @@ PointVectorDouble ScanMatcher::FindValidPoints( } Vector2 delta = firstPoint - currentPoint; + if (delta.SquaredLength() > minSquareDistance) { // This compute the Determinant (viewPoint FirstPoint, viewPoint currentPoint) // Which computes the direction of rotation, if the rotation is counterclock @@ -1558,14 +1609,34 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens std::endl; stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1) << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")"; +#ifdef KARTO_DEBUG + std::cout << "Coarse Response: " << coarseResponse << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")" << std::endl; + + std::cout << "search_space_dimension " << m_pMapper->m_pCorrelationSearchSpaceDimension->GetValue() << std::endl; + std::cout << "loop_search_dimension " << m_pMapper->m_pLoopSearchSpaceDimension->GetValue() << std::endl; + std::cout << "search_space_resolution " << m_pMapper->m_pCorrelationSearchSpaceResolution->GetValue() << std::endl; + std::cout << "search_space_smear_deviation " << m_pMapper->m_pCorrelationSearchSpaceSmearDeviation->GetValue() << std::endl; +#endif m_pMapper->FireLoopClosureCheck(stream.str()); + double coarsedResponse = coarseResponse; + try + { + m_pMapper->SetBestResponse(&coarsedResponse); + } + catch (std::exception & e) { + std::cout << "Exception caught: " << e.what() << std::endl; + } + if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) && (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) && (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())) { +#ifdef KARTO_DEBUG + std::cout << "\n\n\nLast founded pose and response are fine checking fineresponse\n\n\n " << std::endl; +#endif LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector()); tmpScan.SetUniqueId(pScan->GetUniqueId()); tmpScan.SetTime(pScan->GetTime()); @@ -1576,6 +1647,15 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens candidateChain, bestPose, covariance, false); + double finedResponse = fineResponse; + try + { + m_pMapper->SetBestResponse(&finedResponse); + } + catch (std::exception & e) { + std::cout << "Exception caught: " << e.what() << std::endl; + } + std::stringstream stream1; stream1 << "FINE RESPONSE: " << fineResponse << " (>" << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl; @@ -1586,25 +1666,87 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens } else { m_pMapper->FireBeginLoopClosure("Closing loop..."); + double best_response = fineResponse; + + try + { + m_pMapper->SetBestResponse(&best_response); + } + catch (std::exception & e) { + throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " + "unable to publish set best response!"); + } + pScan->SetSensorPose(bestPose); LinkChainToScan(candidateChain, pScan, bestPose, covariance); + +#ifdef KARTO_DEBUG + std::cout << "\n\n\nLoop Closed!, starting pose correction\n\n\n " << std::endl; + std::cout << "\n bestPose: " << bestPose << " bestResponse: " << best_response << std::endl; + std::cout << "pscan sensor pose: " << pScan->GetSensorPose() << std::endl; + std::cout << "pscan corrected pose: " << pScan->GetCorrectedPose() << std::endl; + std::cout << "pscan odometric pose: " << pScan->GetOdometricPose() << std::endl; +#endif CorrectPoses(); +#ifdef KARTO_DEBUG + std::cout << "pscan sensor pose: " << pScan->GetSensorPose() << std::endl; + std::cout << "pscan corrected pose: " << pScan->GetCorrectedPose() << std::endl; + std::cout << "pscan odometric pose: " << pScan->GetOdometricPose() << std::endl; + std::cout << "\n\n\nLoop Closed!, finished pose correction\n\n\n " << std::endl; +#endif m_pMapper->FireEndLoopClosure("Loop closed!"); loopClosed = true; + // TODO baha added here EKleme sebebim uzunca şudur: + // İlk önce robot loopclosure paramereleri ile matchscan atıyor bu değer 0.35 ve cov düşük ise while döngüsü içerisine giriyor. + // bu while döngüsü içerisinde bulunduğu konumu referans alaraktan tekrarda bir matchscan atıyor bu değer 0.45 den yüksk ise + // loopclosure yapıyor ve TRUE diyor. devamında ise tekrardan findpossibleloopclosure a giriyor şimdi burada ne oluyor peki + // tekrardan girince yeni chain ler buluyor ve bu yeni bulduğu chainlere göre tekrardan işleme tabi tutuyor burada referans atıyorum. + // terminal referansı: + //Checking of the candidatecChainsize inside of Loop CLosure: 7 + // Doing MatchScan + + // scanPose.GetPosition() values 0.605067 4.825724 + // validPoints: (-3.218281, 4.160619) + // validPoints: (-inf, inf) + // validPoints: (-inf, inf) + // validPoints: (-1.570163, 3.980981) + // validPoints: (-inf, inf) + // validPoints: (-inf, inf) + // validPoints: (-inf, inf) + // kod fiinish + + // burada ise tekrarda amtch arayınca buradaki verilere göre + + // bestPose: -4.594933 0.625724 -0.244379 + // bestResponse: 0.250306 + // averagePose: -4.594933 0.625724 -0.244379 + // bestResponse: 0.250306 + // BEST POSE = -4.594933 0.625724 -0.244379 BEST RESPONSE = 0.250306, + + // inanılmaz saçma bir depğer atıyor.. fakat zaten beim konumum doğruı ve 0.65 vemrişti ilk healtte niye tekrardan hesapolamaya giriyor + // byunu çözmek için direkt loopClosed değerini returnledim burada. ve bu değer true ise döngüden çıkıyor ve işlemi bitiriyor. +#ifdef KARTO_DEBUG + std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; +#endif + + return loopClosed; } } + candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); #ifdef KARTO_DEBUG std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; + std::cout << "pscan sensor pose: " << pScan->GetSensorPose() << std::endl; + std::cout << "pscan odometric pose: " << pScan->GetOdometricPose() << std::endl; + end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; std::cout << "----------Calculated time of Matchscan after triggered\n" - << "----------INSIDE OF WHILE LOOPCLOSURE function\n" + << "----------INSIDE OF WHILE LOOPCLOSURE but its false function\n" << "----------elapsed time: " << elapsed_seconds.count() << "s\n"; #endif - candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); } return loopClosed; @@ -2056,7 +2198,9 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( #ifdef KARTO_DEBUG std::cout << "\nnScans size " << nScans << std::endl; - + std::cout << "rStartNum size " << rStartNum << std::endl; + std::cout << "Calculating in this pose: " << pose.GetX() << " " << pose.GetY() << std::endl; + std::cout << "Nearlinkedscans size: " << nearLinkedScans.size() << std::endl; std::chrono::time_point start, end; start = std::chrono::system_clock::now(); #endif @@ -2067,7 +2211,6 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( if (pCandidateScan == NULL) { continue; } - Pose2 candidateScanPose = pCandidateScan->GetReferencePose( m_pMapper->m_pUseScanBarycenter->GetValue()); @@ -2075,6 +2218,12 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( if (squaredDistance < math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE) { + + std::cout << "Candidate Scan Position: (" + << pCandidateScan->GetCorrectedPose().GetX() << ", " + << pCandidateScan->GetCorrectedPose().GetY() << ", " + << pCandidateScan->GetCorrectedPose().GetHeading() << ")" << std::endl; + // a linked scan cannot be in the chain if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end()) @@ -2108,10 +2257,11 @@ void MapperGraph::CorrectPoses() ScanSolver * pSolver = m_pMapper->m_pScanOptimizer; if (pSolver != NULL) { pSolver->Compute(); - + std::cout << "\n\n\n\nCorrectPose executed\n\n\n\n" << std::endl; const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections()) { LocalizedRangeScan * scan = m_pMapper->m_pMapperSensorManager->GetScan(iter->first); + if (scan == NULL) { continue; } @@ -2127,6 +2277,10 @@ void MapperGraph::UpdateLoopScanMatcher(kt_double rangeThreshold) if (m_pLoopScanMatcher) { delete m_pLoopScanMatcher; } + std::cout << "m_pMapper->m_pLoopSearchSpaceDimension->GetValue() " << m_pMapper->m_pLoopSearchSpaceDimension->GetValue() << std::endl; + std::cout << "m_pMapper->m_pLoopSearchSpaceResolution->GetValue() " << m_pMapper->m_pLoopSearchSpaceResolution->GetValue() << std::endl; + std::cout << "m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue() " << m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue() << std::endl; + m_pLoopScanMatcher = ScanMatcher::Create(m_pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), @@ -2667,16 +2821,19 @@ void Mapper::setParamAngleVariancePenalty(double d) void Mapper::setParamFineSearchAngleOffset(double d) { m_pFineSearchAngleOffset->SetValue((kt_double)d); + std::cout << "m_pFineSearchAngleOffset: " << *m_pFineSearchAngleOffset << std::endl; } void Mapper::setParamCoarseSearchAngleOffset(double d) { m_pCoarseSearchAngleOffset->SetValue((kt_double)d); + std::cout << "m_pCoarseSearchAngleOffset: " << *m_pCoarseSearchAngleOffset << std::endl; } void Mapper::setParamCoarseAngleResolution(double d) { m_pCoarseAngleResolution->SetValue((kt_double)d); + std::cout << "m_pCoarseAngleResolution: " << *m_pCoarseAngleResolution << std::endl; } void Mapper::setParamMinimumAnglePenalty(double d) @@ -2701,6 +2858,11 @@ void Mapper::Initialize(kt_double rangeThreshold) return; } // create sequential scan and loop matcher, update if deserialized + + std::cout << "m_pCorrelationSearchSpaceDimension: " << *m_pCorrelationSearchSpaceDimension << std::endl; + std::cout << "m_pCorrelationSearchSpaceResolution: " << *m_pCorrelationSearchSpaceResolution << std::endl; + std::cout << "m_pCorrelationSearchSpaceSmearDeviation: " << *m_pCorrelationSearchSpaceSmearDeviation << std::endl; + if (m_pSequentialScanMatcher) { delete m_pSequentialScanMatcher; @@ -2870,6 +3032,7 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad return false; } + std::cout <<"m_Initialized: " << m_Initialized << std::endl; if (m_Initialized == false) { // initialize mapper with range threshold from device Initialize(pLaserRangeFinder->GetRangeThreshold()); @@ -2911,8 +3074,6 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad pScan->SetSensorPose(bestPose); } - pScan->SetOdometricPose(pScan->GetCorrectedPose()); - if (covariance) { *covariance = cov; } @@ -2920,6 +3081,8 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad // add scan to buffer and assign id m_pMapperSensorManager->AddScan(pScan); + std::cout <<"m_pUseScanMatching: " << m_pUseScanMatching->GetValue() << std::endl; + std::cout <<"DoLoopClosing: " << m_pDoLoopClosing->GetValue() << std::endl; Vertex * scan_vertex = NULL; if (m_pUseScanMatching->GetValue()) { // add to graph @@ -2937,21 +3100,24 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad } } } + // sebebi robotu bir konuma atadığımız zamnan + pScan->SetOdometricPose(pScan->GetCorrectedPose()); + std::cout <<"\n\n\n\n\n\n\nprocessagainsnotdeby trycloseloop has finished\n\n\n\n\n\n\n\n" << std::endl; m_pMapperSensorManager->SetLastScan(pScan); - + std::cout <<"the last scas's odometric pose "<< pScan->GetOdometricPose().GetX() << " " << pScan->GetOdometricPose().GetY() << std::endl; + std::cout <<"the last scas's corrected pose "<< pScan->GetCorrectedPose().GetX() << " " << pScan->GetCorrectedPose().GetY() << std::endl; if (addScanToLocalizationBuffer) { AddScanToLocalizationBuffer(pScan, scan_vertex); } - return true; } - return false; } kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance) { + if (pScan == NULL) { return false; } @@ -3257,22 +3423,27 @@ kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * { // test if first scan if (pLastScan == NULL) { + std::cout << "First scan" << std::endl; return true; } // test if enough time has passed kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime(); if (timeInterval >= m_pMinimumTimeInterval->GetValue()) { + std::cout << "Time interval: " << timeInterval << std::endl; return true; } Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose()); Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose()); + Pose2 lastCorrectedPose = pLastScan->GetCorrectedPose(); + Pose2 correctedPose = pScan->GetCorrectedPose(); // test if we have turned enough kt_double deltaHeading = math::NormalizeAngle( scannerPose.GetHeading() - lastScannerPose.GetHeading()); if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue()) { + std::cout << "Delta heading: " << deltaHeading << std::endl; return true; } @@ -3280,6 +3451,11 @@ kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance( scannerPose.GetPosition()); if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE) { + std::cout << "\nLast scanner pose: " << lastScannerPose << std::endl; + std::cout << "Scanner pose: " << scannerPose << std::endl; + std::cout << "Last corrected pose: " << lastCorrectedPose << std::endl; + std::cout << "Corrected pose: " << correctedPose << std::endl; + std::cout << "Squared travel distance: " << squaredTravelDistance << std::endl; return true; } diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 90be0a75f..366035529 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -295,27 +295,6 @@ void SlamToolbox::publishTransformLoop( { boost::mutex::scoped_lock lock(map_to_odom_mutex_); rclcpp::Time scan_timestamp = scan_header.stamp; - - //TODO: In future, we need to remove from there and create new function named as localizationHealthCheck. - double * best_response =smapper_->getMapper()->GetBestResponse(); - static double previous_best_response = 0.0; - - if (best_response != nullptr && *best_response > 0.02) { - try { - if (*best_response != previous_best_response) { - std_msgs::msg::Float32 msg; - msg.data = static_cast(*best_response); // TODO: Change here - localization_health_pub_->publish(msg); - previous_best_response = *best_response; - } - } - catch (std::exception & e) { - //TODO: Write publisher topic when cannot acces the result of health check . - RCLCPP_ERROR(this->get_logger(), "Exception caught while dereferencing best_response: %s", e.what()); - } - } - // - // Avoid publishing tf with initial 0.0 scan timestamp if (scan_timestamp.seconds() > 0.0 && !scan_header.frame_id.empty()) { geometry_msgs::msg::TransformStamped msg; diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index d3ff638c7..4222b2ddb 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -198,9 +198,8 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( update_reprocessing_transform = true; processor_type_ = PROCESS_LOCALIZATION; }else if (processor_type_ == PROCESS_DESIRED_POSE) { - // process_desired_pose_.reset(nullptr); - update_reprocessing_transform = false; - processor_type_ = PROCESS_LOCALIZATION; + process_desired_pose_.reset(nullptr); + processed = true; }else if (processor_type_ == PROCESS_LOCALIZATION) { processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance); update_reprocessing_transform = false; @@ -210,22 +209,61 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( "No valid processor type set! Exiting."); exit(-1); } - // if successfully processed, create odom to map transformation + if (!processed) { delete range_scan; range_scan = nullptr; } - else { - // compute our new transform - setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, - scan->header.stamp, update_reprocessing_transform); + else{ + if (processor_type_ != PROCESS_DESIRED_POSE) { + setTransformFromPoses(range_scan->GetCorrectedPose(), odom_pose, + scan->header.stamp, update_reprocessing_transform); - publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp); + publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp); + } + } + if (processor_type_ == PROCESS_DESIRED_POSE){ + processor_type_ = PROCESS_LOCALIZATION; } return range_scan; } +/* + *TODO: I'll think about this later + */ + +// void setInitialParameters(){ +// double initial_correlation_search_space_dimension = this->get_parameter("correlation_search_space_dimension").as_double(); +// double initial_correlation_search_space_resolution = this->get_parameter("correlation_search_space_resolution").as_double(); +// double initial_correlation_search_space_smear_deviation = this->get_parameter("correlation_search_space_smear_deviation").as_double(); + +// double initial_loop_search_space_dimension = this->get_parameter("loop_search_space_dimension").as_double(); +// double initial_loop_search_maximum_distance = this->get_parameter("loop_search_maximum_distance").as_double(); +// // double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); +// // double initial_loop_search_space_smear_deviation = this->get_parameter("loop_search_space_smear_deviation").as_double(); + +// double initial_fine_search_angle_offset = this->get_parameter("fine_search_angle_offset").as_double(); +// double initial_coarse_search_angle_offset = this->get_parameter("coarse_search_angle_offset").as_double(); +// double initial_coarse_angle_resolution = this->get_parameter("coarse_angle_resolution").as_double(); + +// double initial_do_relocalization = this->get_parameter("position_search_do_relocalization").as_bool(); +// double initial_minimum_best_response = this->get_parameter("position_search_minimum_best_response").as_double(); +// double initial_minimum_link_best_response = this->get_parameter("link_match_minimum_response_fine").as_double(); +// bool initial_do_loop_closing_value = this->get_parameter("do_loop_closing").as_bool(); + +// smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); +// smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); +// smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); +// smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); +// smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); +// smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); +// smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); +// smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); +// smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); +// smapper_->getMapper()->setParamDoLoopClosing(initial_do_loop_closing_value); +// } + /*****************************************************************************/ bool LocalizationSlamToolbox::desiredPoseCheck( const std::shared_ptr req, @@ -234,7 +272,7 @@ bool LocalizationSlamToolbox::desiredPoseCheck( { std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now(); std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); - Matrix3 covariance; + Matrix3 covariance; LocalizedRangeScan * range_scan = nullptr; covariance.SetToIdentity(); { @@ -251,7 +289,7 @@ bool LocalizationSlamToolbox::desiredPoseCheck( double initial_loop_search_space_dimension = this->get_parameter("loop_search_space_dimension").as_double(); double initial_loop_search_maximum_distance = this->get_parameter("loop_search_maximum_distance").as_double(); - // double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); + double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); // double initial_loop_search_space_smear_deviation = this->get_parameter("loop_search_space_smear_deviation").as_double(); double initial_fine_search_angle_offset = this->get_parameter("fine_search_angle_offset").as_double(); @@ -261,29 +299,7 @@ bool LocalizationSlamToolbox::desiredPoseCheck( double initial_do_relocalization = this->get_parameter("position_search_do_relocalization").as_bool(); double initial_minimum_best_response = this->get_parameter("position_search_minimum_best_response").as_double(); double initial_minimum_link_best_response = this->get_parameter("link_match_minimum_response_fine").as_double(); - - /* - * Setting initial_definitions. - */ - - { - boost::mutex::scoped_lock lock(smapper_mutex_); - smapper_->getMapper()->setParamLoopSearchSpaceDimension(2*position_search_distance_); - smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_); - smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset_); - smapper_->getMapper()->setParamCoarseSearchAngleOffset(position_search_coarse_angle_offset_); - smapper_->getMapper()->setParamCoarseAngleResolution(position_search_coarse_angle_resolution_); - smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(position_search_minimum_best_response_); - - if (req->search_distance != 0.0) { - position_search_distance_ = req->search_distance; - smapper_->getMapper()->setParamLoopSearchSpaceDimension(2*position_search_distance_); - smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_); - } - - smapper_->getMapper()->m_Initialized = false; - std::cout << "m_Initialized: " << smapper_->getMapper()->m_Initialized << std::endl; - } + bool initial_do_loop_closing_value = this->get_parameter("do_loop_closing").as_bool(); if (req->pose_x == 0.0 || req->pose_y == 0.0) { RCLCPP_ERROR(get_logger(), "Error: pose_x or pose_y is not provided."); @@ -294,16 +310,40 @@ bool LocalizationSlamToolbox::desiredPoseCheck( if (req->do_relocalization){ RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Searching for best response with relocalize"); - initial_do_relocalization = true; + position_search_do_relocalization_ = true; } else { RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Searching for best response without relocalize"); - initial_do_relocalization = false; + position_search_do_relocalization_ = false; } - - if (req->minimum_best_response != 0.0) { - initial_minimum_best_response = req->minimum_best_response; - this->set_parameter(rclcpp::Parameter("position_search_minimum_best_response", initial_minimum_best_response)); + { + boost::mutex::scoped_lock l(pose_mutex_); + process_desired_pose_ = std::make_unique(req->pose_x, req->pose_y, 0.0); + } + + first_measurement_ = true; + + { + boost::mutex::scoped_lock lock(smapper_mutex_); + smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance_); + smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_-1); + smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset_); + smapper_->getMapper()->setParamCoarseSearchAngleOffset(position_search_coarse_angle_offset_); + smapper_->getMapper()->setParamCoarseAngleResolution(position_search_coarse_angle_resolution_); + smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(position_search_minimum_best_response_); + smapper_->getMapper()->setParamLoopSearchSpaceResolution(position_search_resolution_); + smapper_->getMapper()->setParamLoopSearchSpaceSmearDeviation(position_search_smear_deviation_); + // smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(position_search_distance_); + smapper_->getMapper()->setParamDoLoopClosing(true); + smapper_->getMapper()->m_Initialized = false; + + if (req->search_distance != 0.0) { + position_search_distance_ = req->search_distance; + //for getting from the yaml file + // smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance_); + // smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_-1); + } + } if (!have_scan_values_) { @@ -312,88 +352,79 @@ bool LocalizationSlamToolbox::desiredPoseCheck( return false; } else{ - // for (double angle = 0.0; angle <= 360.0; angle += angle_resolution) { bool processed = false; { boost::mutex::scoped_lock l(pose_mutex_); process_desired_pose_ = std::make_unique(req->pose_x, req->pose_y, 0.0); range_scan = getLocalizedRangeScan(last_laser_stored_, last_scan_stored_, last_odom_pose_stored_); + // first_measurement_ = true; boost::mutex::scoped_lock lock(smapper_mutex_); range_scan->SetOdometricPose(*process_desired_pose_); range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); + std::cout << "Starting ProcessAgainstNodesNearBy" << std::endl; processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); - } - - if (processed) { - double * best_response = smapper_->getMapper()->GetBestResponse(); - std::cout << "best_response " << *best_response << std::endl; - std::cout << "req->minimum_best_response " << initial_minimum_best_response << std::endl; - if (best_response != nullptr && *best_response > initial_minimum_best_response) { - std::cout<< "finded best response" << std::endl; - res->message = std::to_string(*best_response); - res->success = true; - if (initial_do_relocalization) { - - setTransformFromPoses(range_scan->GetCorrectedPose(), last_odom_pose_stored_, - last_scan_stored_->header.stamp, true); - - publishPose(range_scan->GetCorrectedPose(), covariance, last_scan_stored_->header.stamp); - } - - // skartomapper_->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); - // skartomapper_->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); - // skartomapper_->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); - // skartomapper_->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); - // skartomapper_->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); - // skartomapper_->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); - // skartomapper_->setParamCoarseAngleResolution(initial_coarse_angle_resolution); - // skartomapper_->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); - // smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); - - return true; - } else { - res->message = "Couldn't find bestResponse"; - res->success = false; - { - boost::mutex::scoped_lock lock(smapper_mutex_); - smapper_->clearLocalizationBuffer(); - } - // skartomapper_->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); - // skartomapper_->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); - // skartomapper_->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); - // skartomapper_->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); - // skartomapper_->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); - // skartomapper_->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); - // skartomapper_->setParamCoarseAngleResolution(initial_coarse_angle_resolution); - // skartomapper_->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); - // smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); - - return false; + std::cout << "Finished ProcessAgainstNodesNearBy\n\n\n\n\n\n\n" << std::endl; + + if (processed) { + double * best_response = smapper_->getMapper()->GetBestResponse(); + std::cout << "best_response " << *best_response << std::endl; + std::cout << "req->minimum_best_response " << initial_minimum_best_response << std::endl; + if (best_response != nullptr && *best_response > initial_minimum_best_response) { + std::cout<< "finded best response, processing localization." << std::endl; + + if (position_search_do_relocalization_) { + std::cout << "range_scan->GetCorrectedPose() before" << range_scan->GetCorrectedPose().GetX() << " " << range_scan->GetCorrectedPose().GetY() << " " << range_scan->GetCorrectedPose().GetHeading() << std::endl; + std::cout << "range_scan->GetOdometricPose() before" << range_scan->GetOdometricPose().GetX() << " " << range_scan->GetOdometricPose().GetY() << " " << range_scan->GetOdometricPose().GetHeading() << std::endl; + setTransformFromPoses(range_scan->GetCorrectedPose(), last_odom_pose_stored_, + last_scan_stored_->header.stamp, true); + + publishPose(range_scan->GetCorrectedPose(), covariance, last_scan_stored_->header.stamp); + processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance); + std::cout << "range_scan->GetCorrectedPose() after" << range_scan->GetCorrectedPose().GetX() << " " << range_scan->GetCorrectedPose().GetY() << " " << range_scan->GetCorrectedPose().GetHeading() << std::endl; + std::cout << "range_scan->GetOdometricPose() after " << range_scan->GetOdometricPose().GetX() << " " << range_scan->GetOdometricPose().GetY() << " " << range_scan->GetOdometricPose().GetHeading() << std::endl; + } + // todo add the last pose values and do setodometric and correctedpose again. + else { + range_scan->SetOdometricPose(last_odom_pose_stored_); + range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); + } + smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); + smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); + smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); + smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); + smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); + smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); + smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); + smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); + smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); + smapper_->getMapper()->setParamDoLoopClosing(initial_do_loop_closing_value); + + res->message = std::to_string(*best_response); + res->success = true; + return true; + } else { + { + boost::mutex::scoped_lock lock(smapper_mutex_); + smapper_->clearLocalizationBuffer(); + } + + smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); + smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); + smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); + smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); + smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); + smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); + smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); + smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); + smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); + smapper_->getMapper()->setParamDoLoopClosing(initial_do_loop_closing_value); + res->message = "Couldn't find bestResponse"; + res->success = false; + return false; + } } } - // else { - // res->message = "Couldn't process scan will try again"; - // end = std::chrono::high_resolution_clock::now(); - // auto time_elapsed = std::chrono::duration_cast(end - start).count(); - // if (time_elapsed > process_timeout) { - // res->message = "Couldnt process scan in " + std::to_string(process_timeout) + " seconds"; - // res->success = false; - // this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); - - // return false; - // } - // } - - // end = std::chrono::high_resolution_clock::now(); - // auto time_elapsed = std::chrono::duration_cast(end - start).count(); - // if (time_elapsed > process_timeout) { - // res->message = "Couldnt find bestResponse in " + std::to_string(process_timeout) + " seconds"; - // res->success = false; - // this->set_parameter(rclcpp::Parameter("loop_search_maximum_distance", loop_search_maximum_distance)); - // return false; - // } - // } res->message = "Couldn't find with this resolution at desired time, halving the angle_resolution and decreasing best_response then search again."; res->success = false; return false; From 975aa0a4d32c0c30be815391604b7a86b6326dc2 Mon Sep 17 00:00:00 2001 From: Renbago Date: Thu, 19 Sep 2024 13:08:01 +0300 Subject: [PATCH 39/79] adding health publisher again --- src/slam_toolbox_common.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 366035529..c7e31638a 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -295,6 +295,27 @@ void SlamToolbox::publishTransformLoop( { boost::mutex::scoped_lock lock(map_to_odom_mutex_); rclcpp::Time scan_timestamp = scan_header.stamp; + + //TODO: In future, we need to remove from there and create new function named as localizationHealthCheck. + double * best_response =smapper_->getMapper()->GetBestResponse(); + static double previous_best_response = 0.0; + + if (best_response != nullptr && *best_response > 0.02) { + try { + if (*best_response != previous_best_response) { + std_msgs::msg::Float32 msg; + msg.data = static_cast(*best_response); // TODO: Change here + localization_health_pub_->publish(msg); + previous_best_response = *best_response; + } + } + catch (std::exception & e) { + //TODO: Write publisher topic when cannot acces the result of health check . + RCLCPP_ERROR(this->get_logger(), "Exception caught while dereferencing best_response: %s", e.what()); + } + } + // + // Avoid publishing tf with initial 0.0 scan timestamp if (scan_timestamp.seconds() > 0.0 && !scan_header.frame_id.empty()) { geometry_msgs::msg::TransformStamped msg; From 7ec620f98c2f7159ddb3b4401438738c0099af3d Mon Sep 17 00:00:00 2001 From: Renbago Date: Fri, 20 Sep 2024 09:50:33 +0300 Subject: [PATCH 40/79] function and debugs added for readable code. --- .../slam_toolbox_localization.hpp | 6 + lib/karto_sdk/src/Mapper.cpp | 13 +- src/slam_toolbox_localization.cpp | 160 ++++++++---------- 3 files changed, 84 insertions(+), 95 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index 9ce7af58b..98c7e0959 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -61,6 +61,12 @@ class LocalizationSlamToolbox : public SlamToolbox const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, Pose2 & pose) override; + void setInitialParametersForDesiredPose( + double position_search_distance, double position_search_maximum_distance, + double position_search_fine_angle_offset, double position_search_coarse_angle_offset, + double position_search_coarse_angle_resolution, double position_search_resolution, + double position_search_smear_deviation, bool do_loop_closing_flag); + std::shared_ptr> localization_pose_sub_; std::shared_ptr> clear_localization_; diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 8874fc665..883beb3f3 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -581,23 +581,25 @@ kt_double ScanMatcher::MatchScan( //TODO: Baha LOOK HERE // set up correlation grid - +#ifdef KARTO_DEBUG std::cout << "scanPose.GetPosition() values " << scanPose.GetPosition() << std::endl; +#endif AddScans(rBaseScans, scanPose.GetPosition()); - +#ifdef KARTO_DEBUG std::cout << "m_pSearchSpaceProbs width " << m_pSearchSpaceProbs->GetWidth() << std::endl; std::cout << "m_pSearchSpaceProbs height " << m_pSearchSpaceProbs->GetHeight() << std::endl; +#endif // compute how far to search in each direction Vector2 searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight()); Vector2 coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(), 0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution()); - +#ifdef KARTO_DEBUG std::cout << "m_pCorrelationGrid " << m_pCorrelationGrid->GetResolution() << std::endl; std::cout << "searchDimensions " << searchDimensions << std::endl; std::cout << "coarseSearchOffset " << coarseSearchOffset << std::endl; - +#endif // a coarse search only checks half the cells in each dimension Vector2 coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(), 2 * m_pCorrelationGrid->GetResolution()); @@ -2261,7 +2263,8 @@ void MapperGraph::CorrectPoses() const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections()) { LocalizedRangeScan * scan = m_pMapper->m_pMapperSensorManager->GetScan(iter->first); - + std::cout << "Scan: " << iter->first << std::endl; + std::cout << "Corrected Scan Pose: " << iter->second << std::endl; if (scan == NULL) { continue; } diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 4222b2ddb..1b13600c3 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -233,36 +233,38 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( *TODO: I'll think about this later */ -// void setInitialParameters(){ -// double initial_correlation_search_space_dimension = this->get_parameter("correlation_search_space_dimension").as_double(); -// double initial_correlation_search_space_resolution = this->get_parameter("correlation_search_space_resolution").as_double(); -// double initial_correlation_search_space_smear_deviation = this->get_parameter("correlation_search_space_smear_deviation").as_double(); - -// double initial_loop_search_space_dimension = this->get_parameter("loop_search_space_dimension").as_double(); -// double initial_loop_search_maximum_distance = this->get_parameter("loop_search_maximum_distance").as_double(); -// // double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); -// // double initial_loop_search_space_smear_deviation = this->get_parameter("loop_search_space_smear_deviation").as_double(); - -// double initial_fine_search_angle_offset = this->get_parameter("fine_search_angle_offset").as_double(); -// double initial_coarse_search_angle_offset = this->get_parameter("coarse_search_angle_offset").as_double(); -// double initial_coarse_angle_resolution = this->get_parameter("coarse_angle_resolution").as_double(); - -// double initial_do_relocalization = this->get_parameter("position_search_do_relocalization").as_bool(); -// double initial_minimum_best_response = this->get_parameter("position_search_minimum_best_response").as_double(); -// double initial_minimum_link_best_response = this->get_parameter("link_match_minimum_response_fine").as_double(); -// bool initial_do_loop_closing_value = this->get_parameter("do_loop_closing").as_bool(); - -// smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); -// smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); -// smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); -// smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); -// smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); -// smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); -// smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); -// smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); -// smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); -// smapper_->getMapper()->setParamDoLoopClosing(initial_do_loop_closing_value); -// } +void LocalizationSlamToolbox::setInitialParametersForDesiredPose(double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, + double position_search_coarse_angle_offset, double position_search_coarse_angle_resolution, double position_search_resolution, + double position_search_smear_deviation,bool do_loop_closing_flag){ + + // double initial_correlation_search_space_dimension = this->get_parameter("correlation_search_space_dimension").as_double(); + // double initial_correlation_search_space_resolution = this->get_parameter("correlation_search_space_resolution").as_double(); + // double initial_correlation_search_space_smear_deviation = this->get_parameter("correlation_search_space_smear_deviation").as_double(); + + // double initial_loop_search_space_dimension = this->get_parameter("loop_search_space_dimension").as_double(); + // double initial_loop_search_maximum_distance = this->get_parameter("loop_search_maximum_distance").as_double(); + // double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); + + // double initial_fine_search_angle_offset = this->get_parameter("fine_search_angle_offset").as_double(); + // double initial_coarse_search_angle_offset = this->get_parameter("coarse_search_angle_offset").as_double(); + // double initial_coarse_angle_resolution = this->get_parameter("coarse_angle_resolution").as_double(); + + // double initial_do_relocalization = this->get_parameter("position_search_do_relocalization").as_bool(); + // double initial_minimum_best_response = this->get_parameter("position_search_minimum_best_response").as_double(); + // double initial_minimum_link_best_response = this->get_parameter("link_match_minimum_response_fine").as_double(); + // bool initial_do_loop_closing_value = this->get_parameter("do_loop_closing").as_bool(); + + smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance); + smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_maximum_distance); + smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset); + smapper_->getMapper()->setParamCoarseSearchAngleOffset(position_search_coarse_angle_offset); + smapper_->getMapper()->setParamCoarseAngleResolution(position_search_coarse_angle_resolution); + smapper_->getMapper()->setParamLoopSearchSpaceResolution(position_search_resolution); + smapper_->getMapper()->setParamLoopSearchSpaceSmearDeviation(position_search_smear_deviation); + smapper_->getMapper()->setParamDoLoopClosing(do_loop_closing_flag); + smapper_->getMapper()->m_Initialized = false; + +} /*****************************************************************************/ bool LocalizationSlamToolbox::desiredPoseCheck( @@ -283,23 +285,23 @@ bool LocalizationSlamToolbox::desiredPoseCheck( /* Yaml parameters definitions for setting the parameters after process finished */ - double initial_correlation_search_space_dimension = this->get_parameter("correlation_search_space_dimension").as_double(); - double initial_correlation_search_space_resolution = this->get_parameter("correlation_search_space_resolution").as_double(); - double initial_correlation_search_space_smear_deviation = this->get_parameter("correlation_search_space_smear_deviation").as_double(); + // double initial_correlation_search_space_dimension = this->get_parameter("correlation_search_space_dimension").as_double(); + // double initial_correlation_search_space_resolution = this->get_parameter("correlation_search_space_resolution").as_double(); + // double initial_correlation_search_space_smear_deviation = this->get_parameter("correlation_search_space_smear_deviation").as_double(); - double initial_loop_search_space_dimension = this->get_parameter("loop_search_space_dimension").as_double(); - double initial_loop_search_maximum_distance = this->get_parameter("loop_search_maximum_distance").as_double(); - double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); - // double initial_loop_search_space_smear_deviation = this->get_parameter("loop_search_space_smear_deviation").as_double(); + // double initial_loop_search_space_dimension = this->get_parameter("loop_search_space_dimension").as_double(); + // double initial_loop_search_maximum_distance = this->get_parameter("loop_search_maximum_distance").as_double(); + // double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); + // // double initial_loop_search_space_smear_deviation = this->get_parameter("loop_search_space_smear_deviation").as_double(); - double initial_fine_search_angle_offset = this->get_parameter("fine_search_angle_offset").as_double(); - double initial_coarse_search_angle_offset = this->get_parameter("coarse_search_angle_offset").as_double(); - double initial_coarse_angle_resolution = this->get_parameter("coarse_angle_resolution").as_double(); + // double initial_fine_search_angle_offset = this->get_parameter("fine_search_angle_offset").as_double(); + // double initial_coarse_search_angle_offset = this->get_parameter("coarse_search_angle_offset").as_double(); + // double initial_coarse_angle_resolution = this->get_parameter("coarse_angle_resolution").as_double(); - double initial_do_relocalization = this->get_parameter("position_search_do_relocalization").as_bool(); - double initial_minimum_best_response = this->get_parameter("position_search_minimum_best_response").as_double(); - double initial_minimum_link_best_response = this->get_parameter("link_match_minimum_response_fine").as_double(); - bool initial_do_loop_closing_value = this->get_parameter("do_loop_closing").as_bool(); + // double initial_do_relocalization = this->get_parameter("position_search_do_relocalization").as_bool(); + // double initial_minimum_best_response = this->get_parameter("position_search_minimum_best_response").as_double(); + // double initial_minimum_link_best_response = this->get_parameter("link_match_minimum_response_fine").as_double(); + // bool initial_do_loop_closing_value = this->get_parameter("do_loop_closing").as_bool(); if (req->pose_x == 0.0 || req->pose_y == 0.0) { RCLCPP_ERROR(get_logger(), "Error: pose_x or pose_y is not provided."); @@ -325,29 +327,22 @@ bool LocalizationSlamToolbox::desiredPoseCheck( { boost::mutex::scoped_lock lock(smapper_mutex_); - smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance_); - smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_-1); - smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset_); - smapper_->getMapper()->setParamCoarseSearchAngleOffset(position_search_coarse_angle_offset_); - smapper_->getMapper()->setParamCoarseAngleResolution(position_search_coarse_angle_resolution_); - smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(position_search_minimum_best_response_); - smapper_->getMapper()->setParamLoopSearchSpaceResolution(position_search_resolution_); - smapper_->getMapper()->setParamLoopSearchSpaceSmearDeviation(position_search_smear_deviation_); - // smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(position_search_distance_); - smapper_->getMapper()->setParamDoLoopClosing(true); - smapper_->getMapper()->m_Initialized = false; + setInitialParametersForDesiredPose(position_search_distance_,((position_search_distance_*0.5)-1), position_search_fine_angle_offset_, + position_search_coarse_angle_offset_, position_search_coarse_angle_resolution_, + position_search_resolution_, position_search_smear_deviation_,true); if (req->search_distance != 0.0) { position_search_distance_ = req->search_distance; - //for getting from the yaml file - // smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance_); - // smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_distance_-1); + // setInitialParametersForDesiredPose(position_search_distance_,((position_search_distance_*0.5)-1), position_search_fine_angle_offset_, + // position_search_coarse_angle_offset_, position_search_coarse_angle_resolution_, + // position_search_resolution_, position_search_smear_deviation_,true); + } } if (!have_scan_values_) { - res->message = "No scan values stored try later"; + res->message = "No scan values stored try later"; res->success = false; return false; } @@ -369,8 +364,8 @@ bool LocalizationSlamToolbox::desiredPoseCheck( if (processed) { double * best_response = smapper_->getMapper()->GetBestResponse(); std::cout << "best_response " << *best_response << std::endl; - std::cout << "req->minimum_best_response " << initial_minimum_best_response << std::endl; - if (best_response != nullptr && *best_response > initial_minimum_best_response) { + std::cout << "req->minimum_best_response " << position_search_minimum_best_response_ << std::endl; + if (best_response != nullptr && *best_response > position_search_minimum_best_response_) { std::cout<< "finded best response, processing localization." << std::endl; if (position_search_do_relocalization_) { @@ -380,54 +375,39 @@ bool LocalizationSlamToolbox::desiredPoseCheck( last_scan_stored_->header.stamp, true); publishPose(range_scan->GetCorrectedPose(), covariance, last_scan_stored_->header.stamp); - processed = smapper_->getMapper()->ProcessLocalization(range_scan, &covariance); + std::cout << "range_scan->GetCorrectedPose() after" << range_scan->GetCorrectedPose().GetX() << " " << range_scan->GetCorrectedPose().GetY() << " " << range_scan->GetCorrectedPose().GetHeading() << std::endl; std::cout << "range_scan->GetOdometricPose() after " << range_scan->GetOdometricPose().GetX() << " " << range_scan->GetOdometricPose().GetY() << " " << range_scan->GetOdometricPose().GetHeading() << std::endl; } - // todo add the last pose values and do setodometric and correctedpose again. else { range_scan->SetOdometricPose(last_odom_pose_stored_); range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); } - smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); - smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); - smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); - smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); - smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); - smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); - smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); - smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); - smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); - smapper_->getMapper()->setParamDoLoopClosing(initial_do_loop_closing_value); + + setInitialParametersForDesiredPose(this->get_parameter("loop_search_space_dimension").as_double(),this->get_parameter("loop_search_maximum_distance").as_double(), + this->get_parameter("fine_search_angle_offset").as_double(),this->get_parameter("coarse_search_angle_offset").as_double(), + this->get_parameter("coarse_angle_resolution").as_double(),this->get_parameter("loop_search_space_resolution").as_double(), + this->get_parameter("loop_search_space_smear_deviation").as_double(),this->get_parameter("do_loop_closing").as_bool()); res->message = std::to_string(*best_response); res->success = true; return true; } else { - { - boost::mutex::scoped_lock lock(smapper_mutex_); - smapper_->clearLocalizationBuffer(); - } + smapper_->clearLocalizationBuffer(); + setInitialParametersForDesiredPose(this->get_parameter("loop_search_space_dimension").as_double(),this->get_parameter("loop_search_maximum_distance").as_double(), + this->get_parameter("fine_search_angle_offset").as_double(),this->get_parameter("coarse_search_angle_offset").as_double(), + this->get_parameter("coarse_angle_resolution").as_double(),this->get_parameter("loop_search_space_resolution").as_double(), + this->get_parameter("loop_search_space_smear_deviation").as_double(),this->get_parameter("do_loop_closing").as_bool()); - smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(initial_correlation_search_space_dimension); - smapper_->getMapper()->setParamCorrelationSearchSpaceResolution(initial_correlation_search_space_resolution); - smapper_->getMapper()->setParamCorrelationSearchSpaceSmearDeviation(initial_correlation_search_space_smear_deviation); - smapper_->getMapper()->setParamLoopSearchSpaceDimension(initial_loop_search_space_dimension); - smapper_->getMapper()->setParamFineSearchAngleOffset(initial_fine_search_angle_offset); - smapper_->getMapper()->setParamCoarseSearchAngleOffset(initial_coarse_search_angle_offset); - smapper_->getMapper()->setParamCoarseAngleResolution(initial_coarse_angle_resolution); - smapper_->getMapper()->setParamLinkMatchMinimumResponseFine(initial_minimum_link_best_response); - smapper_->getMapper()->setParamLoopSearchMaximumDistance(initial_loop_search_maximum_distance); - smapper_->getMapper()->setParamDoLoopClosing(initial_do_loop_closing_value); res->message = "Couldn't find bestResponse"; res->success = false; return false; } } + res->message = "Couldn't find with this resolution at desired time, halving the angle_resolution and decreasing best_response then search again."; + res->success = false; + return false; } - res->message = "Couldn't find with this resolution at desired time, halving the angle_resolution and decreasing best_response then search again."; - res->success = false; - return false; } } From c737aaf8e4363fac0161fb860c090b6af2115564 Mon Sep 17 00:00:00 2001 From: Renbago Date: Fri, 20 Sep 2024 11:24:38 +0300 Subject: [PATCH 41/79] adding debug --- lib/karto_sdk/src/Mapper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 883beb3f3..b47429d30 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -2259,12 +2259,12 @@ void MapperGraph::CorrectPoses() ScanSolver * pSolver = m_pMapper->m_pScanOptimizer; if (pSolver != NULL) { pSolver->Compute(); +#ifdef KARTO_DEBUG std::cout << "\n\n\n\nCorrectPose executed\n\n\n\n" << std::endl; +#endif const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections()) { LocalizedRangeScan * scan = m_pMapper->m_pMapperSensorManager->GetScan(iter->first); - std::cout << "Scan: " << iter->first << std::endl; - std::cout << "Corrected Scan Pose: " << iter->second << std::endl; if (scan == NULL) { continue; } From ad602da824beb65a6c3c94a7ef2c2a88d792d94d Mon Sep 17 00:00:00 2001 From: Renbago Date: Fri, 20 Sep 2024 16:14:31 +0300 Subject: [PATCH 42/79] instead of using as a pointer the getbestresponse, changed with shared pointer and added best solution's pose_x and pose_y . --- lib/karto_sdk/include/karto_sdk/Mapper.h | 19 ++++- lib/karto_sdk/src/Mapper.cpp | 96 +++++++++++++----------- 2 files changed, 67 insertions(+), 48 deletions(-) diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 142823a3b..4ba8670eb 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1963,11 +1963,22 @@ class KARTO_EXPORT Mapper : public Module public: /** - * Purpose of this function is set the best response to localization health information + * Purpose of this function is set the bestResponse, response and pose values to localization health information */ - double * m_pbestResponse; - double * GetBestResponse(); - void SetBestResponse(double * pBestResponse); + struct LocalizationInfos + { + double bestResponse; + double bestPoseX; + double bestPoseY; + }; + + std::shared_ptr m_pbestResponse; + std::shared_ptr m_pbestPoseX; + std::shared_ptr m_pbestPoseY; + + std::shared_ptr GetBestResponse() const; + void SetBestResponse(const std::shared_ptr& response); + /** * Allocate memory needed for mapping diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index b47429d30..1816f6dff 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1594,15 +1594,18 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens * The reason for getting the bestresponse here instead of the Matchscan function is that the other functions calling Matchscan always >>> * receive outputs greater than 0.5 from bestresponse, regardless of whether the robot's position is correct or not. */ - double best_response = coarseResponse; + auto best_solution_info = std::make_shared(); + + best_solution_info->bestResponse = coarseResponse; + best_solution_info->bestPoseX = pScan->GetSensorPose().GetX(); + best_solution_info->bestPoseY = pScan->GetSensorPose().GetY(); try { - m_pMapper->SetBestResponse(&best_response); + m_pMapper->SetBestResponse(best_solution_info); } catch (std::exception & e) { - throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " - "unable to publish set best response!"); + throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - unable to publish set best response!"); } std::stringstream stream; @@ -1621,16 +1624,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens #endif m_pMapper->FireLoopClosureCheck(stream.str()); - - double coarsedResponse = coarseResponse; - try - { - m_pMapper->SetBestResponse(&coarsedResponse); - } - catch (std::exception & e) { - std::cout << "Exception caught: " << e.what() << std::endl; - } - + if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) && (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) && (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())) @@ -1649,15 +1643,6 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens candidateChain, bestPose, covariance, false); - double finedResponse = fineResponse; - try - { - m_pMapper->SetBestResponse(&finedResponse); - } - catch (std::exception & e) { - std::cout << "Exception caught: " << e.what() << std::endl; - } - std::stringstream stream1; stream1 << "FINE RESPONSE: " << fineResponse << " (>" << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl; @@ -1668,15 +1653,18 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens } else { m_pMapper->FireBeginLoopClosure("Closing loop..."); - double best_response = fineResponse; + auto best_solution_info = std::make_shared(); + + best_solution_info->bestResponse = fineResponse; + best_solution_info->bestPoseX = pScan->GetSensorPose().GetX(); + best_solution_info->bestPoseY = pScan->GetSensorPose().GetY(); try { - m_pMapper->SetBestResponse(&best_response); + m_pMapper->SetBestResponse(best_solution_info); } catch (std::exception & e) { - throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " - "unable to publish set best response!"); + throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - unable to publish set best response!"); } pScan->SetSensorPose(bestPose); @@ -1684,7 +1672,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens #ifdef KARTO_DEBUG std::cout << "\n\n\nLoop Closed!, starting pose correction\n\n\n " << std::endl; - std::cout << "\n bestPose: " << bestPose << " bestResponse: " << best_response << std::endl; + std::cout << "\n bestPose: " << bestPose << " bestResponse: " << fineResponse << std::endl; std::cout << "pscan sensor pose: " << pScan->GetSensorPose() << std::endl; std::cout << "pscan corrected pose: " << pScan->GetCorrectedPose() << std::endl; std::cout << "pscan odometric pose: " << pScan->GetOdometricPose() << std::endl; @@ -1866,16 +1854,22 @@ void MapperGraph::LinkNearChains( * The reason for getting the bestresponse here instead of the Matchscan function is that the other functions calling Matchscan always >>> * receive outputs greater than 0.5 from bestresponse, regardless of whether the robot's position is correct or not. */ - double best_response = response; + auto best_solution_info = std::make_shared(); + + + best_solution_info->bestResponse = response; + best_solution_info->bestPoseX = pScan->GetSensorPose().GetX(); + best_solution_info->bestPoseY = pScan->GetSensorPose().GetY(); try { - m_pMapper->SetBestResponse(&best_response); + m_pMapper->SetBestResponse(best_solution_info); } catch (std::exception & e) { - throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - " - "unable to publish set best response!"); + throw std::runtime_error("LOCALIZATIONHEALTH PUBLISHER FATAL ERROR - unable to publish set best response!"); } + + if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE) { rMeans.push_back(mean); rCovariances.push_back(covariance); @@ -2306,7 +2300,9 @@ Mapper::Mapper() m_pMapperSensorManager(NULL), m_pGraph(NULL), m_pScanOptimizer(NULL), - m_pbestResponse(new double(0.0)) + m_pbestResponse(std::make_shared(0.0)), + m_pbestPoseX(std::make_shared(0.0)), + m_pbestPoseY(std::make_shared(0.0)) { InitializeParameters(); } @@ -2322,7 +2318,9 @@ Mapper::Mapper(const std::string & rName) m_pMapperSensorManager(NULL), m_pGraph(NULL), m_pScanOptimizer(NULL), - m_pbestResponse(new double(0.0)) + m_pbestResponse(std::make_shared(0.0)), + m_pbestPoseX(std::make_shared(0.0)), + m_pbestPoseY(std::make_shared(0.0)) { InitializeParameters(); } @@ -2924,10 +2922,10 @@ void Mapper::Reset() delete m_pMapperSensorManager; m_pMapperSensorManager = NULL; } - if (m_pbestResponse) { - delete m_pbestResponse; - m_pbestResponse = NULL; - } + // if (m_pbestResponse) { + // m_pbestResponse.reset(); + // } + m_Initialized = false; m_Deserialized = false; while (!m_LocalizationScanVertices.empty()) { @@ -3558,16 +3556,26 @@ void Mapper::FireEndLoopClosure(const std::string & rInfo) const } } -double* Mapper::GetBestResponse() +std::shared_ptr Mapper::GetBestResponse() const { - return m_pbestResponse; + auto response = std::make_shared(); + response->bestResponse = *m_pbestResponse; + response->bestPoseX = *m_pbestPoseX; + response->bestPoseY = *m_pbestPoseY; + return response; } -void Mapper::SetBestResponse(double* pBestResponse) +void Mapper::SetBestResponse(const std::shared_ptr& response) { - if (pBestResponse != nullptr) { - *m_pbestResponse = *pBestResponse; - } + if (response) { + if (m_pbestResponse && m_pbestPoseX && m_pbestPoseY) { + *m_pbestResponse = response->bestResponse; + *m_pbestPoseX = response->bestPoseX; + *m_pbestPoseY = response->bestPoseY; + } else { + throw std::runtime_error("Mapper FATAL ERROR - One or more bestResponse pointers are null."); + } + } } void Mapper::SetScanSolver(ScanSolver * pScanOptimizer) From 4ca55ed199ee562fbc34d64df8e0a5213fd5d31d Mon Sep 17 00:00:00 2001 From: Renbago Date: Fri, 20 Sep 2024 16:16:37 +0300 Subject: [PATCH 43/79] edited for const shared pointer getbestresponse and deleted some unneccessery comments. --- src/slam_toolbox_common.cpp | 15 ++++++---- src/slam_toolbox_localization.cpp | 48 +++++++++++-------------------- 2 files changed, 26 insertions(+), 37 deletions(-) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index c7e31638a..17f088e1b 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -297,16 +297,20 @@ void SlamToolbox::publishTransformLoop( rclcpp::Time scan_timestamp = scan_header.stamp; //TODO: In future, we need to remove from there and create new function named as localizationHealthCheck. - double * best_response =smapper_->getMapper()->GetBestResponse(); + std::shared_ptr response = smapper_->getMapper()->GetBestResponse(); + + double best_response = response->bestResponse; + double best_pose_x = response->bestPoseX; + double best_pose_y = response->bestPoseY; static double previous_best_response = 0.0; - if (best_response != nullptr && *best_response > 0.02) { + if (best_response > 0.02) { try { - if (*best_response != previous_best_response) { + if (best_response != previous_best_response) { std_msgs::msg::Float32 msg; - msg.data = static_cast(*best_response); // TODO: Change here + msg.data = static_cast(best_response); localization_health_pub_->publish(msg); - previous_best_response = *best_response; + previous_best_response = best_response; } } catch (std::exception & e) { @@ -314,7 +318,6 @@ void SlamToolbox::publishTransformLoop( RCLCPP_ERROR(this->get_logger(), "Exception caught while dereferencing best_response: %s", e.what()); } } - // // Avoid publishing tf with initial 0.0 scan timestamp if (scan_timestamp.seconds() > 0.0 && !scan_header.frame_id.empty()) { diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 1b13600c3..56fde59d6 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -282,27 +282,6 @@ bool LocalizationSlamToolbox::desiredPoseCheck( smapper_->clearLocalizationBuffer(); } - /* - Yaml parameters definitions for setting the parameters after process finished - */ - // double initial_correlation_search_space_dimension = this->get_parameter("correlation_search_space_dimension").as_double(); - // double initial_correlation_search_space_resolution = this->get_parameter("correlation_search_space_resolution").as_double(); - // double initial_correlation_search_space_smear_deviation = this->get_parameter("correlation_search_space_smear_deviation").as_double(); - - // double initial_loop_search_space_dimension = this->get_parameter("loop_search_space_dimension").as_double(); - // double initial_loop_search_maximum_distance = this->get_parameter("loop_search_maximum_distance").as_double(); - // double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); - // // double initial_loop_search_space_smear_deviation = this->get_parameter("loop_search_space_smear_deviation").as_double(); - - // double initial_fine_search_angle_offset = this->get_parameter("fine_search_angle_offset").as_double(); - // double initial_coarse_search_angle_offset = this->get_parameter("coarse_search_angle_offset").as_double(); - // double initial_coarse_angle_resolution = this->get_parameter("coarse_angle_resolution").as_double(); - - // double initial_do_relocalization = this->get_parameter("position_search_do_relocalization").as_bool(); - // double initial_minimum_best_response = this->get_parameter("position_search_minimum_best_response").as_double(); - // double initial_minimum_link_best_response = this->get_parameter("link_match_minimum_response_fine").as_double(); - // bool initial_do_loop_closing_value = this->get_parameter("do_loop_closing").as_bool(); - if (req->pose_x == 0.0 || req->pose_y == 0.0) { RCLCPP_ERROR(get_logger(), "Error: pose_x or pose_y is not provided."); res->message = "Error: pose_x or pose_y is missing. Please use it"; @@ -333,12 +312,11 @@ bool LocalizationSlamToolbox::desiredPoseCheck( if (req->search_distance != 0.0) { position_search_distance_ = req->search_distance; - // setInitialParametersForDesiredPose(position_search_distance_,((position_search_distance_*0.5)-1), position_search_fine_angle_offset_, - // position_search_coarse_angle_offset_, position_search_coarse_angle_resolution_, - // position_search_resolution_, position_search_smear_deviation_,true); - + //Redefinition of serach distance defined in the service + setInitialParametersForDesiredPose(position_search_distance_,((position_search_distance_*0.5)-1), position_search_fine_angle_offset_, + position_search_coarse_angle_offset_, position_search_coarse_angle_resolution_, + position_search_resolution_, position_search_smear_deviation_,true); } - } if (!have_scan_values_) { @@ -361,11 +339,15 @@ bool LocalizationSlamToolbox::desiredPoseCheck( processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); std::cout << "Finished ProcessAgainstNodesNearBy\n\n\n\n\n\n\n" << std::endl; - if (processed) { - double * best_response = smapper_->getMapper()->GetBestResponse(); - std::cout << "best_response " << *best_response << std::endl; + if (processed) { + std::shared_ptr response = smapper_->getMapper()->GetBestResponse(); + double best_response = response->bestResponse; + double best_pose_x = response->bestPoseX; + double best_pose_y = response->bestPoseY; + + std::cout << "best_response " << best_response << std::endl; std::cout << "req->minimum_best_response " << position_search_minimum_best_response_ << std::endl; - if (best_response != nullptr && *best_response > position_search_minimum_best_response_) { + if (best_response > position_search_minimum_best_response_) { std::cout<< "finded best response, processing localization." << std::endl; if (position_search_do_relocalization_) { @@ -389,7 +371,11 @@ bool LocalizationSlamToolbox::desiredPoseCheck( this->get_parameter("coarse_angle_resolution").as_double(),this->get_parameter("loop_search_space_resolution").as_double(), this->get_parameter("loop_search_space_smear_deviation").as_double(),this->get_parameter("do_loop_closing").as_bool()); - res->message = std::to_string(*best_response); + res->message = "Found bestResponse"; + res->relocated_x= static_cast(best_pose_x); + res->relocated_y= static_cast(best_pose_y); + res->best_response = static_cast(best_response); + res->success = true; return true; } else { From 39d03beb720093d3d31d2bc91dd66ac21b3818e5 Mon Sep 17 00:00:00 2001 From: Renbago Date: Fri, 20 Sep 2024 16:18:10 +0300 Subject: [PATCH 44/79] cleaned som unneccesry comments --- src/slam_toolbox_localization.cpp | 20 +------------------- 1 file changed, 1 insertion(+), 19 deletions(-) diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 56fde59d6..3e8e5f3f2 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -237,23 +237,6 @@ void LocalizationSlamToolbox::setInitialParametersForDesiredPose(double position double position_search_coarse_angle_offset, double position_search_coarse_angle_resolution, double position_search_resolution, double position_search_smear_deviation,bool do_loop_closing_flag){ - // double initial_correlation_search_space_dimension = this->get_parameter("correlation_search_space_dimension").as_double(); - // double initial_correlation_search_space_resolution = this->get_parameter("correlation_search_space_resolution").as_double(); - // double initial_correlation_search_space_smear_deviation = this->get_parameter("correlation_search_space_smear_deviation").as_double(); - - // double initial_loop_search_space_dimension = this->get_parameter("loop_search_space_dimension").as_double(); - // double initial_loop_search_maximum_distance = this->get_parameter("loop_search_maximum_distance").as_double(); - // double initial_loop_search_space_resolution = this->get_parameter("loop_search_space_resolution").as_double(); - - // double initial_fine_search_angle_offset = this->get_parameter("fine_search_angle_offset").as_double(); - // double initial_coarse_search_angle_offset = this->get_parameter("coarse_search_angle_offset").as_double(); - // double initial_coarse_angle_resolution = this->get_parameter("coarse_angle_resolution").as_double(); - - // double initial_do_relocalization = this->get_parameter("position_search_do_relocalization").as_bool(); - // double initial_minimum_best_response = this->get_parameter("position_search_minimum_best_response").as_double(); - // double initial_minimum_link_best_response = this->get_parameter("link_match_minimum_response_fine").as_double(); - // bool initial_do_loop_closing_value = this->get_parameter("do_loop_closing").as_bool(); - smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance); smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_maximum_distance); smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset); @@ -312,7 +295,6 @@ bool LocalizationSlamToolbox::desiredPoseCheck( if (req->search_distance != 0.0) { position_search_distance_ = req->search_distance; - //Redefinition of serach distance defined in the service setInitialParametersForDesiredPose(position_search_distance_,((position_search_distance_*0.5)-1), position_search_fine_angle_offset_, position_search_coarse_angle_offset_, position_search_coarse_angle_resolution_, position_search_resolution_, position_search_smear_deviation_,true); @@ -339,7 +321,7 @@ bool LocalizationSlamToolbox::desiredPoseCheck( processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); std::cout << "Finished ProcessAgainstNodesNearBy\n\n\n\n\n\n\n" << std::endl; - if (processed) { + if (processed) { std::shared_ptr response = smapper_->getMapper()->GetBestResponse(); double best_response = response->bestResponse; double best_pose_x = response->bestPoseX; From 038752946c8235fc6600ecbe87af046487e05faa Mon Sep 17 00:00:00 2001 From: Renbago Date: Mon, 23 Sep 2024 08:41:00 +0300 Subject: [PATCH 45/79] chainging the function default, will be testing that. --- lib/karto_sdk/src/Mapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 1816f6dff..feddfa489 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1721,7 +1721,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; #endif - return loopClosed; +chainging the function default, will be testing that. // return loopClosed; } } candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); From f4edd8f18fe6a581c5c4ee06aaa2b069871591b0 Mon Sep 17 00:00:00 2001 From: Renbago Date: Mon, 23 Sep 2024 12:00:03 +0300 Subject: [PATCH 46/79] fixed some problems which appears while doing merge --- lib/karto_sdk/src/Mapper.cpp | 25 ++++--------------------- src/slam_toolbox_localization.cpp | 1 - 2 files changed, 4 insertions(+), 22 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index cb6d8bfe8..ffcdeb4d8 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1650,15 +1650,6 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens candidateChain, bestPose, covariance, false); - double finedResponse = fineResponse; - try - { - m_pMapper->SetBestResponse(&finedResponse); - } - catch (std::exception & e) { - std::cout << "Exception caught: " << e.what() << std::endl; - } - std::stringstream stream1; stream1 << "FINE RESPONSE: " << fineResponse << " (>" << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl; @@ -1737,7 +1728,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; #endif - // return loopClosed; + return loopClosed; } } candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); @@ -1746,6 +1737,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; std::cout << "pscan sensor pose: " << pScan->GetSensorPose() << std::endl; std::cout << "pscan odometric pose: " << pScan->GetOdometricPose() << std::endl; + end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; std::cout << "----------Calculated time of Matchscan after triggered\n" @@ -1855,11 +1847,6 @@ void MapperGraph::LinkNearChains( #endif // match scan against "near" chain - using namespace std::chrono; - - std::chrono::time_point start, end; - start = std::chrono::system_clock::now(); - kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false); @@ -2211,10 +2198,6 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue()); - std::cout << "FindPossibleLoopClosure nearLinkedScans size: " << nearLinkedScans.size() << std::endl; - std::cout << "Near Linked Scans: " << nearLinkedScans.size() << std::endl; - - kt_int32u nScans = static_cast(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size()); @@ -3230,8 +3213,6 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covari Vertex * scan_vertex = NULL; if (m_pUseScanMatching->GetValue()) { // add to graph - // TODO: Testing here baha - scan_vertex = m_pGraph->AddVertex(pScan); m_pGraph->AddEdges(pScan, cov); @@ -3480,6 +3461,8 @@ kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose()); Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose()); + Pose2 lastCorrectedPose = pLastScan->GetCorrectedPose(); + Pose2 correctedPose = pScan->GetCorrectedPose(); // test if we have turned enough kt_double deltaHeading = math::NormalizeAngle( scannerPose.GetHeading() - lastScannerPose.GetHeading()); diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 9ecc002c5..b0ae9c5b0 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -187,7 +187,6 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( "valid region request. Ignoring scan."); return nullptr; } - 21 std::cout << "LocalizationSlamToolbox: processing nearregion" << std::endl; // smapper_->getMapper()->setParamDoLoopClosing(true); // set our position to the requested pose and proces From 3848749b9291de974d4dfcccc48ad6511f7e6514 Mon Sep 17 00:00:00 2001 From: Renbago Date: Mon, 23 Sep 2024 12:05:39 +0300 Subject: [PATCH 47/79] fixed few mistakes. --- lib/karto_sdk/src/Mapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index feddfa489..cf5cfde7f 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1721,7 +1721,7 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; #endif -chainging the function default, will be testing that. // return loopClosed; + // return loopClosed; // default is false } } candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); From 52f5e7a3fab4cc5afea2b13b90f829a61dea194c Mon Sep 17 00:00:00 2001 From: Renbago Date: Tue, 24 Sep 2024 08:50:38 +0300 Subject: [PATCH 48/79] removed some unnecessry comments and also removed minimum_best_response input for flag. The minimum best_response should be always higher then loop_match_minimum_response_fine --- lib/karto_sdk/src/Mapper.cpp | 37 +----------------------------------- src/slam_toolbox_common.cpp | 2 +- srvs/DesiredPoseChecker.srv | 1 - 3 files changed, 2 insertions(+), 38 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 57fd1ce68..440ee09fa 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -47,7 +47,7 @@ namespace karto { // enable this for verbose debug information -#define KARTO_DEBUG +// #define KARTO_DEBUG #define MAX_VARIANCE 500.0 #define DISTANCE_PENALTY_GAIN 0.2 @@ -478,8 +478,6 @@ ScanMatcher * ScanMatcher::Create( Mapper * pMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold) { - // TODO: Baha Check Here - // invalid parameters if (resolution <= 0) { return NULL; @@ -578,8 +576,6 @@ kt_double ScanMatcher::MatchScan( /////////////////////////////////////// - //TODO: Baha LOOK HERE - // set up correlation grid #ifdef KARTO_DEBUG std::cout << "scanPose.GetPosition() values " << scanPose.GetPosition() << std::endl; @@ -670,7 +666,6 @@ kt_double ScanMatcher::MatchScan( return bestResponse; } -//TODO the scanmatch algoritm running is here check it BAHA void ScanMatcher::operator()(const kt_double & y) const { kt_int32u poseResponseCounter; @@ -862,7 +857,6 @@ kt_double ScanMatcher::CorrelateScan( *ptr = math::Maximum(m_pPoseResponse[i].first, *ptr); } } - //TODO CHECK HERE // average all poses with same highest response Vector2 averagePosition; kt_double thetaX = 0.0; @@ -1695,35 +1689,6 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens m_pMapper->FireEndLoopClosure("Loop closed!"); loopClosed = true; - // TODO baha added here EKleme sebebim uzunca şudur: - // İlk önce robot loopclosure paramereleri ile matchscan atıyor bu değer 0.35 ve cov düşük ise while döngüsü içerisine giriyor. - // bu while döngüsü içerisinde bulunduğu konumu referans alaraktan tekrarda bir matchscan atıyor bu değer 0.45 den yüksk ise - // loopclosure yapıyor ve TRUE diyor. devamında ise tekrardan findpossibleloopclosure a giriyor şimdi burada ne oluyor peki - // tekrardan girince yeni chain ler buluyor ve bu yeni bulduğu chainlere göre tekrardan işleme tabi tutuyor burada referans atıyorum. - // terminal referansı: - //Checking of the candidatecChainsize inside of Loop CLosure: 7 - // Doing MatchScan - - // scanPose.GetPosition() values 0.605067 4.825724 - // validPoints: (-3.218281, 4.160619) - // validPoints: (-inf, inf) - // validPoints: (-inf, inf) - // validPoints: (-1.570163, 3.980981) - // validPoints: (-inf, inf) - // validPoints: (-inf, inf) - // validPoints: (-inf, inf) - // kod fiinish - - // burada ise tekrarda amtch arayınca buradaki verilere göre - - // bestPose: -4.594933 0.625724 -0.244379 - // bestResponse: 0.250306 - // averagePose: -4.594933 0.625724 -0.244379 - // bestResponse: 0.250306 - // BEST POSE = -4.594933 0.625724 -0.244379 BEST RESPONSE = 0.250306, - - // inanılmaz saçma bir depğer atıyor.. fakat zaten beim konumum doğruı ve 0.65 vemrişti ilk healtte niye tekrardan hesapolamaya giriyor - // byunu çözmek için direkt loopClosed değerini returnledim burada. ve bu değer true ise döngüden çıkıyor ve işlemi bitiriyor. #ifdef KARTO_DEBUG std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; #endif diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index e74a8a86b..28424016b 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -225,7 +225,7 @@ void SlamToolbox::setParams() position_search_do_relocalization_); position_search_minimum_best_response_ = 0.45; - position_search_minimum_best_response_ = this->declare_parameter("position_search_minimum_best_response", + position_search_minimum_best_response_ = this->declare_parameter("loop_match_minimum_response_fine", position_search_minimum_best_response_); } diff --git a/srvs/DesiredPoseChecker.srv b/srvs/DesiredPoseChecker.srv index 94c2bc0d2..2d24a7fe2 100644 --- a/srvs/DesiredPoseChecker.srv +++ b/srvs/DesiredPoseChecker.srv @@ -1,7 +1,6 @@ float64 pose_x float64 pose_y float64 search_distance -float64 minimum_best_response bool do_relocalization --- From adbb634cae9d0d582de864bd489d852a13621ceb Mon Sep 17 00:00:00 2001 From: oguzhankose Date: Thu, 26 Sep 2024 14:10:09 +0300 Subject: [PATCH 49/79] trested version zorlu --- lib/karto_sdk/src/Mapper.cpp | 17 ++++++++++++++--- src/slam_toolbox_common.cpp | 2 +- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 440ee09fa..3491faa2b 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -47,7 +47,7 @@ namespace karto { // enable this for verbose debug information -// #define KARTO_DEBUG +#define KARTO_DEBUG #define MAX_VARIANCE 500.0 #define DISTANCE_PENALTY_GAIN 0.2 @@ -830,12 +830,17 @@ kt_double ScanMatcher::CorrelateScan( m_doPenalize = doPenalize; tbb::parallel_for_each(m_yPoses, (*this)); + std::cout << "\n\n\nDOING PARALLEL FOR EACH\n\n\n" << std::endl; // find value of best response (in [0; 1]) kt_double bestResponse = -1; + using namespace std::chrono; + + std::chrono::time_point start, end; + start = std::chrono::steady_clock::now(); + for (kt_int32u i = 0; i < poseResponseSize; i++) { bestResponse = math::Maximum(bestResponse, m_pPoseResponse[i].first); - // std::cout << "bestresponse " << m_pPoseResponse[i].first << "pose" << m_pPoseResponse[i].second.GetPosition() << "heading" << m_pPoseResponse[i].second.GetHeading() << std::endl; // will compute positional covariance, save best relative probability for each cell if (!doingFineMatch) { const Pose2 & rPose = m_pPoseResponse[i].second; @@ -857,6 +862,13 @@ kt_double ScanMatcher::CorrelateScan( *ptr = math::Maximum(m_pPoseResponse[i].first, *ptr); } } + + end = std::chrono::steady_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::cout << "-------finished for inside of correlatescan at\n" + << "-------elapsed time: " << elapsed_seconds.count() << "s\n"; + + // average all poses with same highest response Vector2 averagePosition; kt_double thetaX = 0.0; @@ -1829,7 +1841,6 @@ void MapperGraph::LinkNearChains( */ auto best_solution_info = std::make_shared(); - best_solution_info->bestResponse = response; best_solution_info->bestPoseX = pScan->GetSensorPose().GetX(); best_solution_info->bestPoseY = pScan->GetSensorPose().GetY(); diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 28424016b..e74a8a86b 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -225,7 +225,7 @@ void SlamToolbox::setParams() position_search_do_relocalization_); position_search_minimum_best_response_ = 0.45; - position_search_minimum_best_response_ = this->declare_parameter("loop_match_minimum_response_fine", + position_search_minimum_best_response_ = this->declare_parameter("position_search_minimum_best_response", position_search_minimum_best_response_); } From a512cd84bd62a352a75a946bfa4b11bed60c7062 Mon Sep 17 00:00:00 2001 From: baha Date: Mon, 30 Sep 2024 15:19:19 +0300 Subject: [PATCH 50/79] Remove unnecessary comments and update minimum_best_response flag logic --- lib/karto_sdk/src/Mapper.cpp | 58 ++++--------------------------- src/slam_toolbox_localization.cpp | 16 +-------- 2 files changed, 7 insertions(+), 67 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 3491faa2b..c9476005e 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -47,7 +47,7 @@ namespace karto { // enable this for verbose debug information -#define KARTO_DEBUG +// #define KARTO_DEBUG #define MAX_VARIANCE 500.0 #define DISTANCE_PENALTY_GAIN 0.2 @@ -787,24 +787,6 @@ kt_double ScanMatcher::CorrelateScan( } assert(math::DoubleEqual(m_yPoses.back(), -startY)); - // mx ve my poses benim grid size ımdaki resamplelanmış pose ları tekabul ediyor - // burayı şu anlık açmaya gerek yok yapığı şey belirlenen çözünürlük kadar rsearch center - // dan başlayarak + - tarafa kadar grid size kadar çözünürlükte yeni değer atamak ve bu değerleri - // scanmatch hesabına sokup best response bulmak. - - // std::cout << "m_xPoses: "; - // for (const auto& x : m_xPoses) { - // std::cout << x << " "; - // } - // std::cout << std::endl; - - // std::cout << "m_yPoses: "; - // for (const auto& y : m_yPoses) { - // std::cout << y << " "; - // } - // std::cout << std::endl; - - // calculate pose response array size kt_int32u nAngles = static_cast(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1); @@ -830,15 +812,8 @@ kt_double ScanMatcher::CorrelateScan( m_doPenalize = doPenalize; tbb::parallel_for_each(m_yPoses, (*this)); - std::cout << "\n\n\nDOING PARALLEL FOR EACH\n\n\n" << std::endl; // find value of best response (in [0; 1]) kt_double bestResponse = -1; - - using namespace std::chrono; - - std::chrono::time_point start, end; - start = std::chrono::steady_clock::now(); - for (kt_int32u i = 0; i < poseResponseSize; i++) { bestResponse = math::Maximum(bestResponse, m_pPoseResponse[i].first); // will compute positional covariance, save best relative probability for each cell @@ -863,19 +838,12 @@ kt_double ScanMatcher::CorrelateScan( } } - end = std::chrono::steady_clock::now(); - std::chrono::duration elapsed_seconds = end - start; - std::cout << "-------finished for inside of correlatescan at\n" - << "-------elapsed time: " << elapsed_seconds.count() << "s\n"; - - // average all poses with same highest response Vector2 averagePosition; kt_double thetaX = 0.0; kt_double thetaY = 0.0; kt_int32s averagePoseCount = 0; for (kt_int32u i = 0; i < poseResponseSize; i++) { - // std::cout<< "m_pPoseResponse[i].second.GetPosition();"<< m_pPoseResponse[i].second.GetPosition() << std::endl; if (math::DoubleEqual(m_pPoseResponse[i].first, bestResponse)) { averagePosition += m_pPoseResponse[i].second.GetPosition(); @@ -903,11 +871,6 @@ kt_double ScanMatcher::CorrelateScan( delete[] m_pPoseResponse; m_pPoseResponse = nullptr; -#ifdef KARTO_DEBUG - std::cout << "bestPose: " << averagePose << std::endl; - std::cout << "bestResponse: " << bestResponse << std::endl; -#endif - if (!doingFineMatch) { ComputePositionalCovariance(averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset, rSearchSpaceResolution, searchAngleResolution, rCovariance); @@ -923,7 +886,6 @@ kt_double ScanMatcher::CorrelateScan( std::cout << "bestResponse: " << bestResponse << std::endl; #endif - if (bestResponse > 1.0) { bestResponse = 1.0; } @@ -1601,7 +1563,6 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens #endif Pose2 bestPose; Matrix3 covariance; - std::cout << "-----------------the next scan match is from trycloseloop------------" << std::endl; kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false, false); @@ -1704,8 +1665,6 @@ kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan * pScan, const Name & rSens #ifdef KARTO_DEBUG std::cout << std::boolalpha << "Output of Loop Closure: " << loopClosed << std::endl; #endif - - // return loopClosed; // default is false } } candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); @@ -2810,19 +2769,16 @@ void Mapper::setParamAngleVariancePenalty(double d) void Mapper::setParamFineSearchAngleOffset(double d) { m_pFineSearchAngleOffset->SetValue((kt_double)d); - std::cout << "m_pFineSearchAngleOffset: " << *m_pFineSearchAngleOffset << std::endl; } void Mapper::setParamCoarseSearchAngleOffset(double d) { m_pCoarseSearchAngleOffset->SetValue((kt_double)d); - std::cout << "m_pCoarseSearchAngleOffset: " << *m_pCoarseSearchAngleOffset << std::endl; } void Mapper::setParamCoarseAngleResolution(double d) { m_pCoarseAngleResolution->SetValue((kt_double)d); - std::cout << "m_pCoarseAngleResolution: " << *m_pCoarseAngleResolution << std::endl; } void Mapper::setParamMinimumAnglePenalty(double d) @@ -3022,7 +2978,6 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad return false; } - std::cout <<"m_Initialized: " << m_Initialized << std::endl; if (m_Initialized == false) { // initialize mapper with range threshold from device Initialize(pLaserRangeFinder->GetRangeThreshold()); @@ -3093,12 +3048,14 @@ kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan, kt_bool ad } #ifdef KARTO_DEBUG // sebebi robotu bir konuma atadığımız zaman odometric pose corrected pose a göre updatelenmiyor - // bu sebepten ötürü processlocalization içerisindeki kodda otomatik olarak plastscan - // arası odometric ve corected pose akar güncel sensor pose unu initial x kadar öteliyor. - // bu da biz 2d pose ataması sonucu bulduğumuz sistemlerde otomatik olarka x kadar konum atlatıyor + // bu sebepten ötürü processlocalization içerisindeki kodda otomatik olarak plastscan + // arası odometric ve corected pose oranı kadar güncel sensor pose unu oran kadar öteliyor. + // bu da robotun odometric pose unu matchscan sonucunda bulduğu yere oturtmuyor. // docker kısmında bunu yapmış fakat ana kodun bir kısmında correctedpose ile odometric pose // arasında bir uyumsuzluk var. Kod burada execute edilip çıktıktan sonra direkt processlocalization // koduna girip zıplattığı için onun öncesinde burada eşitliyorum bu sayede hatadan kurtuluyoruz. + // Tam güvenilirliğin bilmediğim için scan buffer içerisindeki scan verilerini siliyorum mantıken yaptıgımız + // işlem burada doğru bir hataya rastlamadım. // üzerinde daha düşüneceğim. pScan->SetOdometricPose(pScan->GetCorrectedPose()); std::cout <<"\n\n\n\n\n\n\nprocessagainsnotdeby trycloseloop has finished\n\n\n\n\n\n\n\n" << std::endl; @@ -3424,14 +3381,12 @@ kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * { // test if first scan if (pLastScan == NULL) { - std::cout << "First scan" << std::endl; return true; } // test if enough time has passed kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime(); if (timeInterval >= m_pMinimumTimeInterval->GetValue()) { - std::cout << "Time interval: " << timeInterval << std::endl; return true; } @@ -3443,7 +3398,6 @@ kt_bool Mapper::HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * kt_double deltaHeading = math::NormalizeAngle( scannerPose.GetHeading() - lastScannerPose.GetHeading()); if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue()) { - std::cout << "Delta heading: " << deltaHeading << std::endl; return true; } diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index b98e7e499..5799389a1 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -229,10 +229,6 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( return range_scan; } -/* - *TODO: I'll think about this later - */ - void LocalizationSlamToolbox::setInitialParametersForDesiredPose(double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, double position_search_coarse_angle_offset, double position_search_coarse_angle_resolution, double position_search_resolution, double position_search_smear_deviation,bool do_loop_closing_flag){ @@ -266,7 +262,7 @@ bool LocalizationSlamToolbox::desiredPoseCheck( } if (req->pose_x == 0.0 || req->pose_y == 0.0) { - RCLCPP_ERROR(get_logger(), "Error: pose_x or pose_y is not provided."); + RCLCPP_ERROR(get_logger(), "Error: pose_x or pose_y is not provided or cannot be equal to <0.0>"); res->message = "Error: pose_x or pose_y is missing. Please use it"; res->success = false; return false; @@ -317,9 +313,7 @@ bool LocalizationSlamToolbox::desiredPoseCheck( boost::mutex::scoped_lock lock(smapper_mutex_); range_scan->SetOdometricPose(*process_desired_pose_); range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); - std::cout << "Starting ProcessAgainstNodesNearBy" << std::endl; processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); - std::cout << "Finished ProcessAgainstNodesNearBy\n\n\n\n\n\n\n" << std::endl; if (processed) { std::shared_ptr response = smapper_->getMapper()->GetBestResponse(); @@ -327,21 +321,13 @@ bool LocalizationSlamToolbox::desiredPoseCheck( double best_pose_x = response->bestPoseX; double best_pose_y = response->bestPoseY; - std::cout << "best_response " << best_response << std::endl; - std::cout << "req->minimum_best_response " << position_search_minimum_best_response_ << std::endl; if (best_response > position_search_minimum_best_response_) { - std::cout<< "finded best response, processing localization." << std::endl; if (position_search_do_relocalization_) { - std::cout << "range_scan->GetCorrectedPose() before" << range_scan->GetCorrectedPose().GetX() << " " << range_scan->GetCorrectedPose().GetY() << " " << range_scan->GetCorrectedPose().GetHeading() << std::endl; - std::cout << "range_scan->GetOdometricPose() before" << range_scan->GetOdometricPose().GetX() << " " << range_scan->GetOdometricPose().GetY() << " " << range_scan->GetOdometricPose().GetHeading() << std::endl; setTransformFromPoses(range_scan->GetCorrectedPose(), last_odom_pose_stored_, last_scan_stored_->header.stamp, true); publishPose(range_scan->GetCorrectedPose(), covariance, last_scan_stored_->header.stamp); - - std::cout << "range_scan->GetCorrectedPose() after" << range_scan->GetCorrectedPose().GetX() << " " << range_scan->GetCorrectedPose().GetY() << " " << range_scan->GetCorrectedPose().GetHeading() << std::endl; - std::cout << "range_scan->GetOdometricPose() after " << range_scan->GetOdometricPose().GetX() << " " << range_scan->GetOdometricPose().GetY() << " " << range_scan->GetOdometricPose().GetHeading() << std::endl; } else { range_scan->SetOdometricPose(last_odom_pose_stored_); From 8a90975702069b3dbcbbbb4bbd939e57de4df75d Mon Sep 17 00:00:00 2001 From: baha Date: Mon, 30 Sep 2024 16:24:35 +0300 Subject: [PATCH 51/79] Remove unnecessary comments and update minimum_best_response flag logic --- src/slam_toolbox_common.cpp | 2 +- src/slam_toolbox_localization.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index e74a8a86b..46432144a 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -225,7 +225,7 @@ void SlamToolbox::setParams() position_search_do_relocalization_); position_search_minimum_best_response_ = 0.45; - position_search_minimum_best_response_ = this->declare_parameter("position_search_minimum_best_response", + position_search_minimum_best_response_ = this->get_parameter("loop_match_minimum_response_fine", position_search_minimum_best_response_); } diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 5799389a1..bfc02a694 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -309,7 +309,6 @@ bool LocalizationSlamToolbox::desiredPoseCheck( process_desired_pose_ = std::make_unique(req->pose_x, req->pose_y, 0.0); range_scan = getLocalizedRangeScan(last_laser_stored_, last_scan_stored_, last_odom_pose_stored_); - // first_measurement_ = true; boost::mutex::scoped_lock lock(smapper_mutex_); range_scan->SetOdometricPose(*process_desired_pose_); range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); From 8f70e86e31945ab2da15a9a51be6288cea7b489e Mon Sep 17 00:00:00 2001 From: baha Date: Tue, 1 Oct 2024 12:39:45 +0300 Subject: [PATCH 52/79] dummy definition mistake --- src/slam_toolbox_common.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 46432144a..491803cf3 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -225,8 +225,7 @@ void SlamToolbox::setParams() position_search_do_relocalization_); position_search_minimum_best_response_ = 0.45; - position_search_minimum_best_response_ = this->get_parameter("loop_match_minimum_response_fine", - position_search_minimum_best_response_); + this->get_parameter("loop_match_minimum_response_fine", position_search_minimum_best_response_); } /*****************************************************************************/ From 2e733a2cb59d6f5bc6d510c1bdd9a4efd2b429bf Mon Sep 17 00:00:00 2001 From: baha Date: Tue, 1 Oct 2024 13:10:37 +0300 Subject: [PATCH 53/79] changed the tuned params, for not depending the slam_toolbox.yaml file. Position_search_res and smear_dev has not been tuned. will be checked later. --- src/slam_toolbox_common.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 491803cf3..f4bcf735c 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -196,19 +196,19 @@ void SlamToolbox::setParams() /* * Those parameters for pose_search service */ - position_search_distance_ = 10.0; + position_search_distance_ = 12.0; position_search_distance_ = this->declare_parameter("position_search_distance", position_search_distance_); - position_search_resolution_ = 0.2; + position_search_resolution_ = 0.05; position_search_resolution_ = this->declare_parameter("position_search_resolution", position_search_resolution_); - position_search_smear_deviation_ = 0.2; + position_search_smear_deviation_ = 0.03; position_search_smear_deviation_ = this->declare_parameter("position_search_smear_deviation", position_search_smear_deviation_); - position_search_fine_angle_offset_ = 0.0314; + position_search_fine_angle_offset_ = 0.00349; position_search_fine_angle_offset_ = this->declare_parameter("position_search_fine_search_angle_offset", position_search_fine_angle_offset_); @@ -216,7 +216,7 @@ void SlamToolbox::setParams() position_search_coarse_angle_offset_ = this->declare_parameter("position_search_coarse_angle_offset", position_search_coarse_angle_offset_); - position_search_coarse_angle_resolution_ = 0.314; + position_search_coarse_angle_resolution_ = 0.0349; position_search_coarse_angle_resolution_ = this->declare_parameter("position_search_coarse_angle_resolution", position_search_coarse_angle_resolution_); From a01641d2a83a63d9adad8dcd8ac8e511374a3272 Mon Sep 17 00:00:00 2001 From: Renbago Date: Tue, 1 Oct 2024 13:10:37 +0300 Subject: [PATCH 54/79] updated ceres lib at humble for ceres version 2.2.0 --- CMakeLists.txt | 1 + .../slam_toolbox_localization.hpp | 8 ++ include/slam_toolbox/toolbox_msgs.hpp | 1 + solvers/ceres_solver.cpp | 120 ++++++++++++------ solvers/ceres_solver.hpp | 10 +- solvers/ceres_utils.h | 34 +++-- src/slam_toolbox_localization.cpp | 36 +++++- srvs/ElevatorMode.srv | 9 ++ 8 files changed, 163 insertions(+), 56 deletions(-) create mode 100644 srvs/ElevatorMode.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index dc7b8bcea..56ee116bc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -106,6 +106,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} srvs/AddSubmap.srv srvs/DeserializePoseGraph.srv srvs/DesiredPoseChecker.srv + srvs/ElevatorMode.srv srvs/SerializePoseGraph.srv DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs ) diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index 98c7e0959..3074900d8 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -47,6 +47,10 @@ class LocalizationSlamToolbox : public SlamToolbox const std::shared_ptr req, std::shared_ptr res); + bool elevatorMode( + const std::shared_ptr req, + std::shared_ptr res); + virtual bool serializePoseGraphCallback( const std::shared_ptr request_header, const std::shared_ptr req, @@ -61,6 +65,9 @@ class LocalizationSlamToolbox : public SlamToolbox const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, Pose2 & pose) override; + void setInitialParametersElevatorMode( + double position_search_maximum_distance, double position_search_distance); + void setInitialParametersForDesiredPose( double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, double position_search_coarse_angle_offset, @@ -71,6 +78,7 @@ class LocalizationSlamToolbox : public SlamToolbox localization_pose_sub_; std::shared_ptr> clear_localization_; std::shared_ptr> ssGetBestResponse_; // TODO: check here + std::shared_ptr> ssGetElevatorMode_; sensor_msgs::msg::LaserScan::ConstSharedPtr last_scan_stored_; Pose2 last_odom_pose_stored_; diff --git a/include/slam_toolbox/toolbox_msgs.hpp b/include/slam_toolbox/toolbox_msgs.hpp index 5ef8597c9..b5e6399f0 100644 --- a/include/slam_toolbox/toolbox_msgs.hpp +++ b/include/slam_toolbox/toolbox_msgs.hpp @@ -40,5 +40,6 @@ #include "slam_toolbox/srv/merge_maps.hpp" #include "slam_toolbox/srv/add_submap.hpp" #include "slam_toolbox/srv/desired_pose_checker.hpp" +#include "slam_toolbox/srv/elevator_mode.hpp" #endif // SLAM_TOOLBOX__TOOLBOX_MSGS_HPP_ diff --git a/solvers/ceres_solver.cpp b/solvers/ceres_solver.cpp index 1828ed8d4..573f1aa27 100644 --- a/solvers/ceres_solver.cpp +++ b/solvers/ceres_solver.cpp @@ -25,37 +25,70 @@ CeresSolver::CeresSolver() void CeresSolver::Configure(rclcpp::Node::SharedPtr node) /*****************************************************************************/ { - node_ = node; + logger_ = node->get_logger(); std::string solver_type, preconditioner_type, dogleg_type, trust_strategy, loss_fn, mode; - solver_type = node->declare_parameter("ceres_linear_solver", - std::string("SPARSE_NORMAL_CHOLESKY")); - preconditioner_type = node->declare_parameter("ceres_preconditioner", - std::string("JACOBI")); - dogleg_type = node->declare_parameter("ceres_dogleg_type", - std::string("TRADITIONAL_DOGLEG")); - trust_strategy = node->declare_parameter("ceres_trust_strategy", - std::string("LM")); - loss_fn = node->declare_parameter("ceres_loss_function", - std::string("None")); - mode = node->declare_parameter("mode", std::string("mapping")); + if (!node->has_parameter("ceres_linear_solver")) { + node->declare_parameter( + "ceres_linear_solver", + rclcpp::ParameterValue(std::string("SPARSE_NORMAL_CHOLESKY"))); + } + solver_type = node->get_parameter("ceres_linear_solver").as_string(); + + if (!node->has_parameter("ceres_preconditioner")) { + node->declare_parameter( + "ceres_preconditioner", + rclcpp::ParameterValue(std::string("JACOBI"))); + } + preconditioner_type = node->get_parameter("ceres_preconditioner").as_string(); + + if (!node->has_parameter("ceres_dogleg_type")) { + node->declare_parameter( + "ceres_dogleg_type", + rclcpp::ParameterValue(std::string("TRADITIONAL_DOGLEG"))); + } + dogleg_type = node->get_parameter("ceres_dogleg_type").as_string(); + + if (!node->has_parameter("ceres_trust_strategy")) { + node->declare_parameter( + "ceres_trust_strategy", + rclcpp::ParameterValue(std::string("LM"))); + } + trust_strategy = node->get_parameter("ceres_trust_strategy").as_string(); + + if (!node->has_parameter("ceres_loss_function")) { + node->declare_parameter( + "ceres_loss_function", + rclcpp::ParameterValue(std::string("None"))); + } + loss_fn = node->get_parameter("ceres_loss_function").as_string(); + + if (!node->has_parameter("mode")) { + node->declare_parameter( + "mode", + rclcpp::ParameterValue(std::string("mapping"))); + } + mode = node->get_parameter("mode").as_string(); + debug_logging_ = node->get_parameter("debug_logging").as_bool(); corrections_.clear(); first_node_ = nodes_->end(); // formulate problem - angle_local_parameterization_ = AngleLocalParameterization::Create(); + angle_manifold_ = AngleManifold::Create(); // choose loss function default squared loss (NULL) loss_function_ = NULL; if (loss_fn == "HuberLoss") { - RCLCPP_INFO(node_->get_logger(), + RCLCPP_INFO( + node->get_logger(), "CeresSolver: Using HuberLoss loss function."); loss_function_ = new ceres::HuberLoss(0.7); } else if (loss_fn == "CauchyLoss") { - RCLCPP_INFO(node_->get_logger(), + RCLCPP_INFO( + node->get_logger(), "CeresSolver: Using CauchyLoss loss function."); loss_function_ = new ceres::CauchyLoss(0.7); } @@ -63,15 +96,18 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node) // choose linear solver default CHOL options_.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; if (solver_type == "SPARSE_SCHUR") { - RCLCPP_INFO(node_->get_logger(), + RCLCPP_INFO( + node->get_logger(), "CeresSolver: Using SPARSE_SCHUR solver."); options_.linear_solver_type = ceres::SPARSE_SCHUR; } else if (solver_type == "ITERATIVE_SCHUR") { - RCLCPP_INFO(node_->get_logger(), + RCLCPP_INFO( + node->get_logger(), "CeresSolver: Using ITERATIVE_SCHUR solver."); options_.linear_solver_type = ceres::ITERATIVE_SCHUR; } else if (solver_type == "CGNR") { - RCLCPP_INFO(node_->get_logger(), + RCLCPP_INFO( + node->get_logger(), "CeresSolver: Using CGNR solver."); options_.linear_solver_type = ceres::CGNR; } @@ -79,11 +115,13 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node) // choose preconditioner default Jacobi options_.preconditioner_type = ceres::JACOBI; if (preconditioner_type == "IDENTITY") { - RCLCPP_INFO(node_->get_logger(), + RCLCPP_INFO( + node->get_logger(), "CeresSolver: Using IDENTITY preconditioner."); options_.preconditioner_type = ceres::IDENTITY; } else if (preconditioner_type == "SCHUR_JACOBI") { - RCLCPP_INFO(node_->get_logger(), + RCLCPP_INFO( + node->get_logger(), "CeresSolver: Using SCHUR_JACOBI preconditioner."); options_.preconditioner_type = ceres::SCHUR_JACOBI; } @@ -99,7 +137,8 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node) // choose trust region strategy default LM options_.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; if (trust_strategy == "DOGLEG") { - RCLCPP_INFO(node_->get_logger(), + RCLCPP_INFO( + node->get_logger(), "CeresSolver: Using DOGLEG trust region strategy."); options_.trust_region_strategy_type = ceres::DOGLEG; } @@ -108,7 +147,8 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node) if (options_.trust_region_strategy_type == ceres::DOGLEG) { options_.dogleg_type = ceres::TRADITIONAL_DOGLEG; if (dogleg_type == "SUBSPACE_DOGLEG") { - RCLCPP_INFO(node_->get_logger(), + RCLCPP_INFO( + node->get_logger(), "CeresSolver: Using SUBSPACE_DOGLEG dogleg type."); options_.dogleg_type = ceres::SUBSPACE_DOGLEG; } @@ -146,7 +186,7 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node) } // we do not want the problem definition to own these objects, otherwise they get - // deleted along with the problem + // deleted along with the problem options_problem_.loss_function_ownership = ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; problem_ = new ceres::Problem(options_problem_); @@ -177,7 +217,8 @@ void CeresSolver::Compute() boost::mutex::scoped_lock lock(nodes_mutex_); if (nodes_->size() == 0) { - RCLCPP_WARN(node_->get_logger(), + RCLCPP_WARN( + logger_, "CeresSolver: Ceres was called when there are no nodes." " This shouldn't happen."); return; @@ -188,7 +229,8 @@ void CeresSolver::Compute() problem_->HasParameterBlock(&first_node_->second(0)) && problem_->HasParameterBlock(&first_node_->second(1)) && problem_->HasParameterBlock(&first_node_->second(2))) { - RCLCPP_DEBUG(node_->get_logger(), + RCLCPP_DEBUG( + logger_, "CeresSolver: Setting first node as a constant pose:" "%0.2f, %0.2f, %0.2f.", first_node_->second(0), first_node_->second(1), first_node_->second(2)); @@ -205,7 +247,8 @@ void CeresSolver::Compute() } if (!summary.IsSolutionUsable()) { - RCLCPP_WARN(node_->get_logger(), "CeresSolver: " + RCLCPP_WARN( + logger_, "CeresSolver: " "Ceres could not find a usable solution to optimize."); return; } @@ -267,7 +310,7 @@ void CeresSolver::Reset() problem_ = new ceres::Problem(options_problem_); first_node_ = nodes_->end(); - angle_local_parameterization_ = AngleLocalParameterization::Create(); + angle_manifold_ = AngleManifold::Create(); } /*****************************************************************************/ @@ -311,7 +354,8 @@ void CeresSolver::AddConstraint(karto::Edge * pEdge) if (node1it == nodes_->end() || node2it == nodes_->end() || node1it == node2it) { - RCLCPP_WARN(node_->get_logger(), + RCLCPP_WARN( + logger_, "CeresSolver: Failed to add constraint, could not find nodes."); return; } @@ -338,10 +382,10 @@ void CeresSolver::AddConstraint(karto::Edge * pEdge) cost_function, loss_function_, &node1it->second(0), &node1it->second(1), &node1it->second(2), &node2it->second(0), &node2it->second(1), &node2it->second(2)); - problem_->SetParameterization(&node1it->second(2), - angle_local_parameterization_); - problem_->SetParameterization(&node2it->second(2), - angle_local_parameterization_); + problem_->SetManifold(&node1it->second(2), + angle_manifold_); + problem_->SetManifold(&node2it->second(2), + angle_manifold_); blocks_->insert(std::pair( GetHash(node1, node2), block)); @@ -362,19 +406,20 @@ void CeresSolver::RemoveNode(kt_int32s id) problem_->RemoveParameterBlock(&nodeit->second(1)); problem_->RemoveParameterBlock(&nodeit->second(2)); RCLCPP_DEBUG( - node_->get_logger(), + logger_, "RemoveNode: Removed node id %d" ,nodeit->first); } else { RCLCPP_DEBUG( - node_->get_logger(), + logger_, "RemoveNode: Missing parameter blocks for " "node id %d", nodeit->first); } nodes_->erase(nodeit); } else { - RCLCPP_ERROR(node_->get_logger(), "RemoveNode: Failed to find node matching id %i", + RCLCPP_ERROR( + logger_, "RemoveNode: Failed to find node matching id %i", (int)id); } } @@ -395,7 +440,8 @@ void CeresSolver::RemoveConstraint(kt_int32s sourceId, kt_int32s targetId) problem_->RemoveResidualBlock(it_b->second); blocks_->erase(it_b); } else { - RCLCPP_ERROR(node_->get_logger(), + RCLCPP_ERROR( + logger_, "RemoveConstraint: Failed to find residual block for %i %i", (int)sourceId, (int)targetId); } @@ -436,4 +482,4 @@ std::unordered_map * CeresSolver::getGraph() } // namespace solver_plugins #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(solver_plugins::CeresSolver, karto::ScanSolver) +PLUGINLIB_EXPORT_CLASS(solver_plugins::CeresSolver, karto::ScanSolver) \ No newline at end of file diff --git a/solvers/ceres_solver.hpp b/solvers/ceres_solver.hpp index 162b55950..75273ace5 100644 --- a/solvers/ceres_solver.hpp +++ b/solvers/ceres_solver.hpp @@ -7,16 +7,17 @@ #define SOLVERS__CERES_SOLVER_HPP_ #include -#include #include #include #include #include #include +#include #include "karto_sdk/Mapper.h" #include "solvers/ceres_utils.h" #include "rclcpp/rclcpp.hpp" +// #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_srvs/srv/empty.hpp" #include "slam_toolbox/toolbox_types.hpp" @@ -38,6 +39,7 @@ class CeresSolver : public karto::ScanSolver virtual void Compute(); // Solve virtual void Clear(); // Resets the corrections virtual void Reset(); // Resets the solver plugin clean + virtual void Configure(rclcpp::Node::SharedPtr node); // Adds a node to the solver @@ -65,7 +67,7 @@ class CeresSolver : public karto::ScanSolver ceres::Problem::Options options_problem_; ceres::LossFunction * loss_function_; ceres::Problem * problem_; - ceres::LocalParameterization * angle_local_parameterization_; + ceres::Manifold * angle_manifold_; bool was_constant_set_, debug_logging_; // graph @@ -75,9 +77,9 @@ class CeresSolver : public karto::ScanSolver boost::mutex nodes_mutex_; // ros - rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_{rclcpp::get_logger("CeresSolver")}; }; } // namespace solver_plugins -#endif // SOLVERS__CERES_SOLVER_HPP_ +#endif // SOLVERS__CERES_SOLVER_HPP_ \ No newline at end of file diff --git a/solvers/ceres_utils.h b/solvers/ceres_utils.h index 42c2fad8d..fab5a8f6d 100644 --- a/solvers/ceres_utils.h +++ b/solvers/ceres_utils.h @@ -7,7 +7,7 @@ #define SOLVERS__CERES_UTILS_H_ #include -#include +#include #include #include @@ -35,21 +35,27 @@ inline T NormalizeAngle(const T & angle_radians) /*****************************************************************************/ /*****************************************************************************/ -class AngleLocalParameterization -{ -public: - template - bool operator()( - const T * theta_radians, const T * delta_theta_radians, - T * theta_radians_plus_delta) const - { - *theta_radians_plus_delta = NormalizeAngle(*theta_radians + *delta_theta_radians); +// Defines a manifold for updating the angle to be constrained in [-pi to pi). +class AngleManifold { + public: + template + bool Plus(const T* x_radians, + const T* delta_radians, + T* x_plus_delta_radians) const { + *x_plus_delta_radians = NormalizeAngle(*x_radians + *delta_radians); return true; } - static ceres::LocalParameterization * Create() - { - return new ceres::AutoDiffLocalParameterization; + template + bool Minus(const T* y_radians, + const T* x_radians, + T* y_minus_x_radians) const { + *y_minus_x_radians = NormalizeAngle(*y_radians - *x_radians); + return true; + } + + static ceres::Manifold* Create() { + return new ceres::AutoDiffManifold; } }; @@ -120,4 +126,4 @@ class PoseGraph2dErrorTerm const Eigen::Matrix3d sqrt_information_; }; -#endif // SOLVERS__CERES_UTILS_H_ +#endif // SOLVERS__CERES_UTILS_H_ \ No newline at end of file diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index bfc02a694..22a76c4cc 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -44,6 +44,10 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) std::bind(&LocalizationSlamToolbox::desiredPoseCheck, this, std::placeholders::_1, std::placeholders::_2)); + ssGetElevatorMode_ = this->create_service( + "slam_toolbox/elevator_mode", + std::bind(&LocalizationSlamToolbox::elevatorMode, this, + std::placeholders::_1, std::placeholders::_2)); // in localization mode, we cannot allow for interactive mode enable_interactive_mode_ = false; @@ -229,6 +233,14 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( return range_scan; } +void LocalizationSlamToolbox::setInitialParametersElevatorMode(double position_search_maximum_distance, double position_search_space_dimension_distance){ + std::cout << "localization_slam_toolbox: Setting elevator mode parameters" << std::endl; + smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_maximum_distance); + smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_space_dimension_distance); + smapper_->getMapper()->GetGraph()->UpdateLoopScanMatcher(30.0); +} + + void LocalizationSlamToolbox::setInitialParametersForDesiredPose(double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, double position_search_coarse_angle_offset, double position_search_coarse_angle_resolution, double position_search_resolution, double position_search_smear_deviation,bool do_loop_closing_flag){ @@ -242,7 +254,6 @@ void LocalizationSlamToolbox::setInitialParametersForDesiredPose(double position smapper_->getMapper()->setParamLoopSearchSpaceSmearDeviation(position_search_smear_deviation); smapper_->getMapper()->setParamDoLoopClosing(do_loop_closing_flag); smapper_->getMapper()->m_Initialized = false; - } /*****************************************************************************/ @@ -364,6 +375,29 @@ bool LocalizationSlamToolbox::desiredPoseCheck( } } +bool LocalizationSlamToolbox::elevatorMode( + const std::shared_ptr req, + std::shared_ptr res) +{ + if (req->inside_elev_zone){ + boost::mutex::scoped_lock lock(smapper_mutex_); + RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Elevator zone settings are enabled"); + double position_search_maximum_distance = 1.0; + double position_search_space_dimension_distance = 1.0; + setInitialParametersElevatorMode(position_search_maximum_distance, position_search_space_dimension_distance); + res->message = "Elevator mode is enabled"; + res->success = true; + return true; + } else { + boost::mutex::scoped_lock lock(smapper_mutex_); + RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Elevator zone settings are disabled"); + setInitialParametersElevatorMode(this->get_parameter("loop_search_maximum_distance").as_double(),this->get_parameter("loop_search_space_dimension").as_double()); + res->message = "Elevator mode is disabled"; + res->success = true; + return true; + } +} + /*****************************************************************************/ void LocalizationSlamToolbox::localizePoseCallback( const diff --git a/srvs/ElevatorMode.srv b/srvs/ElevatorMode.srv new file mode 100644 index 000000000..f10e9454a --- /dev/null +++ b/srvs/ElevatorMode.srv @@ -0,0 +1,9 @@ +bool inside_elev_zone + +--- + +string message +bool success + + + From f289af483ffbc4b8b78622f0d0e86d43d3f23b0b Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Thu, 14 Nov 2024 10:18:31 +0300 Subject: [PATCH 55/79] created subsystem for services --- CMakeLists.txt | 2 + .../slam_toolbox_localization.hpp | 4 ++ include/slam_toolbox/toolbox_msgs.hpp | 2 + lib/karto_sdk/include/karto_sdk/Mapper.h | 4 +- lib/karto_sdk/src/Mapper.cpp | 15 ++++++- src/slam_toolbox_localization.cpp | 42 +++++++++++++++++++ srvs/ModeSelection.srv | 6 +++ srvs/SetParametersService.srv | 6 +++ 8 files changed, 77 insertions(+), 4 deletions(-) create mode 100644 srvs/ModeSelection.srv create mode 100644 srvs/SetParametersService.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index aab4ab8bb..c60b5a550 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -106,6 +106,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} srvs/AddSubmap.srv srvs/DeserializePoseGraph.srv srvs/SerializePoseGraph.srv + srvs/ModeSelection.srv + srvs/SetParametersService.srv DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs ) diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index a99f56e9e..b401c64c9 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -42,6 +42,9 @@ class LocalizationSlamToolbox : public SlamToolbox const std::shared_ptr request_header, const std::shared_ptr req, std::shared_ptr resp); + void set_parameters_callback( + const std::shared_ptr request, + std::shared_ptr response); virtual bool serializePoseGraphCallback( const std::shared_ptr request_header, @@ -60,6 +63,7 @@ class LocalizationSlamToolbox : public SlamToolbox std::shared_ptr> localization_pose_sub_; std::shared_ptr > clear_localization_; + std::shared_ptr > set_parameters_srv_; }; } // namespace slam_toolbox diff --git a/include/slam_toolbox/toolbox_msgs.hpp b/include/slam_toolbox/toolbox_msgs.hpp index 1300962d4..ef80ccc74 100644 --- a/include/slam_toolbox/toolbox_msgs.hpp +++ b/include/slam_toolbox/toolbox_msgs.hpp @@ -39,5 +39,7 @@ #include "slam_toolbox/srv/deserialize_pose_graph.hpp" #include "slam_toolbox/srv/merge_maps.hpp" #include "slam_toolbox/srv/add_submap.hpp" +#include "slam_toolbox/srv/set_parameters_service.hpp" +#include "slam_toolbox/srv/mode_selection.hpp" #endif // SLAM_TOOLBOX__TOOLBOX_MSGS_HPP_ diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index d53d6416d..17ae949a1 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -2164,9 +2164,9 @@ class KARTO_EXPORT Mapper : public Module public: void SetUseScanMatching(kt_bool val) {m_pUseScanMatching->SetValue(val);} - -protected: kt_bool m_Initialized; + +protected: kt_bool m_Deserialized; ScanMatcher * m_pSequentialScanMatcher; diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index f1ccfe96b..7c7e03234 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -47,7 +47,7 @@ namespace karto { // enable this for verbose debug information -// #define KARTO_DEBUG +#define KARTO_DEBUG #define MAX_VARIANCE 500.0 #define DISTANCE_PENALTY_GAIN 0.2 @@ -2127,6 +2127,12 @@ void MapperGraph::UpdateLoopScanMatcher(kt_double rangeThreshold) if (m_pLoopScanMatcher) { delete m_pLoopScanMatcher; } +#ifdef KARTO_DEBUG + std::cout << "\n\n\n UpdateLoopScanMatcher function is called\n\n\n" << std::endl; + std::cout << "m_pMapper->m_pLoopSearchSpaceDimension->GetValue() " << m_pMapper->m_pLoopSearchSpaceDimension->GetValue() << std::endl; + std::cout << "m_pMapper->m_pLoopSearchSpaceResolution->GetValue() " << m_pMapper->m_pLoopSearchSpaceResolution->GetValue() << std::endl; + std::cout << "m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue() " << m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue() << std::endl; +#endif m_pLoopScanMatcher = ScanMatcher::Create(m_pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), @@ -2701,7 +2707,12 @@ void Mapper::Initialize(kt_double rangeThreshold) return; } // create sequential scan and loop matcher, update if deserialized - +#ifdef KARTO_DEBUG + std::cout << "\n\n\n\nInitialize function is called\n\n\n" << std::endl; + std::cout << "m_pCorrelationSearchSpaceDimension: " << *m_pCorrelationSearchSpaceDimension << std::endl; + std::cout << "m_pCorrelationSearchSpaceResolution: " << *m_pCorrelationSearchSpaceResolution << std::endl; + std::cout << "m_pCorrelationSearchSpaceSmearDeviation: " << *m_pCorrelationSearchSpaceSmearDeviation << std::endl; +#endif if (m_pSequentialScanMatcher) { delete m_pSequentialScanMatcher; } diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index fe4e066b2..0d0fd0d65 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -39,6 +39,10 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) std::bind(&LocalizationSlamToolbox::clearLocalizationBuffer, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + set_parameters_srv_ = this->create_service( + "slam_toolbox/set_dynamic_parameters", + std::bind(&LocalizationSlamToolbox::set_parameters_callback, this, std::placeholders::_1, std::placeholders::_2)); + // in localization mode, we cannot allow for interactive mode enable_interactive_mode_ = false; @@ -205,6 +209,44 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( return range_scan; } +void LocalizationSlamToolbox::set_parameters_callback( + const std::shared_ptr request, + std::shared_ptr response +) +{ + if (request->param_names.size() != request->param_values.size()) { + response->success = false; + response->message = "Mismatched param_names and param_values lengths."; + return; + } + boost::mutex::scoped_lock lock(smapper_mutex_); + std::cout << "Setting parameters for the specified mode" << std::endl; + for (size_t i = 0; i < request->param_names.size(); ++i) { + const auto ¶m_name = request->param_names[i]; + const auto ¶m_value = request->param_values[i]; + + if (param_name == "loop_search_maximum_distance") + { + smapper_->getMapper()->setParamLoopSearchMaximumDistance(param_value); + } + else if (param_name == "loop_search_space_dimension") + { + smapper_->getMapper()->setParamLoopSearchSpaceDimension(param_value); + } + else { + std::cerr << "Unknown parameter: " << param_name << std::endl; + } + } + + smapper_->getMapper()->m_Initialized = false; + + // Başarı durumunu ve mesajı ayarla + response->success = true; + response->message = "Parameters set successfully for the specified mode"; + std::cout <<"Parameters set successfully for the specified mode" << std::endl; +} + + /*****************************************************************************/ void LocalizationSlamToolbox::localizePoseCallback( const diff --git a/srvs/ModeSelection.srv b/srvs/ModeSelection.srv new file mode 100644 index 000000000..5c97244e2 --- /dev/null +++ b/srvs/ModeSelection.srv @@ -0,0 +1,6 @@ +string mode + +--- + +string message +bool success \ No newline at end of file diff --git a/srvs/SetParametersService.srv b/srvs/SetParametersService.srv new file mode 100644 index 000000000..c3a23aeee --- /dev/null +++ b/srvs/SetParametersService.srv @@ -0,0 +1,6 @@ +string[] param_names +float64[] param_values +--- + +bool success +string message From 531b0fe9e984a6a7d02ad0195d47d384b0244665 Mon Sep 17 00:00:00 2001 From: Renbago Date: Thu, 14 Nov 2024 13:15:41 +0300 Subject: [PATCH 56/79] removed service from slam to common and added default params loading --- CMakeLists.txt | 1 - .../slam_toolbox_localization.hpp | 6 +++ include/slam_toolbox/toolbox_msgs.hpp | 1 - src/slam_toolbox_localization.cpp | 49 ++++++++++++++++--- srvs/ModeSelection.srv | 6 --- srvs/SetParametersService.srv | 2 + 6 files changed, 50 insertions(+), 15 deletions(-) delete mode 100644 srvs/ModeSelection.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index c60b5a550..ddca2a27e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -106,7 +106,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} srvs/AddSubmap.srv srvs/DeserializePoseGraph.srv srvs/SerializePoseGraph.srv - srvs/ModeSelection.srv srvs/SetParametersService.srv DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs ) diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index b401c64c9..17d73877e 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -60,6 +60,12 @@ class LocalizationSlamToolbox : public SlamToolbox const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, Pose2 & pose) override; + void setInitialParameters( + double position_search_distance, double position_search_maximum_distance, + double position_search_fine_angle_offset, double position_search_coarse_angle_offset, + double position_search_coarse_angle_resolution, double position_search_resolution, + double position_search_smear_deviation, bool do_loop_closing_flag); + std::shared_ptr> localization_pose_sub_; std::shared_ptr > clear_localization_; diff --git a/include/slam_toolbox/toolbox_msgs.hpp b/include/slam_toolbox/toolbox_msgs.hpp index ef80ccc74..c333a206d 100644 --- a/include/slam_toolbox/toolbox_msgs.hpp +++ b/include/slam_toolbox/toolbox_msgs.hpp @@ -40,6 +40,5 @@ #include "slam_toolbox/srv/merge_maps.hpp" #include "slam_toolbox/srv/add_submap.hpp" #include "slam_toolbox/srv/set_parameters_service.hpp" -#include "slam_toolbox/srv/mode_selection.hpp" #endif // SLAM_TOOLBOX__TOOLBOX_MSGS_HPP_ diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 0d0fd0d65..e4aa7c9b9 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -209,28 +209,63 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( return range_scan; } +void LocalizationSlamToolbox::setInitialParameters(double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, + double position_search_coarse_angle_offset, double position_search_coarse_angle_resolution, double position_search_resolution, + double position_search_smear_deviation,bool do_loop_closing_flag){ + + smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance); + smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_maximum_distance); + smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset); + smapper_->getMapper()->setParamCoarseSearchAngleOffset(position_search_coarse_angle_offset); + smapper_->getMapper()->setParamCoarseAngleResolution(position_search_coarse_angle_resolution); + smapper_->getMapper()->setParamLoopSearchSpaceResolution(position_search_resolution); + smapper_->getMapper()->setParamLoopSearchSpaceSmearDeviation(position_search_smear_deviation); + smapper_->getMapper()->setParamDoLoopClosing(do_loop_closing_flag); + smapper_->getMapper()->m_Initialized = false; + +} + void LocalizationSlamToolbox::set_parameters_callback( const std::shared_ptr request, std::shared_ptr response ) { + if (request->default_mode) { + RCLCPP_INFO(get_logger(), "Setting parameters for the default mode"); + boost::mutex::scoped_lock lock(smapper_mutex_); + setInitialParameters( + this->get_parameter("loop_search_space_dimension").as_double(), + this->get_parameter("loop_search_maximum_distance").as_double(), + this->get_parameter("fine_search_angle_offset").as_double(), + this->get_parameter("coarse_search_angle_offset").as_double(), + this->get_parameter("coarse_angle_resolution").as_double(), + this->get_parameter("loop_search_space_resolution").as_double(), + this->get_parameter("loop_search_space_smear_deviation").as_double(), + this->get_parameter("do_loop_closing").as_bool() + ); + + response->success = true; + response->message = "Default parameters set successfully."; + return; + } + if (request->param_names.size() != request->param_values.size()) { response->success = false; response->message = "Mismatched param_names and param_values lengths."; return; } + boost::mutex::scoped_lock lock(smapper_mutex_); std::cout << "Setting parameters for the specified mode" << std::endl; + for (size_t i = 0; i < request->param_names.size(); ++i) { const auto ¶m_name = request->param_names[i]; const auto ¶m_value = request->param_values[i]; - if (param_name == "loop_search_maximum_distance") - { + if (param_name == "loop_search_maximum_distance") { smapper_->getMapper()->setParamLoopSearchMaximumDistance(param_value); } - else if (param_name == "loop_search_space_dimension") - { + else if (param_name == "loop_search_space_dimension") { smapper_->getMapper()->setParamLoopSearchSpaceDimension(param_value); } else { @@ -238,15 +273,15 @@ void LocalizationSlamToolbox::set_parameters_callback( } } + // Parametrelerin başarıyla ayarlandığını bildirin smapper_->getMapper()->m_Initialized = false; - - // Başarı durumunu ve mesajı ayarla response->success = true; response->message = "Parameters set successfully for the specified mode"; - std::cout <<"Parameters set successfully for the specified mode" << std::endl; + std::cout << "Parameters set successfully for the specified mode" << std::endl; } + /*****************************************************************************/ void LocalizationSlamToolbox::localizePoseCallback( const diff --git a/srvs/ModeSelection.srv b/srvs/ModeSelection.srv deleted file mode 100644 index 5c97244e2..000000000 --- a/srvs/ModeSelection.srv +++ /dev/null @@ -1,6 +0,0 @@ -string mode - ---- - -string message -bool success \ No newline at end of file diff --git a/srvs/SetParametersService.srv b/srvs/SetParametersService.srv index c3a23aeee..3363a2d6e 100644 --- a/srvs/SetParametersService.srv +++ b/srvs/SetParametersService.srv @@ -1,5 +1,7 @@ string[] param_names float64[] param_values +bool default_mode + --- bool success From fcbd92b0f0a16a66ee390555b3dc5af4bc2ed096 Mon Sep 17 00:00:00 2001 From: Renbago Date: Thu, 14 Nov 2024 16:05:51 +0300 Subject: [PATCH 57/79] added choosing initializing or updating mode relative to mapping or localization mode. --- src/slam_toolbox_localization.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index e4aa7c9b9..bb3ba5f9f 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -221,8 +221,12 @@ void LocalizationSlamToolbox::setInitialParameters(double position_search_distan smapper_->getMapper()->setParamLoopSearchSpaceResolution(position_search_resolution); smapper_->getMapper()->setParamLoopSearchSpaceSmearDeviation(position_search_smear_deviation); smapper_->getMapper()->setParamDoLoopClosing(do_loop_closing_flag); - smapper_->getMapper()->m_Initialized = false; - + if(processor_type_ == PROCESS_LOCALIZATION){ + smapper_->getMapper()->m_Initialized = false; + } + else{ + smapper_->getMapper()->GetGraph()->UpdateLoopScanMatcher(this->get_parameter("max_laser_range").as_double()); + } } void LocalizationSlamToolbox::set_parameters_callback( @@ -273,8 +277,12 @@ void LocalizationSlamToolbox::set_parameters_callback( } } - // Parametrelerin başarıyla ayarlandığını bildirin - smapper_->getMapper()->m_Initialized = false; + if(processor_type_ == PROCESS_LOCALIZATION){ + smapper_->getMapper()->m_Initialized = false; + } + else{ + smapper_->getMapper()->GetGraph()->UpdateLoopScanMatcher(this->get_parameter("max_laser_range").as_double()); + } response->success = true; response->message = "Parameters set successfully for the specified mode"; std::cout << "Parameters set successfully for the specified mode" << std::endl; From 50cd61f06235b8f67f46888dfd1f36c70715ebe0 Mon Sep 17 00:00:00 2001 From: Renbago Date: Mon, 18 Nov 2024 15:19:14 +0300 Subject: [PATCH 58/79] removed desiredPoseCheck relative settings for now --- CMakeLists.txt | 1 - .../slam_toolbox_localization.hpp | 5 - lib/karto_sdk/src/Mapper.cpp | 4 +- src/slam_toolbox_localization.cpp | 145 +----------------- srvs/DesiredPoseChecker.srv | 15 -- 5 files changed, 3 insertions(+), 167 deletions(-) delete mode 100644 srvs/DesiredPoseChecker.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index ecbf9b420..ddca2a27e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,7 +105,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} srvs/MergeMaps.srv srvs/AddSubmap.srv srvs/DeserializePoseGraph.srv - srvs/DesiredPoseChecker.srv srvs/SerializePoseGraph.srv srvs/SetParametersService.srv DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index e84e861fa..0877b66dc 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -46,10 +46,6 @@ class LocalizationSlamToolbox : public SlamToolbox const std::shared_ptr request, std::shared_ptr response); - bool desiredPoseCheck( - const std::shared_ptr req, - std::shared_ptr res); - virtual bool serializePoseGraphCallback( const std::shared_ptr request_header, const std::shared_ptr req, @@ -64,7 +60,6 @@ class LocalizationSlamToolbox : public SlamToolbox const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan, Pose2 & pose) override; - void setInitialParameters( double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, double position_search_coarse_angle_offset, diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 90ba816ab..86c0ae6a0 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -47,7 +47,7 @@ namespace karto { // enable this for verbose debug information -#define KARTO_DEBUG +// #define KARTO_DEBUG #define MAX_VARIANCE 500.0 #define DISTANCE_PENALTY_GAIN 0.2 @@ -2219,12 +2219,10 @@ void MapperGraph::UpdateLoopScanMatcher(kt_double rangeThreshold) if (m_pLoopScanMatcher) { delete m_pLoopScanMatcher; } -#ifdef KARTO_DEBUG std::cout << "\n\n\n UpdateLoopScanMatcher function is called\n\n\n" << std::endl; std::cout << "m_pMapper->m_pLoopSearchSpaceDimension->GetValue() " << m_pMapper->m_pLoopSearchSpaceDimension->GetValue() << std::endl; std::cout << "m_pMapper->m_pLoopSearchSpaceResolution->GetValue() " << m_pMapper->m_pLoopSearchSpaceResolution->GetValue() << std::endl; std::cout << "m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue() " << m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue() << std::endl; -#endif m_pLoopScanMatcher = ScanMatcher::Create(m_pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 17e6bd1fe..dd8178c0e 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -154,12 +154,6 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( Pose2 & odom_pose) /*****************************************************************************/ { - // dirty storage: - last_laser_stored_ = laser; - last_scan_stored_ = scan; - last_odom_pose_stored_ = odom_pose; - have_scan_values_ = true; - boost::mutex::scoped_lock l(pose_mutex_); if (processor_type_ == PROCESS_LOCALIZATION && process_near_pose_) { @@ -228,140 +222,6 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( return range_scan; } -void LocalizationSlamToolbox::setInitialParametersForDesiredPose(double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, - double position_search_coarse_angle_offset, double position_search_coarse_angle_resolution, double position_search_resolution, - double position_search_smear_deviation,bool do_loop_closing_flag){ - - smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance); - smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_maximum_distance); - smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset); - smapper_->getMapper()->setParamCoarseSearchAngleOffset(position_search_coarse_angle_offset); - smapper_->getMapper()->setParamCoarseAngleResolution(position_search_coarse_angle_resolution); - smapper_->getMapper()->setParamLoopSearchSpaceResolution(position_search_resolution); - smapper_->getMapper()->setParamLoopSearchSpaceSmearDeviation(position_search_smear_deviation); - smapper_->getMapper()->setParamDoLoopClosing(do_loop_closing_flag); - smapper_->getMapper()->m_Initialized = false; - -} - -/*****************************************************************************/ -bool LocalizationSlamToolbox::desiredPoseCheck( - const std::shared_ptr req, - std::shared_ptr res) -/*****************************************************************************/ -{ - std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now(); - std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); - Matrix3 covariance; - LocalizedRangeScan * range_scan = nullptr; - covariance.SetToIdentity(); - { - boost::mutex::scoped_lock lock(smapper_mutex_); - smapper_->clearLocalizationBuffer(); - } - - if (req->pose_x == 0.0 || req->pose_y == 0.0) { - RCLCPP_ERROR(get_logger(), "Error: pose_x or pose_y is not provided or cannot be equal to <0.0>"); - res->message = "Error: pose_x or pose_y is missing. Please use it"; - res->success = false; - return false; - } - - if (req->do_relocalization){ - RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Searching for best response with relocalize"); - position_search_do_relocalization_ = true; - } else { - RCLCPP_INFO(get_logger(), "LocalizationSlamToolbox: Searching for best response without relocalize"); - position_search_do_relocalization_ = false; - } - - { - boost::mutex::scoped_lock l(pose_mutex_); - process_desired_pose_ = std::make_unique(req->pose_x, req->pose_y, 0.0); - } - - first_measurement_ = true; - - { - boost::mutex::scoped_lock lock(smapper_mutex_); - setInitialParametersForDesiredPose(position_search_distance_,((position_search_distance_*0.5)-1), position_search_fine_angle_offset_, - position_search_coarse_angle_offset_, position_search_coarse_angle_resolution_, - position_search_resolution_, position_search_smear_deviation_,true); - - if (req->search_distance != 0.0) { - position_search_distance_ = req->search_distance; - setInitialParametersForDesiredPose(position_search_distance_,((position_search_distance_*0.5)-1), position_search_fine_angle_offset_, - position_search_coarse_angle_offset_, position_search_coarse_angle_resolution_, - position_search_resolution_, position_search_smear_deviation_,true); - } - } - - if (!have_scan_values_) { - res->message = "No scan values stored try later"; - res->success = false; - return false; - } - else{ - bool processed = false; - { - boost::mutex::scoped_lock l(pose_mutex_); - process_desired_pose_ = std::make_unique(req->pose_x, req->pose_y, 0.0); - range_scan = getLocalizedRangeScan(last_laser_stored_, last_scan_stored_, last_odom_pose_stored_); - - boost::mutex::scoped_lock lock(smapper_mutex_); - range_scan->SetOdometricPose(*process_desired_pose_); - range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); - processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true, &covariance); - - if (processed) { - std::shared_ptr response = smapper_->getMapper()->GetBestResponse(); - double best_response = response->bestResponse; - double best_pose_x = response->bestPoseX; - double best_pose_y = response->bestPoseY; - - if (best_response > position_search_minimum_best_response_) { - - if (position_search_do_relocalization_) { - setTransformFromPoses(range_scan->GetCorrectedPose(), last_odom_pose_stored_, - last_scan_stored_->header.stamp, true); - - publishPose(range_scan->GetCorrectedPose(), covariance, last_scan_stored_->header.stamp); - } - else { - range_scan->SetOdometricPose(last_odom_pose_stored_); - range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); - } - - setInitialParametersForDesiredPose(this->get_parameter("loop_search_space_dimension").as_double(),this->get_parameter("loop_search_maximum_distance").as_double(), - this->get_parameter("fine_search_angle_offset").as_double(),this->get_parameter("coarse_search_angle_offset").as_double(), - this->get_parameter("coarse_angle_resolution").as_double(),this->get_parameter("loop_search_space_resolution").as_double(), - this->get_parameter("loop_search_space_smear_deviation").as_double(),this->get_parameter("do_loop_closing").as_bool()); - - res->message = "Found bestResponse"; - res->relocated_x= static_cast(best_pose_x); - res->relocated_y= static_cast(best_pose_y); - res->best_response = static_cast(best_response); - - res->success = true; - return true; - } else { - smapper_->clearLocalizationBuffer(); - setInitialParametersForDesiredPose(this->get_parameter("loop_search_space_dimension").as_double(),this->get_parameter("loop_search_maximum_distance").as_double(), - this->get_parameter("fine_search_angle_offset").as_double(),this->get_parameter("coarse_search_angle_offset").as_double(), - this->get_parameter("coarse_angle_resolution").as_double(),this->get_parameter("loop_search_space_resolution").as_double(), - this->get_parameter("loop_search_space_smear_deviation").as_double(),this->get_parameter("do_loop_closing").as_bool()); - - res->message = "Couldn't find bestResponse"; - res->success = false; - return false; - } - } - res->message = "Couldn't find with this resolution at desired time, halving the angle_resolution and decreasing best_response then search again."; - res->success = false; - return false; - } - } -} void LocalizationSlamToolbox::setInitialParameters(double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, double position_search_coarse_angle_offset, double position_search_coarse_angle_resolution, double position_search_resolution, @@ -414,7 +274,6 @@ void LocalizationSlamToolbox::set_parameters_callback( } boost::mutex::scoped_lock lock(smapper_mutex_); - std::cout << "Setting parameters for the specified mode" << std::endl; for (size_t i = 0; i < request->param_names.size(); ++i) { const auto ¶m_name = request->param_names[i]; @@ -427,7 +286,8 @@ void LocalizationSlamToolbox::set_parameters_callback( smapper_->getMapper()->setParamLoopSearchSpaceDimension(param_value); } else { - std::cerr << "Unknown parameter: " << param_name << std::endl; + RCLCPP_ERROR(get_logger(), + "Parameter not recognized. Returning false"); } } @@ -439,7 +299,6 @@ void LocalizationSlamToolbox::set_parameters_callback( } response->success = true; response->message = "Parameters set successfully for the specified mode"; - std::cout << "Parameters set successfully for the specified mode" << std::endl; } diff --git a/srvs/DesiredPoseChecker.srv b/srvs/DesiredPoseChecker.srv deleted file mode 100644 index 2d24a7fe2..000000000 --- a/srvs/DesiredPoseChecker.srv +++ /dev/null @@ -1,15 +0,0 @@ -float64 pose_x -float64 pose_y -float64 search_distance -bool do_relocalization - ---- -float64 best_response -float64 relocated_x -float64 relocated_y -float64 relocated_yaw -string message -bool success - - - From f31d35e7586b5496446a4ee97fc13b8844690202 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Fri, 22 Nov 2024 10:05:41 +0300 Subject: [PATCH 59/79] added newparams --- .../slam_toolbox_localization.hpp | 3 +- lib/karto_sdk/src/Mapper.cpp | 15 -------- src/slam_toolbox_localization.cpp | 35 +++++++++++++++++-- 3 files changed, 35 insertions(+), 18 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index 0877b66dc..3b5515d85 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -64,7 +64,8 @@ class LocalizationSlamToolbox : public SlamToolbox double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, double position_search_coarse_angle_offset, double position_search_coarse_angle_resolution, double position_search_resolution, - double position_search_smear_deviation, bool do_loop_closing_flag); + double position_search_smear_deviation, bool do_loop_closing_flag, + int scan_buffer_size); std::shared_ptr> localization_pose_sub_; diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 86c0ae6a0..d93e78a1f 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1111,15 +1111,6 @@ void ScanMatcher::AddScan( { PointVectorDouble validPoints = FindValidPoints(pScan, rViewPoint); -#ifdef KARTO_DEBUG - if (!validPoints.empty()) { - std::cout <<"validPoints is representing added points from chain" << std::endl; - std::cout << "validPoints: (" << validPoints.begin()->GetX() << ", " - << validPoints.begin()->GetY() << ")" << std::endl; - } else { - std::cout << "validPoints is empty!" << std::endl; - } -#endif // put in all valid points const_forEach(PointVectorDouble, &validPoints) { @@ -2158,12 +2149,6 @@ LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure( if (squaredDistance < math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE) { -#ifdef KARTO_DEBUG - std::cout << "Candidate Scan Position which is chains: (" - << pCandidateScan->GetCorrectedPose().GetX() << ", " - << pCandidateScan->GetCorrectedPose().GetY() << ", " - << pCandidateScan->GetCorrectedPose().GetHeading() << ")" << std::endl; -#endif // a linked scan cannot be in the chain if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end()) diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index dd8178c0e..1379ce840 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -225,7 +225,8 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( void LocalizationSlamToolbox::setInitialParameters(double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, double position_search_coarse_angle_offset, double position_search_coarse_angle_resolution, double position_search_resolution, - double position_search_smear_deviation,bool do_loop_closing_flag){ + double position_search_smear_deviation,bool do_loop_closing_flag, + int scan_buffer_size){ smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance); smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_maximum_distance); @@ -235,6 +236,7 @@ void LocalizationSlamToolbox::setInitialParameters(double position_search_distan smapper_->getMapper()->setParamLoopSearchSpaceResolution(position_search_resolution); smapper_->getMapper()->setParamLoopSearchSpaceSmearDeviation(position_search_smear_deviation); smapper_->getMapper()->setParamDoLoopClosing(do_loop_closing_flag); + smapper_->getMapper()->setParamScanBufferSize(scan_buffer_size); if(processor_type_ == PROCESS_LOCALIZATION){ smapper_->getMapper()->m_Initialized = false; } @@ -259,7 +261,8 @@ void LocalizationSlamToolbox::set_parameters_callback( this->get_parameter("coarse_angle_resolution").as_double(), this->get_parameter("loop_search_space_resolution").as_double(), this->get_parameter("loop_search_space_smear_deviation").as_double(), - this->get_parameter("do_loop_closing").as_bool() + this->get_parameter("do_loop_closing").as_bool(), + this->get_parameter("scan_buffer_size").as_int() ); response->success = true; @@ -274,6 +277,18 @@ void LocalizationSlamToolbox::set_parameters_callback( } boost::mutex::scoped_lock lock(smapper_mutex_); + // main purpose is resetting all the parameters first call. then initialize. + setInitialParameters( + this->get_parameter("loop_search_space_dimension").as_double(), + this->get_parameter("loop_search_maximum_distance").as_double(), + this->get_parameter("fine_search_angle_offset").as_double(), + this->get_parameter("coarse_search_angle_offset").as_double(), + this->get_parameter("coarse_angle_resolution").as_double(), + this->get_parameter("loop_search_space_resolution").as_double(), + this->get_parameter("loop_search_space_smear_deviation").as_double(), + this->get_parameter("do_loop_closing").as_bool(), + this->get_parameter("scan_buffer_size").as_int() + ); for (size_t i = 0; i < request->param_names.size(); ++i) { const auto ¶m_name = request->param_names[i]; @@ -285,6 +300,22 @@ void LocalizationSlamToolbox::set_parameters_callback( else if (param_name == "loop_search_space_dimension") { smapper_->getMapper()->setParamLoopSearchSpaceDimension(param_value); } + else if (param_name == "loop_search_space_resolution"){ + smapper_->getMapper()->setParamLoopSearchSpaceResolution(param_value); + } + else if (param_name == "loop_search_space_smear_deviation"){ + smapper_->getMapper()->setParamLoopSearchSpaceSmearDeviation(param_value); + } + else if (param_name == "loop_search_maximum_distance"){ + smapper_->getMapper()->setParamLoopSearchMaximumDistance(param_value); + } + else if (param_name == "correlation_search_space_dimension"){ + smapper_->getMapper()->setParamCorrelationSearchSpaceDimension(param_value); + } + else if (param_name == "scan_buffer_size"){ + int value =static_cast(param_value); + smapper_->getMapper()->setParamScanBufferSize(value); + } else { RCLCPP_ERROR(get_logger(), "Parameter not recognized. Returning false"); From 007618db6a5e7993dd870191ed3fd10ac76f593a Mon Sep 17 00:00:00 2001 From: Renbago Date: Mon, 25 Nov 2024 09:13:57 +0300 Subject: [PATCH 60/79] adding functions for tablemode --- .../slam_toolbox_localization.hpp | 2 + lib/karto_sdk/include/karto_sdk/Mapper.h | 29 +++++++++ lib/karto_sdk/src/Mapper.cpp | 59 ++++++++++++++++++- src/slam_toolbox_localization.cpp | 21 +++++-- srvs/SetParametersService.srv | 2 +- 5 files changed, 105 insertions(+), 8 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index 3b5515d85..ad5923296 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -67,6 +67,8 @@ class LocalizationSlamToolbox : public SlamToolbox double position_search_smear_deviation, bool do_loop_closing_flag, int scan_buffer_size); + void triggerTableSave(); + std::shared_ptr> localization_pose_sub_; diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 61408480c..672dfe964 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1658,6 +1658,12 @@ class KARTO_EXPORT MapperSensorManager */ void AddScan(LocalizedRangeScan * pScan); + /** + * Add table which is founded position of scan + * @param pScan + */ + void AddTable(LocalizedRangeScan * pScan); + /** * Adds scan to running scans of device that recorded scan * @param pScan @@ -1979,6 +1985,29 @@ class KARTO_EXPORT Mapper : public Module std::shared_ptr GetBestResponse() const; void SetBestResponse(const std::shared_ptr& response); + std::vector pScanVector; + kt_bool saveTableData_; + + void StartTableStorage(kt_bool saveTableData); + + void StoreAddress(LocalizedRangeScan* pScan) { + pScanVector.push_back(pScan); + std::cout << "Stored pScan at address: " << static_cast(pScan) + << " at index: " << pScanVector.size() - 1 << std::endl; + } + + void AccessByIndex(size_t index) { + if (index < pScanVector.size()) { + LocalizedRangeScan* pScan = pScanVector[index]; + std::cout << "Accessing pScan at address: " << static_cast(pScan) + << " with pose: " + << pScan->GetOdometricPose().GetX() << " " + << pScan->GetOdometricPose().GetY() << " " + << pScan->GetOdometricPose().GetHeading() << std::endl; + } else { + std::cout << "Index out of bounds!" << std::endl; + } + } /** * Allocate memory needed for mapping diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index d93e78a1f..b22ac5140 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1111,6 +1111,28 @@ void ScanMatcher::AddScan( { PointVectorDouble validPoints = FindValidPoints(pScan, rViewPoint); +#ifdef KARTO_DEBUG + if (!validPoints.empty()) { + std::cout <<"validPoints is representing added points from chain" << std::endl; + std::cout << "validPoints: (" << validPoints.begin()->GetX() << ", " + << validPoints.begin()->GetY() << ")" << std::endl; + } else { + std::cout << "validPoints is empty!" << std::endl; + } +#endif + + if (!validPoints.empty()) { + std::cout << "validPoints is representing added points from chain" << std::endl; + + // Print the first valid point's coordinates and address + std::cout << "validPoints: (" << validPoints.begin()->GetX() << ", " + << validPoints.begin()->GetY() << ") at address: " << &(*validPoints.begin()) << std::endl; + + + } else { + std::cout << "validPoints is empty!" << std::endl; + } + // put in all valid points const_forEach(PointVectorDouble, &validPoints) { @@ -2188,13 +2210,25 @@ void MapperGraph::CorrectPoses() #endif const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections()) { + std::cout << "Processing correction for ID: " << iter->first << std::endl; LocalizedRangeScan * scan = m_pMapper->m_pMapperSensorManager->GetScan(iter->first); if (scan == NULL) { continue; } + // Print the corrected pose (iter->second) + std::cout << "Corrected Pose: X=" << iter->second.GetX() + << " Y=" << iter->second.GetY() + << " Heading=" << iter->second.GetHeading() << std::endl; + scan->SetCorrectedPoseAndUpdate(iter->second); - } + + std::cout << "Updated Scan Pose: X=" << scan->GetCorrectedPose().GetX() + << " Y=" << scan->GetCorrectedPose().GetY() + << " Heading=" << scan->GetCorrectedPose().GetHeading() << std::endl; + std::cout << "Scan object address: " << scan << std::endl; + std::cout << "Corrected Pose Address: " << &iter->second << std::endl; + } pSolver->Clear(); } } @@ -2923,6 +2957,18 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) // add scan to buffer and assign id m_pMapperSensorManager->AddScan(pScan); + // size_t index = std::distance(deviceNames.begin(), iter); + // std::cout << "Index of current element: " << index << std::endl; + if (saveTableData_ == true) { + StoreAddress(pScan); + for( auto i = 0; i < 10; i++ ) { + AccessByIndex(i); + } + CorrectPoses(); + } + + // m_pMapperTableManager->AddTable(pScan); + // AccessByIndex(pScan); if (m_pUseScanMatching->GetValue()) { // add to graph @@ -2934,6 +2980,8 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) if (m_pDoLoopClosing->GetValue()) { std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); + size_t index = deviceNames.size(); + const_forEach(std::vector, &deviceNames) { m_pGraph->TryCloseLoop(pScan, *iter); @@ -2942,6 +2990,7 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) } m_pMapperSensorManager->SetLastScan(pScan); + std::cout <<"\n\nAddedScan is PRocess "<< pScan->GetOdometricPose().GetX() << " " << pScan->GetOdometricPose().GetY() << " " << pScan->GetOdometricPose().GetHeading() << std::endl; return true; } @@ -3145,7 +3194,8 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covari m_pMapperSensorManager->SetLastScan(pScan); AddScanToLocalizationBuffer(pScan, scan_vertex); - + std::cout <<"\n\nAddedScan is "<< pScan->GetOdometricPose().GetX() << " " << pScan->GetOdometricPose().GetY() << " " << pScan->GetOdometricPose().GetHeading() << std::endl; + // AddScanPoseToTableData(pScan); return true; } @@ -3516,6 +3566,11 @@ void Mapper::SetBestResponse(const std::shared_ptr& r } } +void Mapper::StartTableStorage(kt_bool saveTableData) +{ + saveTableData_ = saveTableData; +} + void Mapper::SetScanSolver(ScanSolver * pScanOptimizer) { m_pScanOptimizer = pScanOptimizer; diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 1379ce840..ae392c24f 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -41,8 +41,8 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) set_parameters_srv_ = this->create_service( "slam_toolbox/set_dynamic_parameters", - std::bind(&LocalizationSlamToolbox::set_parameters_callback, this, std::placeholders::_1, std::placeholders::_2)); - + std::bind(&LocalizationSlamToolbox::set_parameters_callback, this, + std::placeholders::_1, std::placeholders::_2)); // in localization mode, we cannot allow for interactive mode enable_interactive_mode_ = false; @@ -222,7 +222,6 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( return range_scan; } - void LocalizationSlamToolbox::setInitialParameters(double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, double position_search_coarse_angle_offset, double position_search_coarse_angle_resolution, double position_search_resolution, double position_search_smear_deviation,bool do_loop_closing_flag, @@ -245,11 +244,25 @@ void LocalizationSlamToolbox::setInitialParameters(double position_search_distan } } +void LocalizationSlamToolbox::triggerTableSave(){ + if (processor_type_ == PROCESS){ + RCLCPP_INFO(get_logger(), "Triggering table save function"); + boost::mutex::scoped_lock lock(smapper_mutex_); + smapper_->getMapper()->StartTableStorage(true); + + } +} + void LocalizationSlamToolbox::set_parameters_callback( const std::shared_ptr request, std::shared_ptr response ) { + if(request->table_save_mode){ + triggerTableSave(); + RCLCPP_INFO(get_logger(), "Triggering table save service"); + } + if (request->default_mode) { RCLCPP_INFO(get_logger(), "Setting parameters for the default mode"); boost::mutex::scoped_lock lock(smapper_mutex_); @@ -332,8 +345,6 @@ void LocalizationSlamToolbox::set_parameters_callback( response->message = "Parameters set successfully for the specified mode"; } - - /*****************************************************************************/ void LocalizationSlamToolbox::localizePoseCallback( const diff --git a/srvs/SetParametersService.srv b/srvs/SetParametersService.srv index 3363a2d6e..18ccfca28 100644 --- a/srvs/SetParametersService.srv +++ b/srvs/SetParametersService.srv @@ -1,7 +1,7 @@ string[] param_names float64[] param_values bool default_mode - +bool table_save_mode --- bool success From 395debb9d126e8f884d26059e756ec41ff052186 Mon Sep 17 00:00:00 2001 From: Renbago Date: Wed, 27 Nov 2024 13:54:05 +0300 Subject: [PATCH 61/79] added savetable function and updating saved table functions --- .../slam_toolbox_localization.hpp | 5 ++ include/slam_toolbox/visualization_utils.hpp | 68 +++++++++++++++++ lib/karto_sdk/include/karto_sdk/Mapper.h | 41 ++++++----- lib/karto_sdk/src/Mapper.cpp | 73 ++++++++++++++----- src/slam_toolbox_localization.cpp | 34 +++++++++ 5 files changed, 183 insertions(+), 38 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index ad5923296..118f7c080 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -68,6 +68,11 @@ class LocalizationSlamToolbox : public SlamToolbox int scan_buffer_size); void triggerTableSave(); + void getSavedTableData(); + + rclcpp::Publisher::SharedPtr marker_pub_; + bool tableSaveComplete_ = false; + std::shared_ptr> localization_pose_sub_; diff --git a/include/slam_toolbox/visualization_utils.hpp b/include/slam_toolbox/visualization_utils.hpp index bc053a4e2..4f96c8fea 100644 --- a/include/slam_toolbox/visualization_utils.hpp +++ b/include/slam_toolbox/visualization_utils.hpp @@ -140,6 +140,74 @@ inline void toNavMap( } } +inline visualization_msgs::msg::Marker toRectangleMarker( + const std::string & frame, + const std::string & ns, + const double & x, + const double & y, + const double & yaw, + const double & width, + const double & height, + const double & scale, + const std::array & color, // RGBA array + rclcpp::Node::SharedPtr node) +{ + visualization_msgs::msg::Marker marker; + + // Set header and metadata + marker.header.frame_id = frame; + marker.header.stamp = node->now(); + marker.ns = ns; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; + + marker.scale.x = scale; // Line width + marker.color.r = color[0]; + marker.color.g = color[1]; + marker.color.b = color[2]; + marker.color.a = color[3]; + + // Calculate rectangle corners + double half_width = width / 2.0; + double half_height = height / 2.0; + + double cos_yaw = cos(yaw); + double sin_yaw = sin(yaw); + + std::vector corners(5); + + // Top-right corner + corners[0].x = x + (cos_yaw * half_width - sin_yaw * half_height); + corners[0].y = y + (sin_yaw * half_width + cos_yaw * half_height); + corners[0].z = 0.0; + + // Top-left corner + corners[1].x = x + (cos_yaw * -half_width - sin_yaw * half_height); + corners[1].y = y + (sin_yaw * -half_width + cos_yaw * half_height); + corners[1].z = 0.0; + + // Bottom-left corner + corners[2].x = x + (cos_yaw * -half_width - sin_yaw * -half_height); + corners[2].y = y + (sin_yaw * -half_width + cos_yaw * -half_height); + corners[2].z = 0.0; + + // Bottom-right corner + corners[3].x = x + (cos_yaw * half_width - sin_yaw * -half_height); + corners[3].y = y + (sin_yaw * half_width + cos_yaw * -half_height); + corners[3].z = 0.0; + + // Close the rectangle + corners[4] = corners[0]; + + // Add corners to the marker + for (const auto & corner : corners) { + marker.points.push_back(corner); + } + + return marker; +} + } // namespace vis_utils #endif // SLAM_TOOLBOX__VISUALIZATION_UTILS_HPP_ diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 672dfe964..64e842ed6 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1985,30 +1985,31 @@ class KARTO_EXPORT Mapper : public Module std::shared_ptr GetBestResponse() const; void SetBestResponse(const std::shared_ptr& response); - std::vector pScanVector; + /** + * its for saving the desired pose of the robot + */ + struct TablePose { + double x; + double y; + double yaw; + kt_int32u scanId; + }; + enum class ProcessStatus { + Success, + TableSaved, + Failed + }; + + std::vector poseVector; kt_bool saveTableData_; void StartTableStorage(kt_bool saveTableData); + void StorePose(const LocalizedRangeScan* pScan); + void UpdateStoredPoses(); + bool tableSaveComplete_ = false; - void StoreAddress(LocalizedRangeScan* pScan) { - pScanVector.push_back(pScan); - std::cout << "Stored pScan at address: " << static_cast(pScan) - << " at index: " << pScanVector.size() - 1 << std::endl; - } - - void AccessByIndex(size_t index) { - if (index < pScanVector.size()) { - LocalizedRangeScan* pScan = pScanVector[index]; - std::cout << "Accessing pScan at address: " << static_cast(pScan) - << " with pose: " - << pScan->GetOdometricPose().GetX() << " " - << pScan->GetOdometricPose().GetY() << " " - << pScan->GetOdometricPose().GetHeading() << std::endl; - } else { - std::cout << "Index out of bounds!" << std::endl; - } - } - + // void updatedTableData(LocalizedRangeScan* pScan); + /** * Allocate memory needed for mapping * @param rangeThreshold diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index b22ac5140..ade1c5d1e 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1121,17 +1121,17 @@ void ScanMatcher::AddScan( } #endif - if (!validPoints.empty()) { - std::cout << "validPoints is representing added points from chain" << std::endl; + // if (!validPoints.empty()) { + // std::cout << "validPoints is representing added points from chain" << std::endl; - // Print the first valid point's coordinates and address - std::cout << "validPoints: (" << validPoints.begin()->GetX() << ", " - << validPoints.begin()->GetY() << ") at address: " << &(*validPoints.begin()) << std::endl; + // // Print the first valid point's coordinates and address + // std::cout << "validPoints: (" << validPoints.begin()->GetX() << ", " + // << validPoints.begin()->GetY() << ") at address: " << &(*validPoints.begin()) << std::endl; - } else { - std::cout << "validPoints is empty!" << std::endl; - } + // } else { + // std::cout << "validPoints is empty!" << std::endl; + // } // put in all valid points const_forEach(PointVectorDouble, &validPoints) @@ -2210,7 +2210,7 @@ void MapperGraph::CorrectPoses() #endif const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections()) { - std::cout << "Processing correction for ID: " << iter->first << std::endl; + std::cout << "\n\nProcessing correction for ID: " << iter->first << std::endl; LocalizedRangeScan * scan = m_pMapper->m_pMapperSensorManager->GetScan(iter->first); if (scan == NULL) { continue; @@ -2226,11 +2226,13 @@ void MapperGraph::CorrectPoses() << " Y=" << scan->GetCorrectedPose().GetY() << " Heading=" << scan->GetCorrectedPose().GetHeading() << std::endl; std::cout << "Scan object address: " << scan << std::endl; - std::cout << "Corrected Pose Address: " << &iter->second << std::endl; + std::cout << "Corrected Pose Address: " << &iter->second << "\n\n" << std::endl; } pSolver->Clear(); } + m_pMapper->UpdateStoredPoses(); + } void MapperGraph::UpdateLoopScanMatcher(kt_double rangeThreshold) @@ -2924,7 +2926,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 (!HasMovedEnough(pScan, pLastScan) && !saveTableData_) { return false; } @@ -2957,14 +2959,14 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) // add scan to buffer and assign id m_pMapperSensorManager->AddScan(pScan); - // size_t index = std::distance(deviceNames.begin(), iter); - // std::cout << "Index of current element: " << index << std::endl; + if (saveTableData_ == true) { - StoreAddress(pScan); - for( auto i = 0; i < 10; i++ ) { - AccessByIndex(i); - } - CorrectPoses(); + + StorePose(pScan); + std::cout << "POse of the scan: " << pScan->GetOdometricPose().GetX() << " " << pScan->GetOdometricPose().GetY() << + " " << pScan->GetOdometricPose().GetHeading() << std::endl; + + saveTableData_ = false; } // m_pMapperTableManager->AddTable(pScan); @@ -3566,6 +3568,41 @@ void Mapper::SetBestResponse(const std::shared_ptr& r } } +void Mapper::StorePose(const LocalizedRangeScan * pScan) +{ + TablePose pose; + pose.x = pScan->GetOdometricPose().GetX(); + pose.y = pScan->GetOdometricPose().GetY(); + pose.yaw = pScan->GetOdometricPose().GetHeading(); + pose.scanId = pScan->GetStateId(); + poseVector.push_back(pose); + std::cout << "Stored Pose: x=" << pose.x + << ", y=" << pose.y + << ", yaw=" << pose.yaw + << ", nScans=" << pose.scanId << std::endl; +} + +void Mapper::UpdateStoredPoses() +{ + for (auto& pose : poseVector) { + + LocalizedRangeScan* scan = m_pMapperSensorManager->GetScan(pose.scanId); + if (scan == nullptr) { + std::cerr << "No scan found for scanId: " << pose.scanId << std::endl; + continue; + } + + pose.x = scan->GetCorrectedPose().GetX(); + pose.y = scan->GetCorrectedPose().GetY(); + pose.yaw = scan->GetCorrectedPose().GetHeading(); + + std::cout << "Updated Stored Pose for scanId: " << pose.scanId + << " -> X=" << pose.x + << ", Y=" << pose.y + << ", Yaw=" << pose.yaw << std::endl; + } +} + void Mapper::StartTableStorage(kt_bool saveTableData) { saveTableData_ = saveTableData; diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index ae392c24f..aac372072 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -44,6 +44,9 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) std::bind(&LocalizationSlamToolbox::set_parameters_callback, this, std::placeholders::_1, std::placeholders::_2)); + marker_pub_ = this->create_publisher( + "slam_toolbox/table_vis", 10); + // in localization mode, we cannot allow for interactive mode enable_interactive_mode_ = false; @@ -249,7 +252,38 @@ void LocalizationSlamToolbox::triggerTableSave(){ RCLCPP_INFO(get_logger(), "Triggering table save function"); boost::mutex::scoped_lock lock(smapper_mutex_); smapper_->getMapper()->StartTableStorage(true); + // but its get the last data. + getSavedTableData(); + } +} +void LocalizationSlamToolbox::getSavedTableData(){ + if (processor_type_ == PROCESS){ + RCLCPP_INFO(get_logger(), "Getting saved table data"); + std::cout << "Pose vector size: " << smapper_->getMapper()->poseVector.size() << std::endl; + if (smapper_->getMapper()->poseVector.size() > 0) { + std::vector& localPoseVector = smapper_->getMapper()->poseVector; + visualization_msgs::msg::MarkerArray marker_array; + for (const auto& pose : localPoseVector) { + std::cout << "Pose ID: " << pose.scanId + << ", X: " << pose.x + << ", Y: " << pose.y + << ", Yaw: " << pose.yaw << std::endl; + visualization_msgs::msg::Marker rectangle_marker = + vis_utils::toRectangleMarker( + "map", // Frame ID + "pose_visualization", // Namespace + pose.x, pose.y, pose.yaw, // Pose: x, y, yaw + 1.0, 0.1, // Width and height of the rectangle + 0.1, // Line width (scale) + {1.0, 0.0, 0.0, 1.0}, // Color: red (RGBA) + shared_from_this()); // Node pointer + + rectangle_marker.id = pose.scanId; + marker_array.markers.push_back(rectangle_marker); + } + marker_pub_->publish(marker_array); + } } } From fe272f1e324bb7064fba2de97dcad0c49be8b8eb Mon Sep 17 00:00:00 2001 From: Renbago Date: Wed, 27 Nov 2024 16:01:05 +0300 Subject: [PATCH 62/79] vis changed --- include/slam_toolbox/visualization_utils.hpp | 57 ++++++-------------- src/slam_toolbox_localization.cpp | 42 ++++++++------- 2 files changed, 38 insertions(+), 61 deletions(-) diff --git a/include/slam_toolbox/visualization_utils.hpp b/include/slam_toolbox/visualization_utils.hpp index 4f96c8fea..bdaa8f49b 100644 --- a/include/slam_toolbox/visualization_utils.hpp +++ b/include/slam_toolbox/visualization_utils.hpp @@ -149,7 +149,7 @@ inline visualization_msgs::msg::Marker toRectangleMarker( const double & width, const double & height, const double & scale, - const std::array & color, // RGBA array + const std::array & color, rclcpp::Node::SharedPtr node) { visualization_msgs::msg::Marker marker; @@ -158,53 +158,26 @@ inline visualization_msgs::msg::Marker toRectangleMarker( marker.header.frame_id = frame; marker.header.stamp = node->now(); marker.ns = ns; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.type = visualization_msgs::msg::Marker::SPHERE; marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.scale.x = scale; // Line width + // Set position + marker.pose.position.x = x; + marker.pose.position.y = y; + marker.pose.position.z = 0.0; + marker.pose.orientation.w = 1.0; // No rotation for a sphere + + // Set scale + marker.scale.x = scale; + marker.scale.y = scale; + marker.scale.z = scale; + + // Set color marker.color.r = color[0]; marker.color.g = color[1]; marker.color.b = color[2]; marker.color.a = color[3]; - - // Calculate rectangle corners - double half_width = width / 2.0; - double half_height = height / 2.0; - - double cos_yaw = cos(yaw); - double sin_yaw = sin(yaw); - - std::vector corners(5); - - // Top-right corner - corners[0].x = x + (cos_yaw * half_width - sin_yaw * half_height); - corners[0].y = y + (sin_yaw * half_width + cos_yaw * half_height); - corners[0].z = 0.0; - - // Top-left corner - corners[1].x = x + (cos_yaw * -half_width - sin_yaw * half_height); - corners[1].y = y + (sin_yaw * -half_width + cos_yaw * half_height); - corners[1].z = 0.0; - - // Bottom-left corner - corners[2].x = x + (cos_yaw * -half_width - sin_yaw * -half_height); - corners[2].y = y + (sin_yaw * -half_width + cos_yaw * -half_height); - corners[2].z = 0.0; - - // Bottom-right corner - corners[3].x = x + (cos_yaw * half_width - sin_yaw * -half_height); - corners[3].y = y + (sin_yaw * half_width + cos_yaw * -half_height); - corners[3].z = 0.0; - - // Close the rectangle - corners[4] = corners[0]; - - // Add corners to the marker - for (const auto & corner : corners) { - marker.points.push_back(corner); - } - + return marker; } diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index aac372072..8d58480fa 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -257,31 +257,35 @@ void LocalizationSlamToolbox::triggerTableSave(){ } } -void LocalizationSlamToolbox::getSavedTableData(){ - if (processor_type_ == PROCESS){ +void LocalizationSlamToolbox::getSavedTableData() { + if (processor_type_ == PROCESS) { RCLCPP_INFO(get_logger(), "Getting saved table data"); std::cout << "Pose vector size: " << smapper_->getMapper()->poseVector.size() << std::endl; + if (smapper_->getMapper()->poseVector.size() > 0) { - std::vector& localPoseVector = smapper_->getMapper()->poseVector; + const std::vector& localPoseVector = smapper_->getMapper()->poseVector; visualization_msgs::msg::MarkerArray marker_array; + for (const auto& pose : localPoseVector) { - std::cout << "Pose ID: " << pose.scanId - << ", X: " << pose.x - << ", Y: " << pose.y - << ", Yaw: " << pose.yaw << std::endl; - visualization_msgs::msg::Marker rectangle_marker = - vis_utils::toRectangleMarker( - "map", // Frame ID - "pose_visualization", // Namespace - pose.x, pose.y, pose.yaw, // Pose: x, y, yaw - 1.0, 0.1, // Width and height of the rectangle - 0.1, // Line width (scale) - {1.0, 0.0, 0.0, 1.0}, // Color: red (RGBA) - shared_from_this()); // Node pointer - - rectangle_marker.id = pose.scanId; - marker_array.markers.push_back(rectangle_marker); + std::cout << "Pose ID: " << pose.scanId + << ", X: " << pose.x + << ", Y: " << pose.y + << ", Yaw: " << pose.yaw << std::endl; + + // Create a sphere marker for the pose + visualization_msgs::msg::Marker sphere_marker = + vis_utils::toSphereMarker( + "map", // Frame ID + "pose_visualization", // Namespace + pose.x, pose.y, pose.yaw, // Position (yaw not visualized in a sphere) + 0.2, // Scale (sphere radius) + {0.0, 1.0, 0.0, 1.0}, // Color: green (RGBA) + shared_from_this()); // Node pointer + + sphere_marker.id = pose.scanId; // Assign unique ID to avoid marker conflicts + marker_array.markers.push_back(sphere_marker); } + marker_pub_->publish(marker_array); } } From 161fec9921a6da2d1e5e1a0fc47a589649088b36 Mon Sep 17 00:00:00 2001 From: Renbago Date: Thu, 28 Nov 2024 13:47:22 +0300 Subject: [PATCH 63/79] changed vis to rectangle and changed odometricpose --- include/slam_toolbox/visualization_utils.hpp | 57 ++++++++++++++------ lib/karto_sdk/src/Mapper.cpp | 6 +-- 2 files changed, 45 insertions(+), 18 deletions(-) diff --git a/include/slam_toolbox/visualization_utils.hpp b/include/slam_toolbox/visualization_utils.hpp index bdaa8f49b..4f96c8fea 100644 --- a/include/slam_toolbox/visualization_utils.hpp +++ b/include/slam_toolbox/visualization_utils.hpp @@ -149,7 +149,7 @@ inline visualization_msgs::msg::Marker toRectangleMarker( const double & width, const double & height, const double & scale, - const std::array & color, + const std::array & color, // RGBA array rclcpp::Node::SharedPtr node) { visualization_msgs::msg::Marker marker; @@ -158,26 +158,53 @@ inline visualization_msgs::msg::Marker toRectangleMarker( marker.header.frame_id = frame; marker.header.stamp = node->now(); marker.ns = ns; - marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; - // Set position - marker.pose.position.x = x; - marker.pose.position.y = y; - marker.pose.position.z = 0.0; - marker.pose.orientation.w = 1.0; // No rotation for a sphere - - // Set scale - marker.scale.x = scale; - marker.scale.y = scale; - marker.scale.z = scale; - - // Set color + marker.scale.x = scale; // Line width marker.color.r = color[0]; marker.color.g = color[1]; marker.color.b = color[2]; marker.color.a = color[3]; - + + // Calculate rectangle corners + double half_width = width / 2.0; + double half_height = height / 2.0; + + double cos_yaw = cos(yaw); + double sin_yaw = sin(yaw); + + std::vector corners(5); + + // Top-right corner + corners[0].x = x + (cos_yaw * half_width - sin_yaw * half_height); + corners[0].y = y + (sin_yaw * half_width + cos_yaw * half_height); + corners[0].z = 0.0; + + // Top-left corner + corners[1].x = x + (cos_yaw * -half_width - sin_yaw * half_height); + corners[1].y = y + (sin_yaw * -half_width + cos_yaw * half_height); + corners[1].z = 0.0; + + // Bottom-left corner + corners[2].x = x + (cos_yaw * -half_width - sin_yaw * -half_height); + corners[2].y = y + (sin_yaw * -half_width + cos_yaw * -half_height); + corners[2].z = 0.0; + + // Bottom-right corner + corners[3].x = x + (cos_yaw * half_width - sin_yaw * -half_height); + corners[3].y = y + (sin_yaw * half_width + cos_yaw * -half_height); + corners[3].z = 0.0; + + // Close the rectangle + corners[4] = corners[0]; + + // Add corners to the marker + for (const auto & corner : corners) { + marker.points.push_back(corner); + } + return marker; } diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index ade1c5d1e..045458596 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -3571,9 +3571,9 @@ void Mapper::SetBestResponse(const std::shared_ptr& r void Mapper::StorePose(const LocalizedRangeScan * pScan) { TablePose pose; - pose.x = pScan->GetOdometricPose().GetX(); - pose.y = pScan->GetOdometricPose().GetY(); - pose.yaw = pScan->GetOdometricPose().GetHeading(); + pose.x = pScan->GetCorrectedPose().GetX(); + pose.y = pScan->GetCorrectedPose().GetY(); + pose.yaw = pScan->GetCorrectedPose().GetHeading(); pose.scanId = pScan->GetStateId(); poseVector.push_back(pose); std::cout << "Stored Pose: x=" << pose.x From 0c7d52e9be27d79741eb537b2591aad564207171 Mon Sep 17 00:00:00 2001 From: Renbago Date: Thu, 28 Nov 2024 13:47:59 +0300 Subject: [PATCH 64/79] for always updating the array its implemented in the publishgraph which is automaticly doing it. --- src/loop_closure_assistant.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/loop_closure_assistant.cpp b/src/loop_closure_assistant.cpp index e73a20f96..71f30861d 100644 --- a/src/loop_closure_assistant.cpp +++ b/src/loop_closure_assistant.cpp @@ -259,6 +259,23 @@ void LoopClosureAssistant::publishGraph() marray.markers.push_back(edges_marker); marray.markers.push_back(localization_edges_marker); + // Here is for + const auto& localPoseVector = mapper_->poseVector; + for (const auto& pose : localPoseVector) { + visualization_msgs::msg::Marker rectangle_marker = + vis_utils::toRectangleMarker( + "map", // Frame ID + "pose_visualization", // Namespace + pose.x, pose.y, pose.yaw, // Pose: x, y, yaw + 1.0, 0.1, // Width and height of the rectangle + 0.1, // Line width (scale) + {1.0, 0.0, 0.0, 1.0}, // Color: red (RGBA) + node_); // Node pointer + + rectangle_marker.id = pose.scanId + 5000; // Ensure unique IDs for these markers + marray.markers.push_back(rectangle_marker); + } + // if disabled, clears out old markers interactive_server_->applyChanges(); marker_publisher_->publish(marray); From becb4ba2127e14787517fa82dc27a7c5a18ceac3 Mon Sep 17 00:00:00 2001 From: Renbago Date: Thu, 28 Nov 2024 13:48:35 +0300 Subject: [PATCH 65/79] for now get comment this section referance past commit. --- src/slam_toolbox_localization.cpp | 66 +++++++++++++++---------------- 1 file changed, 32 insertions(+), 34 deletions(-) diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 8d58480fa..a411de7cf 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -253,43 +253,41 @@ void LocalizationSlamToolbox::triggerTableSave(){ boost::mutex::scoped_lock lock(smapper_mutex_); smapper_->getMapper()->StartTableStorage(true); // but its get the last data. - getSavedTableData(); + // getSavedTableData(); } } -void LocalizationSlamToolbox::getSavedTableData() { - if (processor_type_ == PROCESS) { - RCLCPP_INFO(get_logger(), "Getting saved table data"); - std::cout << "Pose vector size: " << smapper_->getMapper()->poseVector.size() << std::endl; - - if (smapper_->getMapper()->poseVector.size() > 0) { - const std::vector& localPoseVector = smapper_->getMapper()->poseVector; - visualization_msgs::msg::MarkerArray marker_array; - - for (const auto& pose : localPoseVector) { - std::cout << "Pose ID: " << pose.scanId - << ", X: " << pose.x - << ", Y: " << pose.y - << ", Yaw: " << pose.yaw << std::endl; - - // Create a sphere marker for the pose - visualization_msgs::msg::Marker sphere_marker = - vis_utils::toSphereMarker( - "map", // Frame ID - "pose_visualization", // Namespace - pose.x, pose.y, pose.yaw, // Position (yaw not visualized in a sphere) - 0.2, // Scale (sphere radius) - {0.0, 1.0, 0.0, 1.0}, // Color: green (RGBA) - shared_from_this()); // Node pointer - - sphere_marker.id = pose.scanId; // Assign unique ID to avoid marker conflicts - marker_array.markers.push_back(sphere_marker); - } - - marker_pub_->publish(marker_array); - } - } -} +// void LocalizationSlamToolbox::getSavedTableData() { +// if (processor_type_ == PROCESS) { +// RCLCPP_INFO(get_logger(), "Getting saved table data"); +// std::cout << "Pose vector size: " << smapper_->getMapper()->poseVector.size() << std::endl; +// std::cout <<" map frame" << map_frame_ << std::endl; + +// if (smapper_->getMapper()->poseVector.size() > 0) { +// std::vector& localPoseVector = smapper_->getMapper()->poseVector; +// visualization_msgs::msg::MarkerArray marker_array; +// for (const auto& pose : localPoseVector) { +// std::cout << "Pose ID: " << pose.scanId +// << ", X: " << pose.x +// << ", Y: " << pose.y +// << ", Yaw: " << pose.yaw << std::endl; +// visualization_msgs::msg::Marker rectangle_marker = +// vis_utils::toRectangleMarker( +// "map", // Frame ID +// "pose_visualization", // Namespace +// pose.x, pose.y, pose.yaw, // Pose: x, y, yaw +// 1.0, 0.1, // Width and height of the rectangle +// 0.1, // Line width (scale) +// {1.0, 0.0, 0.0, 1.0}, // Color: red (RGBA) +// shared_from_this()); // Node pointer + +// rectangle_marker.id = pose.scanId; +// marker_array.markers.push_back(rectangle_marker); +// } +// marker_pub_->publish(marker_array); +// } +// } +// } void LocalizationSlamToolbox::set_parameters_callback( const std::shared_ptr request, From 6be5ea44a0c1fc0d761ece06aaed2071574c0d6e Mon Sep 17 00:00:00 2001 From: Renbago Date: Fri, 29 Nov 2024 10:11:49 +0300 Subject: [PATCH 66/79] its v1.0, currently able to add any pose as table by calling the service at desired pose and its also updated after loopclosure. --- lib/karto_sdk/include/karto_sdk/Mapper.h | 4 +-- src/slam_toolbox_localization.cpp | 35 +----------------------- 2 files changed, 3 insertions(+), 36 deletions(-) diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 64e842ed6..2599bb070 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -2001,12 +2001,12 @@ class KARTO_EXPORT Mapper : public Module }; std::vector poseVector; - kt_bool saveTableData_; + kt_bool saveTableData_{false}; void StartTableStorage(kt_bool saveTableData); void StorePose(const LocalizedRangeScan* pScan); void UpdateStoredPoses(); - bool tableSaveComplete_ = false; + bool tableSaveComplete_{false}; // void updatedTableData(LocalizedRangeScan* pScan); diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index a411de7cf..3bb18d5ae 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -252,43 +252,10 @@ void LocalizationSlamToolbox::triggerTableSave(){ RCLCPP_INFO(get_logger(), "Triggering table save function"); boost::mutex::scoped_lock lock(smapper_mutex_); smapper_->getMapper()->StartTableStorage(true); - // but its get the last data. - // getSavedTableData(); + } } -// void LocalizationSlamToolbox::getSavedTableData() { -// if (processor_type_ == PROCESS) { -// RCLCPP_INFO(get_logger(), "Getting saved table data"); -// std::cout << "Pose vector size: " << smapper_->getMapper()->poseVector.size() << std::endl; -// std::cout <<" map frame" << map_frame_ << std::endl; - -// if (smapper_->getMapper()->poseVector.size() > 0) { -// std::vector& localPoseVector = smapper_->getMapper()->poseVector; -// visualization_msgs::msg::MarkerArray marker_array; -// for (const auto& pose : localPoseVector) { -// std::cout << "Pose ID: " << pose.scanId -// << ", X: " << pose.x -// << ", Y: " << pose.y -// << ", Yaw: " << pose.yaw << std::endl; -// visualization_msgs::msg::Marker rectangle_marker = -// vis_utils::toRectangleMarker( -// "map", // Frame ID -// "pose_visualization", // Namespace -// pose.x, pose.y, pose.yaw, // Pose: x, y, yaw -// 1.0, 0.1, // Width and height of the rectangle -// 0.1, // Line width (scale) -// {1.0, 0.0, 0.0, 1.0}, // Color: red (RGBA) -// shared_from_this()); // Node pointer - -// rectangle_marker.id = pose.scanId; -// marker_array.markers.push_back(rectangle_marker); -// } -// marker_pub_->publish(marker_array); -// } -// } -// } - void LocalizationSlamToolbox::set_parameters_callback( const std::shared_ptr request, std::shared_ptr response From fc2c0d964561b6fbfc7f92b2cbcb3d0c9cbacf16 Mon Sep 17 00:00:00 2001 From: Renbago Date: Fri, 29 Nov 2024 10:33:25 +0300 Subject: [PATCH 67/79] deleted unnecessary things --- lib/karto_sdk/include/karto_sdk/Mapper.h | 2 -- lib/karto_sdk/src/Mapper.cpp | 43 ++++-------------------- 2 files changed, 7 insertions(+), 38 deletions(-) diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 2599bb070..44acdbb0a 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -2007,8 +2007,6 @@ class KARTO_EXPORT Mapper : public Module void StorePose(const LocalizedRangeScan* pScan); void UpdateStoredPoses(); bool tableSaveComplete_{false}; - - // void updatedTableData(LocalizedRangeScan* pScan); /** * Allocate memory needed for mapping diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 045458596..886a70ae8 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -1113,26 +1113,17 @@ void ScanMatcher::AddScan( #ifdef KARTO_DEBUG if (!validPoints.empty()) { - std::cout <<"validPoints is representing added points from chain" << std::endl; + std::cout << "validPoints is representing added points from chain" << std::endl; + + // Print the first valid point's coordinates and address std::cout << "validPoints: (" << validPoints.begin()->GetX() << ", " - << validPoints.begin()->GetY() << ")" << std::endl; + << validPoints.begin()->GetY() << ") at address: " << &(*validPoints.begin()) << std::endl; + } else { std::cout << "validPoints is empty!" << std::endl; } #endif - // if (!validPoints.empty()) { - // std::cout << "validPoints is representing added points from chain" << std::endl; - - // // Print the first valid point's coordinates and address - // std::cout << "validPoints: (" << validPoints.begin()->GetX() << ", " - // << validPoints.begin()->GetY() << ") at address: " << &(*validPoints.begin()) << std::endl; - - - // } else { - // std::cout << "validPoints is empty!" << std::endl; - // } - // put in all valid points const_forEach(PointVectorDouble, &validPoints) { @@ -2210,24 +2201,11 @@ void MapperGraph::CorrectPoses() #endif const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections()) { - std::cout << "\n\nProcessing correction for ID: " << iter->first << std::endl; LocalizedRangeScan * scan = m_pMapper->m_pMapperSensorManager->GetScan(iter->first); if (scan == NULL) { continue; } - // Print the corrected pose (iter->second) - std::cout << "Corrected Pose: X=" << iter->second.GetX() - << " Y=" << iter->second.GetY() - << " Heading=" << iter->second.GetHeading() << std::endl; - - scan->SetCorrectedPoseAndUpdate(iter->second); - - std::cout << "Updated Scan Pose: X=" << scan->GetCorrectedPose().GetX() - << " Y=" << scan->GetCorrectedPose().GetY() - << " Heading=" << scan->GetCorrectedPose().GetHeading() << std::endl; - std::cout << "Scan object address: " << scan << std::endl; - std::cout << "Corrected Pose Address: " << &iter->second << "\n\n" << std::endl; - + scan->SetCorrectedPoseAndUpdate(iter->second); } pSolver->Clear(); } @@ -2961,17 +2939,11 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) m_pMapperSensorManager->AddScan(pScan); if (saveTableData_ == true) { - + std::cout << "\n\nStoreTablePose function has been called\n"; StorePose(pScan); - std::cout << "POse of the scan: " << pScan->GetOdometricPose().GetX() << " " << pScan->GetOdometricPose().GetY() << - " " << pScan->GetOdometricPose().GetHeading() << std::endl; - saveTableData_ = false; } - // m_pMapperTableManager->AddTable(pScan); - // AccessByIndex(pScan); - if (m_pUseScanMatching->GetValue()) { // add to graph @@ -2992,7 +2964,6 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) } m_pMapperSensorManager->SetLastScan(pScan); - std::cout <<"\n\nAddedScan is PRocess "<< pScan->GetOdometricPose().GetX() << " " << pScan->GetOdometricPose().GetY() << " " << pScan->GetOdometricPose().GetHeading() << std::endl; return true; } From c9e3494f1ec1d62bd44af1db8fa8e5a325f488e3 Mon Sep 17 00:00:00 2001 From: Renbago Date: Wed, 4 Dec 2024 08:52:52 +0300 Subject: [PATCH 68/79] added targetinfo msg dependencies for publishing topic --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index ddca2a27e..a4cca49f1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -107,6 +107,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} srvs/DeserializePoseGraph.srv srvs/SerializePoseGraph.srv srvs/SetParametersService.srv + msgs/SavedTargetInfo.msg + msgs/SavedTargetInfoArray.msg DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs ) From f3ab035fa607c8bfd18c11d640e004089e9f9320 Mon Sep 17 00:00:00 2001 From: Renbago Date: Wed, 4 Dec 2024 09:01:15 +0300 Subject: [PATCH 69/79] added dependencies --- include/slam_toolbox/slam_toolbox_common.hpp | 1 + include/slam_toolbox/toolbox_msgs.hpp | 3 +++ 2 files changed, 4 insertions(+) diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index ef9f142c9..96b521265 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -138,6 +138,7 @@ class SlamToolbox : public rclcpp::Node std::shared_ptr> sstm_; std::shared_ptr> pose_pub_; std::shared_ptr> localization_health_pub_; // TODO: Change here + std::shared_ptr> ssSaved_target_data_; std::shared_ptr> ssMap_; std::shared_ptr> ssPauseMeasurements_; std::shared_ptr> ssSerialize_; diff --git a/include/slam_toolbox/toolbox_msgs.hpp b/include/slam_toolbox/toolbox_msgs.hpp index c333a206d..8d1d1572d 100644 --- a/include/slam_toolbox/toolbox_msgs.hpp +++ b/include/slam_toolbox/toolbox_msgs.hpp @@ -29,6 +29,9 @@ #include "visualization_msgs/msg/interactive_marker_control.hpp" #include "visualization_msgs/msg/interactive_marker_feedback.hpp" +#include "slam_toolbox/msg/saved_target_info.hpp" +#include "slam_toolbox/msg/saved_target_info_array.hpp" + #include "slam_toolbox/srv/pause.hpp" #include "slam_toolbox/srv/clear_queue.hpp" #include "slam_toolbox/srv/toggle_interactive.hpp" From b5c278433c36ef97b4347f51f5a526eb3a5e9e63 Mon Sep 17 00:00:00 2001 From: Renbago Date: Wed, 4 Dec 2024 09:01:58 +0300 Subject: [PATCH 70/79] removed function --- include/slam_toolbox/slam_toolbox_localization.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/include/slam_toolbox/slam_toolbox_localization.hpp b/include/slam_toolbox/slam_toolbox_localization.hpp index 118f7c080..ef123f467 100644 --- a/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/include/slam_toolbox/slam_toolbox_localization.hpp @@ -67,7 +67,6 @@ class LocalizationSlamToolbox : public SlamToolbox double position_search_smear_deviation, bool do_loop_closing_flag, int scan_buffer_size); - void triggerTableSave(); void getSavedTableData(); rclcpp::Publisher::SharedPtr marker_pub_; From e0c515d44bfba26c2ae6a43c2b169ea90dbae962 Mon Sep 17 00:00:00 2001 From: Renbago Date: Wed, 4 Dec 2024 09:03:41 +0300 Subject: [PATCH 71/79] added tableVectorUpdated_ and targetname_ , the feedback is only publishing when the poses has updated. --- lib/karto_sdk/include/karto_sdk/Mapper.h | 11 ++++------- lib/karto_sdk/src/Mapper.cpp | 10 +++++++--- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index 44acdbb0a..aa49ce47a 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1993,17 +1993,14 @@ class KARTO_EXPORT Mapper : public Module double y; double yaw; kt_int32u scanId; - }; - enum class ProcessStatus { - Success, - TableSaved, - Failed + std::string targetName; }; std::vector poseVector; kt_bool saveTableData_{false}; - - void StartTableStorage(kt_bool saveTableData); + std::string saveTargetName_ = "masa_0"; + kt_bool tableVectorUpdated_{false}; + void StartTableStorage(kt_bool saveTableData, const std::string & saveTargetName); void StorePose(const LocalizedRangeScan* pScan); void UpdateStoredPoses(); bool tableSaveComplete_{false}; diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 886a70ae8..78c7928ec 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -2941,7 +2941,7 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan, Matrix3 * covariance) if (saveTableData_ == true) { std::cout << "\n\nStoreTablePose function has been called\n"; StorePose(pScan); - saveTableData_ = false; + saveTableData_= false; } if (m_pUseScanMatching->GetValue()) { @@ -3546,11 +3546,13 @@ void Mapper::StorePose(const LocalizedRangeScan * pScan) pose.y = pScan->GetCorrectedPose().GetY(); pose.yaw = pScan->GetCorrectedPose().GetHeading(); pose.scanId = pScan->GetStateId(); + pose.targetName = saveTargetName_; poseVector.push_back(pose); std::cout << "Stored Pose: x=" << pose.x << ", y=" << pose.y << ", yaw=" << pose.yaw - << ", nScans=" << pose.scanId << std::endl; + << ", nScans=" << pose.scanId + << ", targetName=" << pose.targetName << std::endl; } void Mapper::UpdateStoredPoses() @@ -3572,11 +3574,13 @@ void Mapper::UpdateStoredPoses() << ", Y=" << pose.y << ", Yaw=" << pose.yaw << std::endl; } + tableVectorUpdated_ = true; } -void Mapper::StartTableStorage(kt_bool saveTableData) +void Mapper::StartTableStorage(kt_bool saveTableData, const std::string& saveTargetName) { saveTableData_ = saveTableData; + saveTargetName_ = saveTargetName; } void Mapper::SetScanSolver(ScanSolver * pScanOptimizer) From 1a9eeac9641b6ced725cbd1b8e575a5c6b306b39 Mon Sep 17 00:00:00 2001 From: Renbago Date: Wed, 4 Dec 2024 09:03:59 +0300 Subject: [PATCH 72/79] new message formats --- msgs/SavedTargetInfo.msg | 5 +++++ msgs/SavedTargetInfoArray.msg | 1 + 2 files changed, 6 insertions(+) create mode 100644 msgs/SavedTargetInfo.msg create mode 100644 msgs/SavedTargetInfoArray.msg diff --git a/msgs/SavedTargetInfo.msg b/msgs/SavedTargetInfo.msg new file mode 100644 index 000000000..1f699059e --- /dev/null +++ b/msgs/SavedTargetInfo.msg @@ -0,0 +1,5 @@ +float64 x +float64 y +float64 yaw +uint32 scan_id +string target_uid \ No newline at end of file diff --git a/msgs/SavedTargetInfoArray.msg b/msgs/SavedTargetInfoArray.msg new file mode 100644 index 000000000..87d7d5fc9 --- /dev/null +++ b/msgs/SavedTargetInfoArray.msg @@ -0,0 +1 @@ +slam_toolbox/SavedTargetInfo[] targets \ No newline at end of file From d7453fdfac4cee3df945a1c1c6ee2e30a0870197 Mon Sep 17 00:00:00 2001 From: Renbago Date: Wed, 4 Dec 2024 09:06:19 +0300 Subject: [PATCH 73/79] added tableVectorUpdated_ checking into addscan function. if the mode is PROCESS that's meaning we are mapping. so automaticly checking everytime. --- src/slam_toolbox_common.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index f4bcf735c..b7b172f64 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -272,6 +272,9 @@ void SlamToolbox::setROSInterfaces() std::bind(&SlamToolbox::deserializePoseGraphCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + ssSaved_target_data_ = this->create_publisher( + "slam_toolbox/saved_target_data", qos); + scan_filter_sub_ = std::make_unique>( shared_from_this().get(), scan_topic_, rmw_qos_profile_sensor_data); @@ -648,6 +651,24 @@ LocalizedRangeScan * SlamToolbox::addScan( if (processor_type_ == PROCESS) { processed = smapper_->getMapper()->Process(range_scan, &covariance); + if(smapper_->getMapper()->tableVectorUpdated_){ + + smapper_->getMapper()->tableVectorUpdated_= false; + auto target_array_msg = slam_toolbox::msg::SavedTargetInfoArray(); + + for (const auto &pose : smapper_->getMapper()->poseVector) { + slam_toolbox::msg::SavedTargetInfo target_msg; + target_msg.x = pose.x; + target_msg.y = pose.y; + target_msg.yaw = pose.yaw; + target_msg.scan_id = pose.scanId; + target_msg.target_uid = pose.targetName; + + target_array_msg.targets.push_back(target_msg); + } + + ssSaved_target_data_->publish(target_array_msg); + } } else if (processor_type_ == PROCESS_FIRST_NODE) { processed = smapper_->getMapper()->ProcessAtDock(range_scan, &covariance); processor_type_ = PROCESS; From 082cefaaa63a4b21509f7fb650ca1f4cf4b132a4 Mon Sep 17 00:00:00 2001 From: Renbago Date: Wed, 4 Dec 2024 09:11:39 +0300 Subject: [PATCH 74/79] added target_save and target_list_off modes if target_list_off called its publish current data's if its table_save_mode storage current uid which requested --- src/slam_toolbox_localization.cpp | 42 +++++++++++++++++++------------ 1 file changed, 26 insertions(+), 16 deletions(-) diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 3bb18d5ae..1aaf14849 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -44,9 +44,6 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options) std::bind(&LocalizationSlamToolbox::set_parameters_callback, this, std::placeholders::_1, std::placeholders::_2)); - marker_pub_ = this->create_publisher( - "slam_toolbox/table_vis", 10); - // in localization mode, we cannot allow for interactive mode enable_interactive_mode_ = false; @@ -224,12 +221,13 @@ LocalizedRangeScan * LocalizationSlamToolbox::addScan( return range_scan; } - +/*****************************************************************************/ void LocalizationSlamToolbox::setInitialParameters(double position_search_distance, double position_search_maximum_distance, double position_search_fine_angle_offset, double position_search_coarse_angle_offset, double position_search_coarse_angle_resolution, double position_search_resolution, double position_search_smear_deviation,bool do_loop_closing_flag, - int scan_buffer_size){ - + int scan_buffer_size) +/*****************************************************************************/ +{ smapper_->getMapper()->setParamLoopSearchSpaceDimension(position_search_distance); smapper_->getMapper()->setParamLoopSearchMaximumDistance(position_search_maximum_distance); smapper_->getMapper()->setParamFineSearchAngleOffset(position_search_fine_angle_offset); @@ -247,23 +245,35 @@ void LocalizationSlamToolbox::setInitialParameters(double position_search_distan } } -void LocalizationSlamToolbox::triggerTableSave(){ - if (processor_type_ == PROCESS){ - RCLCPP_INFO(get_logger(), "Triggering table save function"); - boost::mutex::scoped_lock lock(smapper_mutex_); - smapper_->getMapper()->StartTableStorage(true); - - } -} +/*****************************************************************************/ void LocalizationSlamToolbox::set_parameters_callback( const std::shared_ptr request, std::shared_ptr response ) +/*****************************************************************************/ { if(request->table_save_mode){ - triggerTableSave(); - RCLCPP_INFO(get_logger(), "Triggering table save service"); + if (processor_type_ == PROCESS){ + if (request->target_uid.empty()){ + RCLCPP_ERROR(get_logger(), "Triggering target save without target name or type"); + response->success = false; + response->message = "There is no target name! Please provide a target name."; + return; + } + else{ + RCLCPP_INFO(get_logger(), "Triggering target save function"); + boost::mutex::scoped_lock lock(smapper_mutex_); + smapper_->getMapper()->StartTableStorage(true,request->target_uid); + } + } + } + + if(request->target_list_off){ + if (processor_type_ == PROCESS){ + boost::mutex::scoped_lock lock(smapper_mutex_); + smapper_->getMapper()->tableVectorUpdated_= true; + } } if (request->default_mode) { From 826427934ef41c6e3d78d30766c397b5a4d0914a Mon Sep 17 00:00:00 2001 From: Renbago Date: Wed, 4 Dec 2024 09:12:10 +0300 Subject: [PATCH 75/79] added target_list_off and target_uid srv --- srvs/SetParametersService.srv | 3 +++ 1 file changed, 3 insertions(+) diff --git a/srvs/SetParametersService.srv b/srvs/SetParametersService.srv index 18ccfca28..824eaaad7 100644 --- a/srvs/SetParametersService.srv +++ b/srvs/SetParametersService.srv @@ -2,6 +2,9 @@ string[] param_names float64[] param_values bool default_mode bool table_save_mode +bool target_list_off +string target_uid + --- bool success From 0f0e5d312a26dbfd34e77bdd5964661f5515e468 Mon Sep 17 00:00:00 2001 From: baha Date: Wed, 4 Dec 2024 09:40:49 +0300 Subject: [PATCH 76/79] removed unused function --- lib/karto_sdk/include/karto_sdk/Mapper.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h index aa49ce47a..d9dc8e024 100644 --- a/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1658,12 +1658,6 @@ class KARTO_EXPORT MapperSensorManager */ void AddScan(LocalizedRangeScan * pScan); - /** - * Add table which is founded position of scan - * @param pScan - */ - void AddTable(LocalizedRangeScan * pScan); - /** * Adds scan to running scans of device that recorded scan * @param pScan From 4e81f0bc4174daaf5db0feb22b5e373c9673df3e Mon Sep 17 00:00:00 2001 From: baha Date: Wed, 4 Dec 2024 09:54:55 +0300 Subject: [PATCH 77/79] removed ceres 2.2.0 version settings --- solvers/ceres_solver.cpp | 118 ++++++++++++--------------------------- solvers/ceres_solver.hpp | 8 +-- solvers/ceres_utils.h | 32 +++++------ 3 files changed, 52 insertions(+), 106 deletions(-) diff --git a/solvers/ceres_solver.cpp b/solvers/ceres_solver.cpp index 573f1aa27..9325097e1 100644 --- a/solvers/ceres_solver.cpp +++ b/solvers/ceres_solver.cpp @@ -25,70 +25,37 @@ CeresSolver::CeresSolver() void CeresSolver::Configure(rclcpp::Node::SharedPtr node) /*****************************************************************************/ { - logger_ = node->get_logger(); + node_ = node; std::string solver_type, preconditioner_type, dogleg_type, trust_strategy, loss_fn, mode; - if (!node->has_parameter("ceres_linear_solver")) { - node->declare_parameter( - "ceres_linear_solver", - rclcpp::ParameterValue(std::string("SPARSE_NORMAL_CHOLESKY"))); - } - solver_type = node->get_parameter("ceres_linear_solver").as_string(); - - if (!node->has_parameter("ceres_preconditioner")) { - node->declare_parameter( - "ceres_preconditioner", - rclcpp::ParameterValue(std::string("JACOBI"))); - } - preconditioner_type = node->get_parameter("ceres_preconditioner").as_string(); - - if (!node->has_parameter("ceres_dogleg_type")) { - node->declare_parameter( - "ceres_dogleg_type", - rclcpp::ParameterValue(std::string("TRADITIONAL_DOGLEG"))); - } - dogleg_type = node->get_parameter("ceres_dogleg_type").as_string(); - - if (!node->has_parameter("ceres_trust_strategy")) { - node->declare_parameter( - "ceres_trust_strategy", - rclcpp::ParameterValue(std::string("LM"))); - } - trust_strategy = node->get_parameter("ceres_trust_strategy").as_string(); - - if (!node->has_parameter("ceres_loss_function")) { - node->declare_parameter( - "ceres_loss_function", - rclcpp::ParameterValue(std::string("None"))); - } - loss_fn = node->get_parameter("ceres_loss_function").as_string(); - - if (!node->has_parameter("mode")) { - node->declare_parameter( - "mode", - rclcpp::ParameterValue(std::string("mapping"))); - } - mode = node->get_parameter("mode").as_string(); - + solver_type = node->declare_parameter("ceres_linear_solver", + std::string("SPARSE_NORMAL_CHOLESKY")); + preconditioner_type = node->declare_parameter("ceres_preconditioner", + std::string("JACOBI")); + dogleg_type = node->declare_parameter("ceres_dogleg_type", + std::string("TRADITIONAL_DOGLEG")); + trust_strategy = node->declare_parameter("ceres_trust_strategy", + std::string("LM")); + loss_fn = node->declare_parameter("ceres_loss_function", + std::string("None")); + mode = node->declare_parameter("mode", std::string("mapping")); debug_logging_ = node->get_parameter("debug_logging").as_bool(); corrections_.clear(); first_node_ = nodes_->end(); // formulate problem - angle_manifold_ = AngleManifold::Create(); + angle_local_parameterization_ = AngleLocalParameterization::Create(); // choose loss function default squared loss (NULL) loss_function_ = NULL; if (loss_fn == "HuberLoss") { - RCLCPP_INFO( - node->get_logger(), + RCLCPP_INFO(node_->get_logger(), "CeresSolver: Using HuberLoss loss function."); loss_function_ = new ceres::HuberLoss(0.7); } else if (loss_fn == "CauchyLoss") { - RCLCPP_INFO( - node->get_logger(), + RCLCPP_INFO(node_->get_logger(), "CeresSolver: Using CauchyLoss loss function."); loss_function_ = new ceres::CauchyLoss(0.7); } @@ -96,18 +63,15 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node) // choose linear solver default CHOL options_.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; if (solver_type == "SPARSE_SCHUR") { - RCLCPP_INFO( - node->get_logger(), + RCLCPP_INFO(node_->get_logger(), "CeresSolver: Using SPARSE_SCHUR solver."); options_.linear_solver_type = ceres::SPARSE_SCHUR; } else if (solver_type == "ITERATIVE_SCHUR") { - RCLCPP_INFO( - node->get_logger(), + RCLCPP_INFO(node_->get_logger(), "CeresSolver: Using ITERATIVE_SCHUR solver."); options_.linear_solver_type = ceres::ITERATIVE_SCHUR; } else if (solver_type == "CGNR") { - RCLCPP_INFO( - node->get_logger(), + RCLCPP_INFO(node_->get_logger(), "CeresSolver: Using CGNR solver."); options_.linear_solver_type = ceres::CGNR; } @@ -115,13 +79,11 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node) // choose preconditioner default Jacobi options_.preconditioner_type = ceres::JACOBI; if (preconditioner_type == "IDENTITY") { - RCLCPP_INFO( - node->get_logger(), + RCLCPP_INFO(node_->get_logger(), "CeresSolver: Using IDENTITY preconditioner."); options_.preconditioner_type = ceres::IDENTITY; } else if (preconditioner_type == "SCHUR_JACOBI") { - RCLCPP_INFO( - node->get_logger(), + RCLCPP_INFO(node_->get_logger(), "CeresSolver: Using SCHUR_JACOBI preconditioner."); options_.preconditioner_type = ceres::SCHUR_JACOBI; } @@ -137,8 +99,7 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node) // choose trust region strategy default LM options_.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; if (trust_strategy == "DOGLEG") { - RCLCPP_INFO( - node->get_logger(), + RCLCPP_INFO(node_->get_logger(), "CeresSolver: Using DOGLEG trust region strategy."); options_.trust_region_strategy_type = ceres::DOGLEG; } @@ -147,8 +108,7 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node) if (options_.trust_region_strategy_type == ceres::DOGLEG) { options_.dogleg_type = ceres::TRADITIONAL_DOGLEG; if (dogleg_type == "SUBSPACE_DOGLEG") { - RCLCPP_INFO( - node->get_logger(), + RCLCPP_INFO(node_->get_logger(), "CeresSolver: Using SUBSPACE_DOGLEG dogleg type."); options_.dogleg_type = ceres::SUBSPACE_DOGLEG; } @@ -186,7 +146,7 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node) } // we do not want the problem definition to own these objects, otherwise they get - // deleted along with the problem + // deleted along with the problem options_problem_.loss_function_ownership = ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; problem_ = new ceres::Problem(options_problem_); @@ -217,8 +177,7 @@ void CeresSolver::Compute() boost::mutex::scoped_lock lock(nodes_mutex_); if (nodes_->size() == 0) { - RCLCPP_WARN( - logger_, + RCLCPP_WARN(node_->get_logger(), "CeresSolver: Ceres was called when there are no nodes." " This shouldn't happen."); return; @@ -229,8 +188,7 @@ void CeresSolver::Compute() problem_->HasParameterBlock(&first_node_->second(0)) && problem_->HasParameterBlock(&first_node_->second(1)) && problem_->HasParameterBlock(&first_node_->second(2))) { - RCLCPP_DEBUG( - logger_, + RCLCPP_DEBUG(node_->get_logger(), "CeresSolver: Setting first node as a constant pose:" "%0.2f, %0.2f, %0.2f.", first_node_->second(0), first_node_->second(1), first_node_->second(2)); @@ -247,8 +205,7 @@ void CeresSolver::Compute() } if (!summary.IsSolutionUsable()) { - RCLCPP_WARN( - logger_, "CeresSolver: " + RCLCPP_WARN(node_->get_logger(), "CeresSolver: " "Ceres could not find a usable solution to optimize."); return; } @@ -310,7 +267,7 @@ void CeresSolver::Reset() problem_ = new ceres::Problem(options_problem_); first_node_ = nodes_->end(); - angle_manifold_ = AngleManifold::Create(); + angle_local_parameterization_ = AngleLocalParameterization::Create(); } /*****************************************************************************/ @@ -354,8 +311,7 @@ void CeresSolver::AddConstraint(karto::Edge * pEdge) if (node1it == nodes_->end() || node2it == nodes_->end() || node1it == node2it) { - RCLCPP_WARN( - logger_, + RCLCPP_WARN(node_->get_logger(), "CeresSolver: Failed to add constraint, could not find nodes."); return; } @@ -382,10 +338,10 @@ void CeresSolver::AddConstraint(karto::Edge * pEdge) cost_function, loss_function_, &node1it->second(0), &node1it->second(1), &node1it->second(2), &node2it->second(0), &node2it->second(1), &node2it->second(2)); - problem_->SetManifold(&node1it->second(2), - angle_manifold_); - problem_->SetManifold(&node2it->second(2), - angle_manifold_); + problem_->SetParameterization(&node1it->second(2), + angle_local_parameterization_); + problem_->SetParameterization(&node2it->second(2), + angle_local_parameterization_); blocks_->insert(std::pair( GetHash(node1, node2), block)); @@ -406,20 +362,19 @@ void CeresSolver::RemoveNode(kt_int32s id) problem_->RemoveParameterBlock(&nodeit->second(1)); problem_->RemoveParameterBlock(&nodeit->second(2)); RCLCPP_DEBUG( - logger_, + node_->get_logger(), "RemoveNode: Removed node id %d" ,nodeit->first); } else { RCLCPP_DEBUG( - logger_, + node_->get_logger(), "RemoveNode: Missing parameter blocks for " "node id %d", nodeit->first); } nodes_->erase(nodeit); } else { - RCLCPP_ERROR( - logger_, "RemoveNode: Failed to find node matching id %i", + RCLCPP_ERROR(node_->get_logger(), "RemoveNode: Failed to find node matching id %i", (int)id); } } @@ -440,8 +395,7 @@ void CeresSolver::RemoveConstraint(kt_int32s sourceId, kt_int32s targetId) problem_->RemoveResidualBlock(it_b->second); blocks_->erase(it_b); } else { - RCLCPP_ERROR( - logger_, + RCLCPP_ERROR(node_->get_logger(), "RemoveConstraint: Failed to find residual block for %i %i", (int)sourceId, (int)targetId); } diff --git a/solvers/ceres_solver.hpp b/solvers/ceres_solver.hpp index 75273ace5..c6f436e9f 100644 --- a/solvers/ceres_solver.hpp +++ b/solvers/ceres_solver.hpp @@ -7,17 +7,16 @@ #define SOLVERS__CERES_SOLVER_HPP_ #include +#include #include #include #include #include #include -#include #include "karto_sdk/Mapper.h" #include "solvers/ceres_utils.h" #include "rclcpp/rclcpp.hpp" -// #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_srvs/srv/empty.hpp" #include "slam_toolbox/toolbox_types.hpp" @@ -39,7 +38,6 @@ class CeresSolver : public karto::ScanSolver virtual void Compute(); // Solve virtual void Clear(); // Resets the corrections virtual void Reset(); // Resets the solver plugin clean - virtual void Configure(rclcpp::Node::SharedPtr node); // Adds a node to the solver @@ -67,7 +65,7 @@ class CeresSolver : public karto::ScanSolver ceres::Problem::Options options_problem_; ceres::LossFunction * loss_function_; ceres::Problem * problem_; - ceres::Manifold * angle_manifold_; + ceres::LocalParameterization * angle_local_parameterization_; bool was_constant_set_, debug_logging_; // graph @@ -77,7 +75,7 @@ class CeresSolver : public karto::ScanSolver boost::mutex nodes_mutex_; // ros - rclcpp::Logger logger_{rclcpp::get_logger("CeresSolver")}; + rclcpp::Node::SharedPtr node_; }; } // namespace solver_plugins diff --git a/solvers/ceres_utils.h b/solvers/ceres_utils.h index fab5a8f6d..1b2b2d927 100644 --- a/solvers/ceres_utils.h +++ b/solvers/ceres_utils.h @@ -7,7 +7,7 @@ #define SOLVERS__CERES_UTILS_H_ #include -#include +#include #include #include @@ -35,27 +35,21 @@ inline T NormalizeAngle(const T & angle_radians) /*****************************************************************************/ /*****************************************************************************/ -// Defines a manifold for updating the angle to be constrained in [-pi to pi). -class AngleManifold { - public: - template - bool Plus(const T* x_radians, - const T* delta_radians, - T* x_plus_delta_radians) const { - *x_plus_delta_radians = NormalizeAngle(*x_radians + *delta_radians); - return true; - } - - template - bool Minus(const T* y_radians, - const T* x_radians, - T* y_minus_x_radians) const { - *y_minus_x_radians = NormalizeAngle(*y_radians - *x_radians); +class AngleLocalParameterization +{ +public: + template + bool operator()( + const T * theta_radians, const T * delta_theta_radians, + T * theta_radians_plus_delta) const + { + *theta_radians_plus_delta = NormalizeAngle(*theta_radians + *delta_theta_radians); return true; } - static ceres::Manifold* Create() { - return new ceres::AutoDiffManifold; + static ceres::LocalParameterization * Create() + { + return new ceres::AutoDiffLocalParameterization; } }; From 1359556f8753c51019c6926b175c97eba948b9e6 Mon Sep 17 00:00:00 2001 From: baha Date: Thu, 5 Dec 2024 16:06:25 +0300 Subject: [PATCH 78/79] removed markerarray --- include/slam_toolbox/visualization_utils.hpp | 68 -------------------- src/loop_closure_assistant.cpp | 17 ----- 2 files changed, 85 deletions(-) diff --git a/include/slam_toolbox/visualization_utils.hpp b/include/slam_toolbox/visualization_utils.hpp index 4f96c8fea..bc053a4e2 100644 --- a/include/slam_toolbox/visualization_utils.hpp +++ b/include/slam_toolbox/visualization_utils.hpp @@ -140,74 +140,6 @@ inline void toNavMap( } } -inline visualization_msgs::msg::Marker toRectangleMarker( - const std::string & frame, - const std::string & ns, - const double & x, - const double & y, - const double & yaw, - const double & width, - const double & height, - const double & scale, - const std::array & color, // RGBA array - rclcpp::Node::SharedPtr node) -{ - visualization_msgs::msg::Marker marker; - - // Set header and metadata - marker.header.frame_id = frame; - marker.header.stamp = node->now(); - marker.ns = ns; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - - marker.scale.x = scale; // Line width - marker.color.r = color[0]; - marker.color.g = color[1]; - marker.color.b = color[2]; - marker.color.a = color[3]; - - // Calculate rectangle corners - double half_width = width / 2.0; - double half_height = height / 2.0; - - double cos_yaw = cos(yaw); - double sin_yaw = sin(yaw); - - std::vector corners(5); - - // Top-right corner - corners[0].x = x + (cos_yaw * half_width - sin_yaw * half_height); - corners[0].y = y + (sin_yaw * half_width + cos_yaw * half_height); - corners[0].z = 0.0; - - // Top-left corner - corners[1].x = x + (cos_yaw * -half_width - sin_yaw * half_height); - corners[1].y = y + (sin_yaw * -half_width + cos_yaw * half_height); - corners[1].z = 0.0; - - // Bottom-left corner - corners[2].x = x + (cos_yaw * -half_width - sin_yaw * -half_height); - corners[2].y = y + (sin_yaw * -half_width + cos_yaw * -half_height); - corners[2].z = 0.0; - - // Bottom-right corner - corners[3].x = x + (cos_yaw * half_width - sin_yaw * -half_height); - corners[3].y = y + (sin_yaw * half_width + cos_yaw * -half_height); - corners[3].z = 0.0; - - // Close the rectangle - corners[4] = corners[0]; - - // Add corners to the marker - for (const auto & corner : corners) { - marker.points.push_back(corner); - } - - return marker; -} - } // namespace vis_utils #endif // SLAM_TOOLBOX__VISUALIZATION_UTILS_HPP_ diff --git a/src/loop_closure_assistant.cpp b/src/loop_closure_assistant.cpp index 71f30861d..e73a20f96 100644 --- a/src/loop_closure_assistant.cpp +++ b/src/loop_closure_assistant.cpp @@ -259,23 +259,6 @@ void LoopClosureAssistant::publishGraph() marray.markers.push_back(edges_marker); marray.markers.push_back(localization_edges_marker); - // Here is for - const auto& localPoseVector = mapper_->poseVector; - for (const auto& pose : localPoseVector) { - visualization_msgs::msg::Marker rectangle_marker = - vis_utils::toRectangleMarker( - "map", // Frame ID - "pose_visualization", // Namespace - pose.x, pose.y, pose.yaw, // Pose: x, y, yaw - 1.0, 0.1, // Width and height of the rectangle - 0.1, // Line width (scale) - {1.0, 0.0, 0.0, 1.0}, // Color: red (RGBA) - node_); // Node pointer - - rectangle_marker.id = pose.scanId + 5000; // Ensure unique IDs for these markers - marray.markers.push_back(rectangle_marker); - } - // if disabled, clears out old markers interactive_server_->applyChanges(); marker_publisher_->publish(marray); From 39c81a0cb0dc845edb5c240b85fc680bdc5a0db7 Mon Sep 17 00:00:00 2001 From: Oguzhan Kose Date: Thu, 5 Dec 2024 18:02:34 +0300 Subject: [PATCH 79/79] remove unnecessary log --- lib/karto_sdk/src/Mapper.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp index 78c7928ec..bddc66a0e 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -3167,8 +3167,7 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covari m_pMapperSensorManager->SetLastScan(pScan); AddScanToLocalizationBuffer(pScan, scan_vertex); - std::cout <<"\n\nAddedScan is "<< pScan->GetOdometricPose().GetX() << " " << pScan->GetOdometricPose().GetY() << " " << pScan->GetOdometricPose().GetHeading() << std::endl; - // AddScanPoseToTableData(pScan); + // std::cout <<"\n\nAddedScan is "<< pScan->GetOdometricPose().GetX() << " " << pScan->GetOdometricPose().GetY() << " " << pScan->GetOdometricPose().GetHeading() << std::endl; return true; }