From 2dcc1727d26542acfa055ff68746acfde197d19f Mon Sep 17 00:00:00 2001 From: abyl Date: Tue, 10 Mar 2020 16:08:53 +0100 Subject: [PATCH 1/2] Adding LoopClosure checker to serialize a posegraph correctly --- .../lib/karto_sdk/include/karto_sdk/Mapper.h | 19 +++++++++++++++++++ slam_toolbox/lib/karto_sdk/src/Mapper.cpp | 7 ++++++- slam_toolbox/src/slam_toolbox_common.cpp | 5 +++++ 3 files changed, 30 insertions(+), 1 deletion(-) diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h index 643333654..366ebb400 100644 --- a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -758,6 +758,12 @@ namespace karto */ kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName); + /** + * Returns the state of a loop closure (True or False) + */ + + kt_bool IsLoopClosed(); + /** * Optimizes scan poses */ @@ -927,6 +933,11 @@ namespace karto ar & BOOST_SERIALIZATION_NVP(m_pTraversal); } + /** + * Member bool variable to keep track of the state of a loop closure. + */ + kt_bool m_pLoopClosed; + }; // MapperGraph //////////////////////////////////////////////////////////////////////////////////////// @@ -2018,6 +2029,14 @@ namespace karto return m_pGraph->TryCloseLoop(pScan, rSensorName); } + /** + * Returns the state of a loop closure (True or False) + */ + inline kt_bool IsLoopClosed() + { + return m_pGraph->IsLoopClosed(); + } + inline void CorrectPoses() { m_pGraph->CorrectPoses(); diff --git a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp index abadb2766..045634f1f 100644 --- a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp +++ b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp @@ -1527,7 +1527,7 @@ namespace karto CorrectPoses(); m_pMapper->FireEndLoopClosure("Loop closed!"); - + m_pLoopClosed = true; loopClosed = true; } } @@ -1538,6 +1538,11 @@ namespace karto return loopClosed; } + kt_bool MapperGraph::IsLoopClosed() + { + return m_pLoopClosed; + } + LocalizedRangeScan* MapperGraph::GetClosestScanToPose(const LocalizedRangeScanVector& rScans, const Pose2& rPose) const { diff --git a/slam_toolbox/src/slam_toolbox_common.cpp b/slam_toolbox/src/slam_toolbox_common.cpp index 3c0a2d605..98cb5241e 100644 --- a/slam_toolbox/src/slam_toolbox_common.cpp +++ b/slam_toolbox/src/slam_toolbox_common.cpp @@ -586,6 +586,11 @@ bool SlamToolbox::serializePoseGraphCallback( slam_toolbox_msgs::SerializePoseGraph::Response &resp) /*****************************************************************************/ { + if (!smapper_->getMapper()->IsLoopClosed()) + { + return false; + } + std::string filename = req.filename; // if we're inside the snap, we need to write to commonly accessible space From e616205a13baee28484e12b7d542bf677a2478ba Mon Sep 17 00:00:00 2001 From: abyl Date: Thu, 19 Mar 2020 22:54:28 +0100 Subject: [PATCH 2/2] Added ROS_WARN and changed m_pLoopClosed to m_pOptimizerCached --- .../lib/karto_sdk/include/karto_sdk/Mapper.h | 14 ++++++++------ slam_toolbox/lib/karto_sdk/src/Mapper.cpp | 4 ++-- slam_toolbox/src/slam_toolbox_common.cpp | 1 + 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h index 366ebb400..4311bcdf8 100644 --- a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -916,6 +916,11 @@ namespace karto */ GraphTraversal* m_pTraversal; + /** + * Member bool variable to keep track of the state of a loop closure. + */ + kt_bool m_pOptimizerCached; + /** * Serialization: class MapperGraph */ @@ -929,15 +934,12 @@ namespace karto ar & BOOST_SERIALIZATION_NVP(m_pMapper); std::cout << "MapperGraph <- m_pLoopScanMatcher; "; ar & BOOST_SERIALIZATION_NVP(m_pLoopScanMatcher); - std::cout << "MapperGraph <- m_pTraversal\n"; + std::cout << "MapperGraph <- m_pTraversal; "; ar & BOOST_SERIALIZATION_NVP(m_pTraversal); + std::cout << "MapperGraph <- m_pOptimizerCached\n"; + ar & BOOST_SERIALIZATION_NVP(m_pOptimizerCached); } - /** - * Member bool variable to keep track of the state of a loop closure. - */ - kt_bool m_pLoopClosed; - }; // MapperGraph //////////////////////////////////////////////////////////////////////////////////////// diff --git a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp index 045634f1f..4a2b21f2f 100644 --- a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp +++ b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp @@ -1527,7 +1527,7 @@ namespace karto CorrectPoses(); m_pMapper->FireEndLoopClosure("Loop closed!"); - m_pLoopClosed = true; + m_pOptimizerCached = true; loopClosed = true; } } @@ -1540,7 +1540,7 @@ namespace karto kt_bool MapperGraph::IsLoopClosed() { - return m_pLoopClosed; + return m_pOptimizerCached; } LocalizedRangeScan* MapperGraph::GetClosestScanToPose(const LocalizedRangeScanVector& rScans, diff --git a/slam_toolbox/src/slam_toolbox_common.cpp b/slam_toolbox/src/slam_toolbox_common.cpp index 98cb5241e..722532158 100644 --- a/slam_toolbox/src/slam_toolbox_common.cpp +++ b/slam_toolbox/src/slam_toolbox_common.cpp @@ -588,6 +588,7 @@ bool SlamToolbox::serializePoseGraphCallback( { if (!smapper_->getMapper()->IsLoopClosed()) { + ROS_WARN("Could not serialize the map because no loop closure has been detected yet!"); return false; }