compressor.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <ros/ros.h>
00003 #include <sensor_msgs/PointCloud2.h>
00004 #include <std_msgs/String.h>
00005 #include <sstream>
00006 #include <stdio.h>
00007 #include <stdlib.h>
00008 
00009 //PCL specific includes
00010 #include <pcl/compression/octree_pointcloud_compression.h>
00011 #include <pcl/point_cloud.h>
00012 #include <pcl/point_types.h>
00013 #include <pcl/ros/conversions.h>
00014 
00015 #include <compressed_pointcloud_transport/CompressedPointCloud.h>
00016 
00017 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120
00018 
00019 typedef pcl::PointXYZRGB PointT;
00020 
00021 ros::Publisher pub;
00022 pcl::octree::PointCloudCompression<PointT>* PointCloudEncoder;
00023 pcl::PointCloud<PointT>::Ptr pclCloud(new pcl::PointCloud<PointT>);
00024 
00025 // TODO: Could unsubscribe when no one is subscribed to the compressed output.
00026 // Right now, though, we are assuming that local-loopback is low enough load that we don't care.
00027 void cloud_cb (const sensor_msgs::PointCloud2::ConstPtr& rosCloud)
00028 {
00029   
00030   if(pub.getNumSubscribers())
00031   {
00032     // Stringstream to store compressed point cloud
00033     std::stringstream compressedData;
00034 
00035     // Must convert from sensor_msg::PointCloud2 to pcl::PointCloud<PointT> for the encoder
00036     pcl::fromROSMsg(*rosCloud, *pclCloud);
00037 
00038     ROS_DEBUG_NAMED("compressor" ,"Compressing cloud with frame [%s] and stamp [%f]", pclCloud->header.frame_id.c_str(), pclCloud->header.stamp.toSec());
00039 
00040     // Compress the pointcloud
00041     PointCloudEncoder->encodePointCloud (pclCloud, compressedData);
00042 
00043     // Pack into a compressed message
00044     compressed_pointcloud_transport::CompressedPointCloud output;
00045     output.header = rosCloud->header;
00046     output.data = compressedData.str();
00047     pub.publish(output);
00048 
00049     int original_size = sizeof(rosCloud->data);
00050     int compressed_size = sizeof(output);
00051     ROS_DEBUG_NAMED("compressor", "Published cloud, original size %d bytes, compressed size %d bytes, %.3f of original.",
00052              original_size, compressed_size, (float)compressed_size/(float)original_size);
00053   }
00054   else
00055     ROS_DEBUG_NAMED("compressor" ,"Received input cloud but there are no subscribers; not publishing.");
00056 }
00057 
00058 
00059 pcl::octree::compression_Profiles_e getCompressionProfile(ros::NodeHandle nh)
00060 {
00061   std::string compression_level;
00062   if(nh.getParam("compression_profile", compression_level))
00063   {
00064     if(compression_level == "LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR")        return pcl::octree::LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;
00065     else if(compression_level == "LOW_RES_ONLINE_COMPRESSION_WITH_COLOR")      return pcl::octree::LOW_RES_ONLINE_COMPRESSION_WITH_COLOR;
00066     else if(compression_level == "MED_RES_ONLINE_COMPRESSION_WITHOUT_COLOR")   return pcl::octree::MED_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;
00067     else if(compression_level == "MED_RES_ONLINE_COMPRESSION_WITH_COLOR")      return pcl::octree::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;
00068     else if(compression_level == "HIGH_RES_ONLINE_COMPRESSION_WITHOUT_COLOR")  return pcl::octree::HIGH_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;
00069     else if(compression_level == "HIGH_RES_ONLINE_COMPRESSION_WITH_COLOR")     return pcl::octree::HIGH_RES_ONLINE_COMPRESSION_WITH_COLOR;
00070     else if(compression_level == "LOW_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR")  return pcl::octree::LOW_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR;
00071     else if(compression_level == "LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR")     return pcl::octree::LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR;
00072     else if(compression_level == "MED_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR")  return pcl::octree::MED_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR;
00073     else if(compression_level == "MED_RES_OFFLINE_COMPRESSION_WITH_COLOR")     return pcl::octree::MED_RES_OFFLINE_COMPRESSION_WITH_COLOR;
00074     else if(compression_level == "HIGH_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR") return pcl::octree::HIGH_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR;
00075     else if(compression_level == "HIGH_RES_OFFLINE_COMPRESSION_WITH_COLOR")    return pcl::octree::HIGH_RES_OFFLINE_COMPRESSION_WITH_COLOR;
00076     else return pcl::octree::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;
00077   }
00078   else
00079   {
00080      return pcl::octree::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;
00081   }
00082 }
00083 
00084 int main(int argc, char** argv) {
00085   // Initialize ROS
00086   ros::init(argc, argv, "cloud_compressor");
00087   ros::NodeHandle nh, pnh("~");
00088   
00089   // Initialize encoder
00090   pcl::octree::compression_Profiles_e compressionProfile = getCompressionProfile(pnh);
00091   PointCloudEncoder = new pcl::octree::PointCloudCompression<PointT>(compressionProfile, false);
00092 
00093   //Create a ROS subscriber for the input point cloud
00094   ros::Subscriber sub = nh.subscribe("input", 1, cloud_cb);
00095 
00096   //Create a ROS publisher for the output point cloud
00097   pub = nh.advertise<compressed_pointcloud_transport::CompressedPointCloud>("output", 10);
00098 
00099   //Spin
00100   ros::spin();
00101 }


compressed_pointcloud_transport
Author(s):
autogenerated on Thu Jan 2 2014 11:37:47