diff --git a/crosbot_3d_graphslam_display/include/crosbot_3d_graphslam_display/graphSlamDisplay.hpp b/crosbot_3d_graphslam_display/include/crosbot_3d_graphslam_display/graphSlamDisplay.hpp index c0930851cc29224c141feb6614e58da5e1613ead..9800a95116794d97ba7f0c16a6b53f050789745c 100644 --- a/crosbot_3d_graphslam_display/include/crosbot_3d_graphslam_display/graphSlamDisplay.hpp +++ b/crosbot_3d_graphslam_display/include/crosbot_3d_graphslam_display/graphSlamDisplay.hpp @@ -67,6 +67,11 @@ public: * Returns the point cloud of entire map */ PointCloud &getPointCloud(); + + /* + * Saves the current global map to a file in vtk format + */ + void outputMapToFile(string fileName); private: //Structure for storing each local map. @@ -114,6 +119,7 @@ private: bool hasPositionChanged(Pose oldPose, Pose newPose); inline void rotPoseToVector(Pose &pose, tf::Vector3 &vec); + }; diff --git a/crosbot_3d_graphslam_display/include/crosbot_3d_graphslam_display/graphSlamDisplayNode.hpp b/crosbot_3d_graphslam_display/include/crosbot_3d_graphslam_display/graphSlamDisplayNode.hpp index a2b3868038809b2469c1614ae7fb020ac77a52fa..c6bf15d089de0a1b7fd0b0056a8c5d1e90d86ab5 100644 --- a/crosbot_3d_graphslam_display/include/crosbot_3d_graphslam_display/graphSlamDisplayNode.hpp +++ b/crosbot_3d_graphslam_display/include/crosbot_3d_graphslam_display/graphSlamDisplayNode.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -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; @@ -64,6 +66,11 @@ private: * Callback for receiving information about local maps after they have been optimised */ 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 diff --git a/crosbot_3d_graphslam_display/src/graphSlamDisplay.cpp b/crosbot_3d_graphslam_display/src/graphSlamDisplay.cpp index 5078cdb4ca7fc53b1db47a5aad149f47f8f89773..df80606eef2e0cf8616fde639d7d083e6766427d 100644 --- a/crosbot_3d_graphslam_display/src/graphSlamDisplay.cpp +++ b/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); diff --git a/crosbot_3d_graphslam_display/src/graphSlamDisplayNode.cpp b/crosbot_3d_graphslam_display/src/graphSlamDisplayNode.cpp index 790d838906054ac832e9cf3377d542ab5f729872..eda2b877664fc437452889da5762f67f635218b1 100644 --- a/crosbot_3d_graphslam_display/src/graphSlamDisplayNode.cpp +++ b/crosbot_3d_graphslam_display/src/graphSlamDisplayNode.cpp @@ -22,12 +22,14 @@ void GraphSlamDisplayNode::initialise(ros::NodeHandle& nh) { paramNH.param("local_map_sub", local_map_sub, "localMapPoints"); //Subsribe to new optimised map positions given by 3D slam paramNH.param("optimise_map_sub", optimise_map_sub, "optimised3DLocalMaps"); + paramNH.param("save_map_sub", save_map_sub, "saveMap"); paramNH.param("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(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();