00001 #include "pointcloud_transport_base/decimated_publisher.h" 00002 00003 namespace decimated_transport { 00004 00005 void DecimatedPublisher::publish(const sensor_msgs::PointCloud& pointcloud, 00006 const message_transport::SimplePublisherPlugin<sensor_msgs::PointCloud,sensor_msgs::PointCloud>::PublishFn& publish_fn) const 00007 { 00008 // Set up message and publish 00009 unsigned int n = pointcloud.points.size(); 00010 unsigned int m = pointcloud.channels.size(); 00011 00012 sensor_msgs::PointCloud decimated; 00013 decimated.header = pointcloud.header; 00014 decimated.points.resize(n/decimation); 00015 decimated.channels.resize(m); 00016 for (unsigned int i=0;i<m;i++) { 00017 decimated.channels[i].name = pointcloud.channels[i].name; 00018 decimated.channels[i].values.resize(n/decimation); 00019 } 00020 for (unsigned int i=0;i<n/decimation;i++) { 00021 decimated.points[i] = pointcloud.points[i*decimation]; 00022 for (unsigned int j=0;j<m;j++) { 00023 decimated.channels[j].values[i] = pointcloud.channels[j].values[i*decimation]; 00024 } 00025 } 00026 00027 publish_fn(decimated); 00028 } 00029 00030 } //namespace decimated_transport