Added a topic to cause the current 3d map to be saved to a file

crosbot_3d_graphslam_display/include/crosbot_3d_graphslam_display/graphSlamDisplay.hpp

@@ -68,6 +68,11 @@ public:
     */
    PointCloud &getPointCloud();
    
+   /*
+    * Saves the current global map to a file in vtk format
+    */
+   void outputMapToFile(string fileName);
+
 private:
    //Structure for storing each local map.
    typedef struct {
@@ -114,6 +119,7 @@ private:
    bool hasPositionChanged(Pose oldPose, Pose newPose);
 
    inline void rotPoseToVector(Pose &pose, tf::Vector3 &vec);
+
 };
    
 

crosbot_3d_graphslam_display/include/crosbot_3d_graphslam_display/graphSlamDisplayNode.hpp

@@ -11,6 +11,7 @@
 #include <ros/ros.h>
 #include <tf/transform_listener.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/String.h>
 
 #include <crosbot/data.hpp>
 #include <crosbot/utils.hpp>
@@ -42,7 +43,7 @@ private:
    /*
     * ROS config params
     */
-   string local_map_sub, optimise_map_sub;
+   string local_map_sub, optimise_map_sub, save_map_sub;
    string point_cloud_pub;
 
    /*
@@ -50,6 +51,7 @@ private:
     */
    ros::Subscriber localMapSub;
    ros::Subscriber optimiseMapSub;
+   ros::Subscriber saveMapSub;
    ros::Publisher pointCloudPub;
 
    
@@ -65,6 +67,11 @@ private:
     */
    void callbackOptimiseMap(const crosbot_graphslam::LocalMapMsgListConstPtr& localMapMsgList);
    
+   /*
+    * Callback for saving the current global map to a file in vtk format
+    */
+   void callbackSaveMap(const std_msgs::String& filename);
+
    /*
     * Publishes the entire point cloud
     */

crosbot_3d_graphslam_display/src/graphSlamDisplay.cpp

@@ -187,6 +187,8 @@ void GraphSlamDisplay::addMap(LocalMapInfoPtr localMapPoints) {
       //TODO: implement saving the entire mesh to file if selected by a parameter
       //Can view files made by the following command by using pcd_viewer
       //pcl::io::saveVTKFile ("/home/adrianrobolab/groovy_workspace/crosbot/src/crosbot_3d_graphslam_display/mesh.vtk", triangles);
+      
+      //outputMapToFile("/home/adrianrobolab/mesh.vtk");
    } else if (CreateMesh) {
       cout << "ERROR: Creating an empty mesh" << endl;
       LocalMap newMap;
@@ -475,6 +477,48 @@ PointCloud &GraphSlamDisplay::getPointCloud() {
    return points;
 }
 
+void GraphSlamDisplay::outputMapToFile(string fileName) {
+   
+   if (maps.size() == 0) {
+      cout << "No map to save yet!" << endl;
+      return;
+   }
+
+   //cout << "before" << maps[0].mesh->cloud.height << " " << maps[0].mesh->cloud.width << endl;
+   pcl::PolygonMesh fullMesh = *(maps[0].mesh);
+   pcl::uint32_t pointStep = maps[0].mesh->cloud.point_step;
+
+   int numPoints = maps[0].mesh->cloud.width;
+   for (int i = 1; i < maps.size(); i++) {
+      pcl::uint32_t oldDataSize = fullMesh.cloud.data.size();
+      pcl::uint32_t cloudDataSize = maps[i].mesh->cloud.data.size();
+      fullMesh.cloud.data.resize(oldDataSize + cloudDataSize);
+      for (unsigned int j = 0; j < cloudDataSize; j++) {
+         fullMesh.cloud.data[j + oldDataSize] = maps[i].mesh->cloud.data[j];
+      }
+      fullMesh.cloud.width += maps[i].mesh->cloud.width;
+      
+      int oldPolygons = fullMesh.polygons.size();
+      int numNewPolygons = maps[i].mesh->polygons.size();
+      fullMesh.polygons.resize(oldPolygons + numNewPolygons);
+      for (unsigned int j = 0; j < numNewPolygons; j++) {
+         fullMesh.polygons[j + oldPolygons].vertices.resize(3);
+         fullMesh.polygons[j + oldPolygons].vertices[0] = maps[i].mesh->polygons[j].vertices[0] +
+            numPoints;
+         fullMesh.polygons[j + oldPolygons].vertices[1] = maps[i].mesh->polygons[j].vertices[1] +
+            numPoints;
+         fullMesh.polygons[j + oldPolygons].vertices[2] = maps[i].mesh->polygons[j].vertices[2] +
+            numPoints;
+      }
+      numPoints += maps[i].mesh->cloud.width;
+   }
+
+   fullMesh.cloud.row_step = fullMesh.cloud.data.size();
+   
+   cout << "Saving map!!" << endl;
+   pcl::io::saveVTKFile (fileName, fullMesh);
+}
+
 void GraphSlamDisplay::run() {
    viewer = new pcl::visualization::PCLVisualizer ("3D Viewer");
    viewer->setBackgroundColor(1.0, 1.0, 1.0);

crosbot_3d_graphslam_display/src/graphSlamDisplayNode.cpp

@@ -22,12 +22,14 @@ void GraphSlamDisplayNode::initialise(ros::NodeHandle& nh) {
    paramNH.param<std::string>("local_map_sub", local_map_sub, "localMapPoints");
    //Subsribe to new optimised map positions given by 3D slam
    paramNH.param<std::string>("optimise_map_sub", optimise_map_sub, "optimised3DLocalMaps");
+   paramNH.param<std::string>("save_map_sub", save_map_sub, "saveMap");
    paramNH.param<std::string>("point_cloud_pub", point_cloud_pub, "pointCloudPub");
 
    graph_slam_display.initialise(nh);
 
    localMapSub = nh.subscribe(local_map_sub, 20, &GraphSlamDisplayNode::callbackLocalMap, this);
    optimiseMapSub = nh.subscribe(optimise_map_sub, 20, &GraphSlamDisplayNode::callbackOptimiseMap, this);
+   saveMapSub = nh.subscribe(save_map_sub, 1, &GraphSlamDisplayNode::callbackSaveMap, this);
 
    if (graph_slam_display.PublishPointCloud) {
       pointCloudPub = nh.advertise<sensor_msgs::PointCloud>(point_cloud_pub, 3);
@@ -54,6 +56,10 @@ void GraphSlamDisplayNode::callbackOptimiseMap(const crosbot_graphslam::LocalMap
    graph_slam_display.correctMap(newPos);
 }
 
+void GraphSlamDisplayNode::callbackSaveMap(const std_msgs::String& filename) {
+   graph_slam_display.outputMapToFile(filename.data);
+}
+
 void GraphSlamDisplayNode::publishPointCloud() {
    if (graph_slam_display.PublishPointCloud) {
       PointCloud &pc = graph_slam_display.getPointCloud();