Skip to content
Snippets Groups Projects
Commit 3a50ad61 authored by adrianrobolab's avatar adrianrobolab
Browse files

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

parent 1eb2ec1b
Branches
No related tags found
No related merge requests found
...@@ -68,6 +68,11 @@ public: ...@@ -68,6 +68,11 @@ public:
*/ */
PointCloud &getPointCloud(); PointCloud &getPointCloud();
/*
* Saves the current global map to a file in vtk format
*/
void outputMapToFile(string fileName);
private: private:
//Structure for storing each local map. //Structure for storing each local map.
typedef struct { typedef struct {
...@@ -114,6 +119,7 @@ private: ...@@ -114,6 +119,7 @@ private:
bool hasPositionChanged(Pose oldPose, Pose newPose); bool hasPositionChanged(Pose oldPose, Pose newPose);
inline void rotPoseToVector(Pose &pose, tf::Vector3 &vec); inline void rotPoseToVector(Pose &pose, tf::Vector3 &vec);
}; };
......
...@@ -11,6 +11,7 @@ ...@@ -11,6 +11,7 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <tf/transform_listener.h> #include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <std_msgs/String.h>
#include <crosbot/data.hpp> #include <crosbot/data.hpp>
#include <crosbot/utils.hpp> #include <crosbot/utils.hpp>
...@@ -42,7 +43,7 @@ private: ...@@ -42,7 +43,7 @@ private:
/* /*
* ROS config params * ROS config params
*/ */
string local_map_sub, optimise_map_sub; string local_map_sub, optimise_map_sub, save_map_sub;
string point_cloud_pub; string point_cloud_pub;
/* /*
...@@ -50,6 +51,7 @@ private: ...@@ -50,6 +51,7 @@ private:
*/ */
ros::Subscriber localMapSub; ros::Subscriber localMapSub;
ros::Subscriber optimiseMapSub; ros::Subscriber optimiseMapSub;
ros::Subscriber saveMapSub;
ros::Publisher pointCloudPub; ros::Publisher pointCloudPub;
...@@ -65,6 +67,11 @@ private: ...@@ -65,6 +67,11 @@ private:
*/ */
void callbackOptimiseMap(const crosbot_graphslam::LocalMapMsgListConstPtr& localMapMsgList); 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 * Publishes the entire point cloud
*/ */
......
...@@ -187,6 +187,8 @@ void GraphSlamDisplay::addMap(LocalMapInfoPtr localMapPoints) { ...@@ -187,6 +187,8 @@ void GraphSlamDisplay::addMap(LocalMapInfoPtr localMapPoints) {
//TODO: implement saving the entire mesh to file if selected by a parameter //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 //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); //pcl::io::saveVTKFile ("/home/adrianrobolab/groovy_workspace/crosbot/src/crosbot_3d_graphslam_display/mesh.vtk", triangles);
//outputMapToFile("/home/adrianrobolab/mesh.vtk");
} else if (CreateMesh) { } else if (CreateMesh) {
cout << "ERROR: Creating an empty mesh" << endl; cout << "ERROR: Creating an empty mesh" << endl;
LocalMap newMap; LocalMap newMap;
...@@ -475,6 +477,48 @@ PointCloud &GraphSlamDisplay::getPointCloud() { ...@@ -475,6 +477,48 @@ PointCloud &GraphSlamDisplay::getPointCloud() {
return points; 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() { void GraphSlamDisplay::run() {
viewer = new pcl::visualization::PCLVisualizer ("3D Viewer"); viewer = new pcl::visualization::PCLVisualizer ("3D Viewer");
viewer->setBackgroundColor(1.0, 1.0, 1.0); viewer->setBackgroundColor(1.0, 1.0, 1.0);
......
...@@ -22,12 +22,14 @@ void GraphSlamDisplayNode::initialise(ros::NodeHandle& nh) { ...@@ -22,12 +22,14 @@ void GraphSlamDisplayNode::initialise(ros::NodeHandle& nh) {
paramNH.param<std::string>("local_map_sub", local_map_sub, "localMapPoints"); paramNH.param<std::string>("local_map_sub", local_map_sub, "localMapPoints");
//Subsribe to new optimised map positions given by 3D slam //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>("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"); paramNH.param<std::string>("point_cloud_pub", point_cloud_pub, "pointCloudPub");
graph_slam_display.initialise(nh); graph_slam_display.initialise(nh);
localMapSub = nh.subscribe(local_map_sub, 20, &GraphSlamDisplayNode::callbackLocalMap, this); localMapSub = nh.subscribe(local_map_sub, 20, &GraphSlamDisplayNode::callbackLocalMap, this);
optimiseMapSub = nh.subscribe(optimise_map_sub, 20, &GraphSlamDisplayNode::callbackOptimiseMap, 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) { if (graph_slam_display.PublishPointCloud) {
pointCloudPub = nh.advertise<sensor_msgs::PointCloud>(point_cloud_pub, 3); pointCloudPub = nh.advertise<sensor_msgs::PointCloud>(point_cloud_pub, 3);
...@@ -54,6 +56,10 @@ void GraphSlamDisplayNode::callbackOptimiseMap(const crosbot_graphslam::LocalMap ...@@ -54,6 +56,10 @@ void GraphSlamDisplayNode::callbackOptimiseMap(const crosbot_graphslam::LocalMap
graph_slam_display.correctMap(newPos); graph_slam_display.correctMap(newPos);
} }
void GraphSlamDisplayNode::callbackSaveMap(const std_msgs::String& filename) {
graph_slam_display.outputMapToFile(filename.data);
}
void GraphSlamDisplayNode::publishPointCloud() { void GraphSlamDisplayNode::publishPointCloud() {
if (graph_slam_display.PublishPointCloud) { if (graph_slam_display.PublishPointCloud) {
PointCloud &pc = graph_slam_display.getPointCloud(); PointCloud &pc = graph_slam_display.getPointCloud();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment