Adding LoopClosure checker to serialize a posegraph correctly

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<LocalizedRangeScan>* 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();

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
   {

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