void writePCD(std::string path, PCLPointCloud::Ptr cloud) { cloud->width = 1; cloud->height = cloud->points.size(); static pcl::PCDWriter writer; if (not cloud->empty()) writer.writeASCII(path, *cloud); }
void keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*) { static pcl::PCDWriter writer; static std::ostringstream frame; if (event.keyUp ()) { if ((event.getKeyCode () == 's') || (event.getKeyCode () == 'S')) { boost::mutex::scoped_lock lock (cloud_mutex_); frame.str ("frame-"); frame << boost::posix_time::to_iso_string (boost::posix_time::microsec_clock::local_time ()) << ".pcd"; writer.writeBinaryCompressed (frame.str (), *cloud_); PCL_INFO ("Written cloud %s.\n", frame.str ().c_str ()); } } }
/* ---[ */ int main(int argc, char** argv) { fn = "transformed_" + std::string(argv[1])+".pcd"; ros::init(argc, argv, "pcmerger"); char *topic="/camera/rgb/points"; if(argc>2) topic=argv[2]; if (!reader.open (argv[1], topic)) { cout <<"Couldn't read bag file on topic" <<(topic); return (-1); } // skipNum = atoi(argv[2]); viewer.createViewPort(0.0, 0.0, 1.0, 1.0, viewportOrig); viewer.addCoordinateSystem (1); //viewer.createViewPort(0.5, 0.0, 1.0, 1.0, viewportPred); //for (int i = 1 ; i <= max_segment_num ; i++ ){ srv = new dynamic_reconfigure::Server < scene_processing::pcmergerConfig > (global_mutex); dynamic_reconfigure::Server < scene_processing::pcmergerConfig >::CallbackType f = boost::bind(&reconfig, _1, _2); srv->setCallback(f); conf.done = false; while (true) { viewer.spinOnce(); ros::spinOnce(); if (conf.done) { conf.done = false; srv->updateConfig(conf); //savePCDAndLabels (); break; } if (doUpdate) { doUpdate = false; srv->updateConfig(conf); } } pcl::PCDWriter writer; writer.write ( fn,*cloud_merged_ptr, true); transformFile.close(); // viewer.removePointCloud("pred"); return (0); }
//////////////////////////////////////////////////////////////////////////////// // cloud_cb (!) void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc) { bool found_transform = tf_.waitForTransform(pc->header.frame_id, to_frame_, pc->header.stamp, ros::Duration(10.0)); if (found_transform) { //ROS_ASSERT_MSG(found_transform, "Could not transform to camera frame"); tf::StampedTransform transform; tf_.lookupTransform(to_frame_,pc->header.frame_id, pc->header.stamp, transform); pcl_ros::transformPointCloud(to_frame_, transform, *pc, output_cloud_); ROS_DEBUG("[TransformPointcloudNode:] Point cloud published in frame %s", output_cloud_.header.frame_id.c_str()); pub_.publish (output_cloud_); if (save_point_cloud_) { pcl::PointCloud<pcl::PointXYZ> pcl_cloud; pcl::fromROSMsg(output_cloud_, pcl_cloud); pcd_writer_.write (point_cloud_name_ + ".pcd", pcl_cloud); ROS_INFO("Point cloud saved as %s.pcd", point_cloud_name_.c_str()); } } }
void reconfig(scene_processing::pcmergerConfig & config, uint32_t level) { conf = config; boost::recursive_mutex::scoped_lock lock(global_mutex); // pcl::PointCloud<PointT>::Ptr prev_cloud_ptr(new pcl::PointCloud<PointT > ()); // pcl::PointCloud<PointT>::Ptr new_cloud_ptr(new pcl::PointCloud<PointT > ()); if(conf.add_pc) { //conf.add_pc = false; doUpdate = true; *cloud_new_ptr = *cloud_mod_ptr; *cloud_merged_backup_ptr=*cloud_merged_ptr; if(Merged) { *cloud_merged_ptr += *cloud_new_ptr; Matrix4f globalTrans=computeTransformXYZYPR(config.x, config.y, config.z, config.yaw/180.0*PI, config.pitch/180.0*PI, config.roll/180.0*PI); writeMatrixToFile(globalTrans); } else *cloud_merged_ptr = *cloud_new_ptr; //*cloud_merged_ptr =*cloud_new_ptr; viewer.removePointCloud("new"); if(Merged) viewer.removePointCloud("merged"); Merged = true; pcl::toROSMsg(*cloud_merged_ptr, cloud_blobc_merged); color_handler_merged.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_merged)); viewer.addPointCloud(*cloud_merged_ptr, color_handler_merged, "merged", viewportOrig); ROS_INFO("displaying mergered pointcloud"); } if(conf.setIT) { std::string transformsFileName=fn+".transforms.txt"; transformFile.open(transformsFileName.data()); conf.setIT = false; doUpdate = true; InitialTransformConfig = conf; Matrix4f globalTrans=computeTransformXYZYPR(InitialTransformConfig.x, InitialTransformConfig.y, InitialTransformConfig.z, InitialTransformConfig.yaw/180.0*PI, InitialTransformConfig.pitch/180.0*PI, InitialTransformConfig.roll/180.0*PI); writeMatrixToFile(globalTrans); *cloud_new_ptr = *cloud_mod_ptr; ITpresent = true; } if(config.undo) { if(noMoreUndo) { conf.undo=false; doUpdate=true; return; } noMoreUndo=true; transformFile<<"endo"<<endl; *cloud_merged_ptr=*cloud_merged_backup_ptr; if(Merged) viewer.removePointCloud("merged"); Merged = true; pcl::toROSMsg(*cloud_merged_ptr, cloud_blobc_merged); color_handler_merged.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_merged)); viewer.addPointCloud(*cloud_merged_ptr, color_handler_merged, "merged", viewportOrig); ROS_INFO("undo:displaying mergered pointcloud"); conf.undo=false; doUpdate=true; if (pcl::io::loadPCDFile (std::string("tempAppend.pcd"), cloud_blobc_new) == -1) { ROS_ERROR ("Couldn't read file "); return ; } // ROS_INFO ("Loaded %d data points from %s with the following fields: %s", (int)(cloud_blob.width * cloud_blob.height), argv[1] ,pcl::getFieldsList (cloud_blob).c_str ()); // Convert to the templated message type pcl::fromROSMsg (cloud_blobc_new, *cloud_new_ptr); // pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT> (cloud)); if(ITpresent){ cout<<"inside IT"<<endl; transformXYZYPR<PointT>(*cloud_new_ptr, *cloud_mod_ptr, InitialTransformConfig.x, InitialTransformConfig.y, InitialTransformConfig.z, InitialTransformConfig.yaw/180.0*PI, InitialTransformConfig.pitch/180.0*PI, InitialTransformConfig.roll/180.0*PI); *cloud_new_ptr = *cloud_mod_ptr; conf.pitch=0; conf.yaw=0; conf.roll=0; } //ROS_INFO("PointCloud with %d data points and frame %s (%f) received.", (int) cloud_new_ptr->points.size(), cloud_new_ptr->header.frame_id.c_str(), cloud_new_ptr->header.stamp.toSec()); viewer.removePointCloud("new"); // pcl::toROSMsg<PointT>(*cloud_new_ptr, cloud_blobc_new); color_handler_new.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_new)); viewer.addPointCloud(*cloud_new_ptr, color_handler_new, "new", viewportOrig); ROS_INFO("undo:displaying new point cloud"); conf.x=0; conf.y=0; conf.z=0; conf.yaw=0; conf.pitch=0; conf.roll=0; } if(conf.skip_pc || conf.add_pc) { if (conf.skip_pc) { conf.x = -1; conf.y = -0.3; conf.z = 0.78; conf.yaw = 0; conf.pitch = 30; conf.roll = 0; } else { conf.x = -0.1; conf.y = -0.8; conf.z = 0.019; conf.yaw = 175; conf.pitch = 0; conf.roll = 0; } noMoreUndo=false; conf.skip_pc = false; conf.add_pc =false; doUpdate = true; int count = 0; cloud_blob_prev = cloud_blob_new; cloud_blob_new = reader.getNextCloud(); cout<<"header"<<cloud_blob_new->header<<endl; // cloud_blob_new-> ros::M_string::iterator iter; //for(iter=cloud_blob_new->__connection_header->begin ();iter!=cloud_blob_new->__connection_header->end ();iter++) // cout<<iter->first<<","<<iter->second<<endl; while(count < skipNum && cloud_blob_prev != cloud_blob_new) { cloud_blob_prev = cloud_blob_new; cloud_blob_new = reader.getNextCloud(); count ++; } if (cloud_blob_prev != cloud_blob_new) { pcl::fromROSMsg(*cloud_blob_new, *cloud_temp_ptr); cloud_temp_ptr->header; cout<<"numPoints="<<cloud_temp_ptr->size ()<<endl; appendCamIndexAndDistance (cloud_temp_ptr,cloud_new_ptr,globalFrameCount,VectorG(0,0,0)); globalFrameCount++; cloud_new_ptr->width=1; cloud_new_ptr->height=cloud_new_ptr->size(); writer.write (std::string("tempAppend.pcd"),*cloud_new_ptr,true); if (pcl::io::loadPCDFile (std::string("tempAppend.pcd"), cloud_blobc_new) == -1) { ROS_ERROR ("Couldn't read temp file "); return ; } // ROS_INFO ("Loaded %d data points from %s with the following fields: %s", (int)(cloud_blob.width * cloud_blob.height), argv[1] ,pcl::getFieldsList (cloud_blob).c_str ()); // Convert to the templated message type pcl::fromROSMsg (cloud_blobc_new, *cloud_new_ptr); // pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT> (cloud)); if(ITpresent){ cout<<"inside IT"<<endl; transformXYZYPR<PointT>(*cloud_new_ptr, *cloud_mod_ptr, InitialTransformConfig.x, InitialTransformConfig.y, InitialTransformConfig.z, InitialTransformConfig.yaw/180.0*PI, InitialTransformConfig.pitch/180.0*PI, InitialTransformConfig.roll/180.0*PI); *cloud_new_ptr = *cloud_mod_ptr; // conf.pitch=0; // conf.yaw=0; // conf.roll=0; } //ROS_INFO("PointCloud with %d data points and frame %s (%f) received.", (int) cloud_new_ptr->points.size(), cloud_new_ptr->header.frame_id.c_str(), cloud_new_ptr->header.stamp.toSec()); viewer.removePointCloud("new"); // pcl::toROSMsg<PointT>(*cloud_new_ptr, cloud_blobc_new); color_handler_new.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_new)); viewer.addPointCloud(*cloud_new_ptr, color_handler_new, "new", viewportOrig); ROS_INFO("displaying new point cloud"); /* if(Merged){ viewer.removePointCloud("merged"); pcl::toROSMsg(*cloud_merged_ptr, cloud_blobc_merged); color_handler_merged.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_merged)); viewer.addPointCloud(*cloud_merged_ptr, color_handler_merged, "merged", viewportOrig); }*/ }else { ROS_INFO("Finised reading all pointclouds! Select done to save."); } } if(conf.set_skip){ conf.set_skip = false; doUpdate = true; skipNum = (conf.skipNum); } if(conf.update) { conf.update = false; doUpdate = true; updateUI(); } }