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();
}
}