Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00040 #include <pluginlib/class_list_macros.h>
00041 #include <pcl/io/io.h>
00042 #include "pcl_ros/io/concatenate_fields.h"
00043
00044 #include <pcl_conversions/pcl_conversions.h>
00045
00047 void
00048 pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit ()
00049 {
00050 private_nh_ = getMTPrivateNodeHandle ();
00051
00052 if (!private_nh_.getParam ("input_messages", input_messages_))
00053 {
00054 NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!");
00055 return;
00056 }
00057 if (input_messages_ < 2)
00058 {
00059 NODELET_ERROR ("[onInit] Invalid 'input_messages' parameter given!");
00060 return;
00061 }
00062
00063 private_nh_.getParam ("max_queue_size", maximum_queue_size_);
00064 private_nh_.getParam ("maximum_seconds", maximum_seconds_);
00065 sub_input_ = private_nh_.subscribe ("input", maximum_queue_size_, &PointCloudConcatenateFieldsSynchronizer::input_callback, this);
00066 pub_output_ = private_nh_.advertise<sensor_msgs::PointCloud2> ("output", maximum_queue_size_);
00067 }
00068
00070 void
00071 pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointCloudConstPtr &cloud)
00072 {
00073 NODELET_DEBUG ("[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00074 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), private_nh_.resolveName ("input").c_str ());
00075
00076
00077 if (maximum_seconds_ > 0 && queue_.size () > 0)
00078 {
00079 while (fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ) > maximum_seconds_ && queue_.size () > 0)
00080 {
00081 NODELET_WARN ("[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_,
00082 (*queue_.begin ()).first.toSec (), fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ));
00083 queue_.erase (queue_.begin ());
00084 }
00085 }
00086
00087
00088 queue_[cloud->header.stamp].push_back (cloud);
00089 if ((int)queue_[cloud->header.stamp].size () >= input_messages_)
00090 {
00091
00092 std::vector<PointCloudConstPtr> &clouds = queue_[cloud->header.stamp];
00093 PointCloud cloud_out = *clouds[0];
00094
00095
00096 int data_size = cloud_out.data.size ();
00097 int nr_fields = cloud_out.fields.size ();
00098 int nr_points = cloud_out.width * cloud_out.height;
00099 for (size_t i = 1; i < clouds.size (); ++i)
00100 {
00101 assert (clouds[i]->data.size () / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step);
00102
00103 if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height)
00104 {
00105 NODELET_ERROR ("[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!",
00106 i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height);
00107 break;
00108 }
00109
00110 cloud_out.point_step += clouds[i]->point_step;
00111
00112 data_size += clouds[i]->data.size ();
00113
00114
00115 cloud_out.fields.resize (nr_fields + clouds[i]->fields.size ());
00116 int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize (cloud_out.fields[nr_fields - 1].datatype);
00117 for (size_t d = 0; d < clouds[i]->fields.size (); ++d)
00118 {
00119 cloud_out.fields[nr_fields + d] = clouds[i]->fields[d];
00120 cloud_out.fields[nr_fields + d].offset += delta_offset;
00121 }
00122 nr_fields += clouds[i]->fields.size ();
00123 }
00124
00125 cloud_out.row_step = cloud_out.point_step * cloud_out.width;
00126 cloud_out.data.resize (data_size);
00127
00128
00129 int point_offset = 0;
00130 for (int cp = 0; cp < nr_points; ++cp)
00131 {
00132 for (size_t i = 0; i < clouds.size (); ++i)
00133 {
00134
00135 memcpy (&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], clouds[i]->point_step);
00136 point_offset += clouds[i]->point_step;
00137 }
00138 }
00139 pub_output_.publish (boost::make_shared<const PointCloud> (cloud_out));
00140 queue_.erase (cloud->header.stamp);
00141 }
00142
00143
00144 if (maximum_queue_size_ > 0)
00145 {
00146 while ((int)queue_.size () > maximum_queue_size_)
00147 queue_.erase (queue_.begin ());
00148 }
00149
00150 }
00151
00152 typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer;
00153 PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer,nodelet::Nodelet);
00154