Updated to ROS-kinetic and made lvr_plane_classifier online runnable

README.md

@@ -0,0 +1,23 @@
+# Semantic Object Maps
+Fork of the Lucia 2016 Winter School Semantic Object Maps Tutorial.
+
+## Online Changes
+This repository contains an altered LVR Plain Classifier that is able to get its mesh data from ROS messages and is therefore online runnable.
+These Messages can be generated by providing a ROS service on /kinfu/mesh_srv which needs to provide a TriangleMeshStamped and a constant GET which routes the request correctly.
+The TriangleMeshStamped must be filled at least with the faces and vertices array.
+
+The LVR KFusion Node provides this service.
+
+## Compilation
+To compile this package you have to have ROS-kinetic, the ROS package mesh_msgs and LVR compiled and installed.
+## Execution
+To launch the pipeline you have to:
+1. Start the furniture classifier node:
+```
+cd "YOUR_WORKSPACE"/src/semantic_object_maps/semantic_furniture_classifier/semantic_furniture_classifier_project/
+./build/install/semantic_furniture_classifier_project/bin/semantic_furniture_classifier_project com.github.semantic_furniture_classifier.semantic_furniture_classifier_project.SemanticFurnitureClassifierNode
+```
+2. Start the service providing Node
+3. Start the lvr_plane_classifier:
+`rosrun lvr_plane_classifier lvr_plane_classifier_node`
+(This has to be executed in a directory that contains a valid config.xml!)

lvr_plane_classifier/CMakeLists.txt

@@ -10,7 +10,8 @@ find_package(catkin REQUIRED COMPONENTS
   semantic_object_maps_msgs
   std_msgs
   tf
-)
+  mesh_msgs
+  )
 
 ## System dependencies are found with CMake's conventions
 find_package(LVR REQUIRED)

lvr_plane_classifier/src/LVRClassifierNode.cpp

@@ -12,6 +12,12 @@
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
 #include <tf/LinearMath/Transform.h>
+#include <mesh_msgs/TriangleMesh.h>
+#include <mesh_msgs/TriggerMesh.h>
+#include <std_msgs/String.h>
+#include <lvr/io/DataStruct.hpp>
+#include <lvr/io/MeshBuffer.hpp>
+#include <lvr/io/Model.hpp>
 
 using namespace lvr;
 
@@ -89,30 +95,18 @@ void transformModel(ModelPtr& model, const tf::Transform& transform)
  *
  * @param	filename		The name of the loaded model
  ******************************************************************************/
-tf::Transform getTransfromFromCalibration(std::string filename)
+tf::Transform getTransfromFromCalibration()
 {
 	tf::Transform t;
 
-	// Construct name for calibration file
-	std::string calibrationFileName = "";
-	size_t dot = filename.find_last_of(".");
-	if (dot != std::string::npos)
-	{
-		calibrationFileName = filename.substr(0, dot) + ".calibration";
-	}
-
-	// Try to load calibration
-	ifstream in(calibrationFileName.c_str());
-	if(!in.good())
-	{
-		ROS_WARN ("Could not open calibration file %s", calibrationFileName.c_str());
-		return t;
-	}
 	float qx, qy, qz, qw, x, y, z;
-	in >> qx >> qy >> qz >> qw >> x >> y >> z;
-
-	ROS_INFO("Found calibration %s: %f %f %f %f %f %f %f", calibrationFileName.c_str(), qx, qy, qz, qw, x, y, z);
-
+	x = 0.186;
+	y = 0.8;
+	z = -0.64;
+	qx = 1.25;
+	qy = -0.4;
+	qz = 0.6;
+	qw = 1;
 	tf::Vector3 origin(x, y, z);
 	tf::Quaternion quat(qx, qy, qz, qw);
 	t = tf::Transform(quat, origin);
@@ -184,15 +178,11 @@ void generateMarkerArray(vector<visualization_msgs::Marker>& contour_markers, ve
  * @param	contour_markers	A pointer to model data
  * @param	marker			A mesh marker
  ******************************************************************************/
-void createMeshMarker(string filename, ModelPtr model, visualization_msgs::Marker& mesh_marker)
+void createMeshMarker(ModelPtr model, visualization_msgs::Marker& mesh_marker)
 {
 	// Construct name for calibration file
-	std::string stlFileName = "";
-	size_t dot = filename.find_last_of(".");
-	if (dot != std::string::npos)
-	{
-		stlFileName = "/tmp/" + filename.substr(0, dot) + ".stl";
-	}
+	std::string stlFileName = "meshFile";
+	stlFileName = "/tmp/meshFile1.stl";
 
 	string stlRessource = "file://" + stlFileName;
 
@@ -290,74 +280,109 @@ int main(int argc, char** argv)
 	// and one for the visualization markers
 	ros::init(argc, argv, "lvr_classifier_node");
 	ros::NodeHandle nh;
-
+	ros::Rate loop_rate(0.2);
 	ros::Publisher plane_publisher = nh.advertise<semantic_object_maps_msgs::PlanarPatchArray>("lvr_classified_planes", 1000);
 	ros::Publisher contour_publisher = nh.advertise<visualization_msgs::MarkerArray>("lvr_plane_contours", 1000);
 	ros::Publisher mesh_publisher = nh.advertise<visualization_msgs::Marker>("lvr_mesh", 1000);
+	ros::ServiceClient service = nh.serviceClient<mesh_msgs::TriggerMesh>("/kinfu/mesh_srv");
+	while(ros::ok())	
+	{
+		int vertexSize = 0;	
+		mesh_msgs::TriggerMesh srv;
+		srv.request.request = mesh_msgs::TriggerMesh::Request::GET;
+		MeshBufferPtr mBuffer(new MeshBuffer);
+		bool selvic = service.exists();
+		ROS_INFO("Checking if service exists... : %d",selvic);
+		if(service.call(srv))
+		{
+			uintArr faces(new uint[srv.response.mesh.mesh.triangles.size() * 3]);
+			floatArr vertices(new float[srv.response.mesh.mesh.vertices.size() * 3]);
+			int facePosition = 0;
+			int vertexPosition = 0;
+			int faceSize = srv.response.mesh.mesh.triangles.size();
+			vertexSize = srv.response.mesh.mesh.vertices.size();
+			for(int i = 0; i < faceSize; i++ )
+			{
+				faces[facePosition++] = (uint) srv.response.mesh.mesh.triangles[i].vertex_indices[0];
+				faces[facePosition++] = srv.response.mesh.mesh.triangles[i].vertex_indices[1];
+				faces[facePosition++] = srv.response.mesh.mesh.triangles[i].vertex_indices[2];
+			}
+			for(int i = 0; i < vertexSize; i++ )
+			{
+				vertices[vertexPosition++] = srv.response.mesh.mesh.vertices[i].x;
+				vertices[vertexPosition++] = srv.response.mesh.mesh.vertices[i].y;
+				vertices[vertexPosition++] = srv.response.mesh.mesh.vertices[i].z;
+			}
+
+			mBuffer->setFaceArray(faces,(size_t) faceSize);
+			mBuffer->setVertexArray(vertices, (size_t) vertexSize);
+		}else
+		{
+			ROS_ERROR("NO MESH RECEIVED!");
+			return 1;
+		}
 
-	// Load data from given file
-	string modelFileName(argv[1]);
-	ModelPtr model = ModelFactory::readModel(modelFileName);
-
-	// Search for a calibration for the given model
-	tf::Transform t = getTransfromFromCalibration(modelFileName);
-
-	// Transform the model according to the loaded transformation
-	transformModel(model, t.inverse());
-
-	// Generate half edge mesh representation of the loaded mesh
-	HalfEdgeMesh<cVertex, cNormal > mesh( model->m_mesh );
-
-	// Get a pointer to he planar regions and create a furniture classifier
-	vector<cRegion* >* regionPtr = mesh.getRegionsPtr();
-	FurnitureFeatureClassifier<cVertex , cNormal > classifier(regionPtr);
-	mesh.setClassifier(&classifier);
-
-	// Apply mesh optimization filters
-	mesh.cleanContours(CONTOUR_ITERATIONS);
-	mesh.setDepth(100);
-
-	mesh.optimizePlanes(
-			PLANE_ITERATIONS,
-			NORMAL_THRESHOLD,
-			MIN_PLANE_SIZE,
-			SMALL_REGION_THRESHOLD,
-			true);
-
-	mesh.fillHoles(FILL_HOLES);
-	mesh.optimizePlaneIntersections();
-	mesh.restorePlanes(MIN_PLANE_SIZE);
-
-	// Create a marker for each contour (we to generate them here since mesh.finalize
-	// will destroy the topology of the initial mesh when re-triangulating the
-	// contours
-	mesh.resetUsedFlags();
-	vector<visualization_msgs::Marker> contourMarkers;
-	generateMarkerArray(contourMarkers, regionPtr);
-
-	// Finalize and reduce mesh, includes classification
-	mesh.finalizeAndRetesselate(
-			false, // Textures not yet supported in this tool
-			LINE_FUSION_THRESHOLD);
-
-	// Create output model and save to file
-	ModelPtr out_model( new Model( mesh.meshBuffer() ) );
-	ModelFactory::saveModel( out_model, "optimized_mesh.ply");
-
-	// Generate final messages from classifier and pre-computed markers
-	semantic_object_maps_msgs::PlanarPatchArray patch_array;
-	visualization_msgs::MarkerArray markerArray;
-	generateMessages(classifier, contourMarkers, markerArray, patch_array);
-
-	// Create a mesh marker
-	visualization_msgs::Marker mesh_marker;
-	createMeshMarker(modelFileName, model, mesh_marker);
-
-	// Publish stuff
-	plane_publisher.publish(patch_array);
-	contour_publisher.publish(markerArray);
-	mesh_publisher.publish(mesh_marker);
-	ros::spin();
-
+		ModelPtr model(new Model(mBuffer));
+		// Search for a calibration for the given model
+		tf::Transform t = getTransfromFromCalibration();
+
+		// Transform the model according to the loaded transformation
+		transformModel(model, t.inverse());
+		// Generate half edge mesh representation of the loaded mesh
+		HalfEdgeMesh<cVertex, cNormal> mesh( model->m_mesh );
+		// Get a pointer to he planar regions and create a furniture classifier
+		vector<cRegion* >* regionPtr = mesh.getRegionsPtr();
+		FurnitureFeatureClassifier<cVertex , cNormal > classifier(regionPtr);
+		mesh.setClassifier(&classifier);
+
+		// Apply mesh optimization filters
+		mesh.cleanContours(CONTOUR_ITERATIONS);
+		mesh.setDepth(100);
+
+		mesh.optimizePlanes(
+				PLANE_ITERATIONS,
+				NORMAL_THRESHOLD,
+				MIN_PLANE_SIZE,
+				SMALL_REGION_THRESHOLD,
+				true);
+
+		mesh.fillHoles(FILL_HOLES);
+		mesh.optimizePlaneIntersections();
+		mesh.restorePlanes(MIN_PLANE_SIZE);
+
+		// Create a marker for each contour (we to generate them here since mesh.finalize
+		// will destroy the topology of the initial mesh when re-triangulating the
+		// contours
+		mesh.resetUsedFlags();
+		vector<visualization_msgs::Marker> contourMarkers;
+		generateMarkerArray(contourMarkers, regionPtr);
+
+		// Finalize and reduce mesh, includes classification
+		mesh.finalizeAndRetesselate(
+				false, // Textures not yet supported in this tool
+				LINE_FUSION_THRESHOLD);
+
+		// Create output model and save to file
+		ModelPtr out_model( new Model( mesh.meshBuffer() ) );
+		ModelFactory::saveModel( out_model, "optimized_mesh.ply");
+
+		// Generate final messages from classifier and pre-computed markers
+		semantic_object_maps_msgs::PlanarPatchArray patch_array;
+		visualization_msgs::MarkerArray markerArray;
+		generateMessages(classifier, contourMarkers, markerArray, patch_array);
+
+		// Create a mesh marker
+		visualization_msgs::Marker mesh_marker;
+		createMeshMarker(model, mesh_marker);
+
+		// Publish stuff
+		plane_publisher.publish(patch_array);
+		contour_publisher.publish(markerArray);
+		mesh_publisher.publish(mesh_marker);
+		ros::spin();
+		//loop_rate.sleep();
+		//Maybe-TODO: Broadcast idle
+	}
 	return 0;
 }
+