decompressor.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 #include <boost/bind.hpp>
00010 #include <boost/thread.hpp>
00011 
00012 //PCL specific includes
00013 #include <pcl/compression/octree_pointcloud_compression.h>
00014 #include <pcl/io/pcd_io.h>
00015 #include <pcl/point_cloud.h>
00016 #include <pcl/point_types.h>
00017 #include <pcl/ros/conversions.h>
00018 
00019 #include <compressed_pointcloud_transport/CompressedPointCloud.h>
00020 
00021 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120
00022 
00023 typedef pcl::PointXYZRGB PointT;
00024 
00025 
00026 class Decompressor{
00027 
00028 protected:
00029   ros::NodeHandle nh_, pnh_;
00030   ros::Publisher pub_;
00031   boost::shared_ptr<ros::Subscriber> sub_;
00032 
00033   pcl::octree::PointCloudCompression<PointT> decoder_;
00034   ros::Timer subscriber_timer_;
00035 
00036 public:
00037   Decompressor()
00038     :
00039     nh_()
00040     , pnh_("~")
00041     // ,pcl_cloud(new pcl::PointCloud<PointT>)
00042   {
00043     // Create a ROS publisher for the output point cloud
00044     pub_ = nh_.advertise<sensor_msgs::PointCloud2>("output", 1);
00045 
00046     // Create a ROS subscriber for the input point cloud
00047     sub_.reset(new ros::Subscriber());
00048     *sub_ = nh_.subscribe("input", 2, &Decompressor::cloud_cb, this);
00049 
00050     subscriber_timer_ = nh_.createTimer(ros::Duration(1.0), boost::bind(&Decompressor::timerCB, this));
00051 
00052   }
00053 
00054 protected:
00055   //void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) {
00056   void cloud_cb (const compressed_pointcloud_transport::CompressedPointCloud::ConstPtr& input) {
00057 
00058     if(!pub_.getNumSubscribers())
00059     {
00060       ROS_INFO("Got a cloud!");
00061 
00062       // clouds
00063       pcl::PointCloud<PointT>::Ptr pcl_cloud(new pcl::PointCloud<PointT>);
00064       sensor_msgs::PointCloud2::Ptr ros_cloud(new sensor_msgs::PointCloud2);
00065 
00066       // Stringstream to retrieve compressed point cloud
00067       std::stringstream compressed_data(input->data);
00068 
00069       //Decompress the point cloud
00070       decoder_.decodePointCloud(compressed_data, pcl_cloud);
00071 
00072       //Convert back to sensor_msgs::PointCloud2
00073       pcl::toROSMsg(*pcl_cloud, *ros_cloud);
00074 
00075       // restore the cloud header
00076       ros_cloud->header = input->header;
00077 
00078       ROS_DEBUG_NAMED("decompressor", "Uncompressed cloud with frame [%s] and stamp [%f]", pcl_cloud->header.frame_id.c_str(), pcl_cloud->header.stamp.toSec());
00079 
00080       pub_.publish(ros_cloud);
00081       int output_size = sizeof(ros_cloud->data);
00082       int compressed_size = sizeof(input->data);
00083       ROS_DEBUG_NAMED("decompressor", "Published cloud, input size %d bytes, inflated size %d bytes.", compressed_size, output_size);
00084     }
00085     else
00086       ROS_DEBUG_NAMED("decompressor" ,"Received compressed cloud but there are no subscribers; not publishing.");
00087   }
00088 
00089   void timerCB()
00090   {
00091     if(pub_.getNumSubscribers())
00092     {
00093       if( !sub_ )
00094       {
00095         ROS_INFO("Making a new subscriber!");
00096         sub_.reset(new ros::Subscriber());
00097         *sub_ = nh_.subscribe("input", 2, &Decompressor::cloud_cb, this);
00098       }
00099     }
00100     else
00101     {
00102       if( sub_)
00103       {
00104         ROS_INFO("Deleting subscriber!");
00105         sub_.reset();
00106       }
00107     }
00108   }
00109 };
00110 
00111 int main(int argc, char** argv) {
00112   // Initialize ROS
00113   ros::init(argc, argv, "cloud_decompressor");
00114   Decompressor decomp;
00115 
00116   ros::Duration(1.0).sleep();
00117   ros::spin();
00118 }


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