pointcloud_to_stl_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, JSK Lab
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros_utils/pointcloud_to_stl.h"
00037 
00038 #include <geometry_msgs/PoseStamped.h>
00039 #include <geometry_msgs/PointStamped.h>
00040 
00041 #include <pcl/point_types.h>
00042 #include <pcl/io/pcd_io.h>
00043 #include <pcl/kdtree/kdtree_flann.h>
00044 #include <pcl/features/normal_3d.h>
00045 #include <pcl/surface/gp3.h>
00046 #include <pcl/io/vtk_lib_io.h>
00047 #include <pcl/TextureMesh.h>
00048 #include <pcl/surface/texture_mapping.h>
00049 #include <pcl/surface/mls.h>
00050 #include <pcl/io/obj_io.h>
00051 #include <pcl/filters/filter.h>
00052 
00053 #include "jsk_recognition_utils/pcl_conversion_util.h"
00054 
00055 namespace jsk_pcl_ros_utils
00056 {
00057   void PointCloudToSTL::cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& input)
00058   {
00059     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00060     pcl::fromROSMsg(*input, *cloud);
00061     exportSTL(cloud);
00062 
00063 
00064     visualization_msgs::Marker marker_msg;
00065     marker_msg.header = input->header;
00066     marker_msg.mesh_resource = std::string("package://jsk_pcl_ros_utils/temp.stl");//file_name_;//latest_output_path_;
00067     marker_msg.ns = "pcl_mesh_reconstrunction";
00068     marker_msg.id = 0;
00069     marker_msg.type = visualization_msgs::Marker::MESH_RESOURCE;
00070     marker_msg.action = visualization_msgs::Marker::ADD;
00071 
00072     marker_msg.pose.position.x = 1;
00073     marker_msg.pose.position.y = 1;
00074     marker_msg.pose.position.z = 1;
00075     marker_msg.pose.orientation.x = 0.0;
00076     marker_msg.pose.orientation.y = 0.0;
00077     marker_msg.pose.orientation.z = 0.0;
00078     marker_msg.pose.orientation.w = 1.0;
00079     marker_msg.scale.x = 1;
00080     marker_msg.scale.y = 1;
00081     marker_msg.scale.z = 1;
00082     pub_mesh_.publish(marker_msg);
00083 
00084   }
00085 
00086   void PointCloudToSTL::exportSTL(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &input_cloud){
00087 
00088     pcl::PolygonMesh triangles;
00089     ofm.setInputCloud (input_cloud);
00090     ofm.reconstruct (triangles);
00091 
00092     /*
00093 
00094 
00095     // Store the results in a temporary object
00096     boost::shared_ptr<std::vector<pcl::Vertices> > temp_verts (new std::vector<pcl::Vertices>);
00097     ofm.reconstruct (*temp_verts);
00098 
00099 
00100     ROS_INFO("Got Input PointClouds");
00101     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00102 
00103     std::vector<int> indices;
00104     pcl::removeNaNFromPointCloud(*input_cloud, *cloud, indices);
00105 
00106     pcl::search::KdTree<pcl::PointXYZRGB>::Ptr mls_tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
00107 
00108     // Output has the PointNormal type in order to store the normals calculated by MLS
00109     pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls_points(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00110 
00111     // Init object (second point type is for the normals, even if unused)
00112     pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;
00113  
00114     mls.setComputeNormals (true);
00115 
00116     // Set parameters
00117     mls.setInputCloud (cloud);
00118     mls.setPolynomialFit (true);
00119     mls.setSearchMethod (mls_tree);
00120     mls.setSearchRadius (0.03);
00121 
00122     // Reconstruct
00123     mls.process (*mls_points);
00124 
00125     mls_points->is_dense = false;
00126     pcl::removeNaNFromPointCloud(*mls_points, *mls_points, indices);
00127     pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
00128     tree2->setInputCloud (mls_points);//cloud_with_normals);
00129 
00130     // Initialize objects
00131     pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
00132     pcl::PolygonMesh triangles;
00133 
00134     // Set the maximum distance between connected points (maximum edge length)
00135     gp3.setSearchRadius (search_radius_);
00136 
00137     // Set typical values for the parameters
00138     gp3.setMu (mu_);
00139     gp3.setMaximumNearestNeighbors (maximum_nearest_neighbors_);
00140     gp3.setMaximumSurfaceAngle(maximum_surface_angle_); // 45 degrees
00141     gp3.setMinimumAngle(minimum_angle_); // 10 degrees
00142     gp3.setMaximumAngle(maximum_angle_); // 120 degrees
00143     gp3.setNormalConsistency(normal_consistency_);
00144 
00145     // Get result
00146     gp3.setInputCloud (mls_points);//cloud_with_normals);
00147     gp3.setSearchMethod (tree2);
00148     gp3.reconstruct (triangles);
00149 
00150 */
00151 
00152     ros::Time now_time = ros::Time::now();
00153     std::stringstream ss;
00154     if (file_name_.length())
00155       ss << file_name_.c_str();
00156     else
00157       ss << "/tmp/" << now_time.toNSec() << "_pointcloud.stl";
00158     
00159     ROS_INFO("Writing... %s", ss.str().c_str());
00160     pcl::io::savePolygonFileSTL(ss.str(),triangles);
00161     latest_output_path_ = ss.str();
00162 
00163     /*
00164     std::vector<std::string> tex_files;
00165     tex_files.push_back("text4.jpg");
00166 
00167     // initialize texture mesh
00168     pcl::TextureMesh tex_mesh;
00169     tex_mesh.cloud = triangles.cloud;
00170 
00171     // add the 1st mesh
00172     tex_mesh.tex_polygons.push_back(triangles.polygons);
00173 
00174     // update mesh and texture mesh
00175     //gp3.updateMesh(cloud_with_normals, triangles, tex_mesh);
00176     // set texture for added cloud
00177     tex_files.push_back("tex8.jpg");
00178     // save updated mesh
00179 
00180     pcl::TextureMapping<pcl::PointXYZ> tm;
00181 
00182     tm.setF(0.01);
00183     tm.setVectorField(1, 0, 0);
00184 
00185     pcl::TexMaterial tex_material;
00186 
00187     tex_material.tex_Ka.r = 0.2f;
00188     tex_material.tex_Ka.g = 0.2f;
00189     tex_material.tex_Ka.b = 0.2f;
00190 
00191     tex_material.tex_Kd.r = 0.8f;
00192     tex_material.tex_Kd.g = 0.8f;
00193     tex_material.tex_Kd.b = 0.8f;
00194 
00195     tex_material.tex_Ks.r = 1.0f;
00196     tex_material.tex_Ks.g = 1.0f;
00197     tex_material.tex_Ks.b = 1.0f;
00198     tex_material.tex_d = 1.0f;
00199     tex_material.tex_Ns = 0.0f;
00200     tex_material.tex_illum = 2;
00201 
00202     tm.setTextureMaterials(tex_material);
00203     tm.setTextureFiles(tex_files);
00204     tm.mapTexture2Mesh(tex_mesh);
00205 
00206     pcl::io::saveOBJFile ("/tmp/test.obj", tex_mesh);
00207     */
00208   }
00209 
00210   bool PointCloudToSTL::createSTL(jsk_recognition_msgs::SetPointCloud2::Request &req,
00211                                   jsk_recognition_msgs::SetPointCloud2::Response &res)
00212   {
00213     if(req.name.length())
00214       file_name_ = req.name;
00215     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00216     pcl::fromROSMsg(req.cloud, *cloud);
00217     exportSTL(cloud);
00218     res.output = latest_output_path_;
00219   }
00220 
00221   void PointCloudToSTL::onInit(void)
00222   {
00223     PCLNodelet::onInit();
00224     pnh_->param("filename", file_name_ , std::string(""));
00225     pnh_->param("search_radius", search_radius_ , 0.05);
00226     pnh_->param("mu", mu_, 3.5);
00227     pnh_->param("maximum_nearest_neighbors", maximum_nearest_neighbors_, 100);
00228     pnh_->param("maximum_surface_angle", maximum_surface_angle_, 0.7853981633974483);
00229     pnh_->param("minimum_angle", minimum_angle_, 0.17453292519943295);
00230     pnh_->param("maximum_angle", maximum_angle_, 2.0943951023931953);
00231     pnh_->param("normal_consistency", normal_consistency_, false);
00232 
00233     pnh_->param("triangle_pixel_size", triangle_pixel_size_, 1.0);
00234     pnh_->param("max_edge_length", max_edge_length_, 4.5);
00235     pnh_->param("store_shadow_faces", store_shadow_faces_, true);
00236 
00237     sub_input_ = pnh_->subscribe("input", 1, &PointCloudToSTL::cloudCallback, this);
00238     create_stl_srv_
00239       = pnh_->advertiseService("create_stl", &PointCloudToSTL::createSTL, this);
00240 
00241     pub_mesh_ = pnh_->advertise<visualization_msgs::Marker>("pc_stl_mesh", 1);
00242 
00243     ofm.setTrianglePixelSize (triangle_pixel_size_);
00244     ofm.setTriangulationType (pcl::OrganizedFastMesh<pcl::PointXYZRGB>::TRIANGLE_ADAPTIVE_CUT);
00245     ofm.setMaxEdgeLength(max_edge_length_);
00246     ofm.storeShadowedFaces(store_shadow_faces_);
00247   }
00248 }
00249 
00250 #include <pluginlib/class_list_macros.h>
00251 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PointCloudToSTL, nodelet::Nodelet);


jsk_pcl_ros_utils
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:05