54 NODELET_ERROR (
"[onInit] Need a 'input_messages' parameter to be set before continuing!");
59 NODELET_ERROR (
"[onInit] Invalid 'input_messages' parameter given!");
81 sub_input_.shutdown ();
88 NODELET_DEBUG (
"[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
89 cloud->width * cloud->height,
pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (
"input").c_str ());
92 if (maximum_seconds_ > 0 && queue_.size () > 0)
94 while (fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ) > maximum_seconds_ && queue_.size () > 0)
96 NODELET_WARN (
"[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_,
97 (*queue_.begin ()).first.toSec (), fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ));
98 queue_.erase (queue_.begin ());
103 queue_[cloud->header.stamp].push_back (cloud);
104 if ((
int)queue_[cloud->header.stamp].size () >= input_messages_)
107 std::vector<PointCloudConstPtr> &clouds = queue_[cloud->header.stamp];
111 size_t data_size = cloud_out.data.size ();
112 size_t nr_fields = cloud_out.fields.size ();
113 size_t nr_points = cloud_out.width * cloud_out.height;
114 for (
size_t i = 1; i < clouds.size (); ++i)
116 assert (clouds[i]->data.size () / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step);
118 if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height)
120 NODELET_ERROR (
"[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!",
121 i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height);
125 cloud_out.point_step += clouds[i]->point_step;
127 data_size += clouds[i]->data.size ();
130 cloud_out.fields.resize (nr_fields + clouds[i]->fields.size ());
131 int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize (cloud_out.fields[nr_fields - 1].datatype);
132 for (
size_t d = 0;
d < clouds[i]->fields.size (); ++
d)
134 cloud_out.fields[nr_fields +
d] = clouds[i]->fields[
d];
135 cloud_out.fields[nr_fields +
d].offset += delta_offset;
137 nr_fields += clouds[i]->fields.size ();
140 cloud_out.row_step = cloud_out.point_step * cloud_out.width;
141 cloud_out.data.resize (data_size);
144 int point_offset = 0;
145 for (
size_t cp = 0; cp < nr_points; ++cp)
147 for (
size_t i = 0; i < clouds.size (); ++i)
150 memcpy (&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], clouds[i]->point_step);
151 point_offset += clouds[i]->point_step;
154 pub_output_.publish (boost::make_shared<const PointCloud> (cloud_out));
155 queue_.erase (cloud->header.stamp);
159 if (maximum_queue_size_ > 0)
161 while ((
int)queue_.size () > maximum_queue_size_)
162 queue_.erase (queue_.begin ());