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..4311bcdf8 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 */ @@ -910,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 */ @@ -923,8 +934,10 @@ 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); } }; // MapperGraph @@ -2018,6 +2031,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..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_pOptimizerCached = true; loopClosed = true; } } @@ -1538,6 +1538,11 @@ namespace karto return loopClosed; } + kt_bool MapperGraph::IsLoopClosed() + { + return m_pOptimizerCached; + } + 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..722532158 100644 --- a/slam_toolbox/src/slam_toolbox_common.cpp +++ b/slam_toolbox/src/slam_toolbox_common.cpp @@ -586,6 +586,12 @@ bool SlamToolbox::serializePoseGraphCallback( slam_toolbox_msgs::SerializePoseGraph::Response &resp) /*****************************************************************************/ { + if (!smapper_->getMapper()->IsLoopClosed()) + { + ROS_WARN("Could not serialize the map because no loop closure has been detected yet!"); + return false; + } + std::string filename = req.filename; // if we're inside the snap, we need to write to commonly accessible space