concatenate_fields.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id: concatenate_fields.cpp 35052 2011-01-03 21:04:57Z rusu $
35  *
36  */
37 
41 #include <pcl/io/io.h>
43 
45 
47 void
49 {
51 
52  // ---[ Mandatory parameters
53  if (!pnh_->getParam ("input_messages", input_messages_))
54  {
55  NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!");
56  return;
57  }
58  if (input_messages_ < 2)
59  {
60  NODELET_ERROR ("[onInit] Invalid 'input_messages' parameter given!");
61  return;
62  }
63  // ---[ Optional parameters
64  pnh_->getParam ("max_queue_size", maximum_queue_size_);
65  pnh_->getParam ("maximum_seconds", maximum_seconds_);
66  pub_output_ = advertise<sensor_msgs::PointCloud2> (*pnh_, "output", maximum_queue_size_);
67 
69 }
70 
72 void
74 {
76 }
77 
79 void
81 {
83 }
84 
86 void
88 {
89  NODELET_DEBUG ("[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
90  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
91 
92  // Erase old data in the queue
93  if (maximum_seconds_ > 0 && queue_.size () > 0)
94  {
95  while (fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ) > maximum_seconds_ && queue_.size () > 0)
96  {
97  NODELET_WARN ("[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_,
98  (*queue_.begin ()).first.toSec (), fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ));
99  queue_.erase (queue_.begin ());
100  }
101  }
102 
103  // Push back new data
104  queue_[cloud->header.stamp].push_back (cloud);
105  if ((int)queue_[cloud->header.stamp].size () >= input_messages_)
106  {
107  // Concatenate together and publish
108  std::vector<PointCloudConstPtr> &clouds = queue_[cloud->header.stamp];
109  PointCloud cloud_out = *clouds[0];
110 
111  // Resize the output dataset
112  int data_size = cloud_out.data.size ();
113  int nr_fields = cloud_out.fields.size ();
114  int nr_points = cloud_out.width * cloud_out.height;
115  for (size_t i = 1; i < clouds.size (); ++i)
116  {
117  assert (clouds[i]->data.size () / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step);
118 
119  if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height)
120  {
121  NODELET_ERROR ("[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!",
122  i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height);
123  break;
124  }
125  // Point step must increase with the length of each new field
126  cloud_out.point_step += clouds[i]->point_step;
127  // Resize data to hold all clouds
128  data_size += clouds[i]->data.size ();
129 
130  // Concatenate fields
131  cloud_out.fields.resize (nr_fields + clouds[i]->fields.size ());
132  int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize (cloud_out.fields[nr_fields - 1].datatype);
133  for (size_t d = 0; d < clouds[i]->fields.size (); ++d)
134  {
135  cloud_out.fields[nr_fields + d] = clouds[i]->fields[d];
136  cloud_out.fields[nr_fields + d].offset += delta_offset;
137  }
138  nr_fields += clouds[i]->fields.size ();
139  }
140  // Recalculate row_step
141  cloud_out.row_step = cloud_out.point_step * cloud_out.width;
142  cloud_out.data.resize (data_size);
143 
144  // Iterate over each point and perform the appropriate memcpys
145  int point_offset = 0;
146  for (int cp = 0; cp < nr_points; ++cp)
147  {
148  for (size_t i = 0; i < clouds.size (); ++i)
149  {
150  // Copy each individual point
151  memcpy (&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], clouds[i]->point_step);
152  point_offset += clouds[i]->point_step;
153  }
154  }
155  pub_output_.publish (boost::make_shared<const PointCloud> (cloud_out));
156  queue_.erase (cloud->header.stamp);
157  }
158 
159  // Clean the queue to avoid overflowing
160  if (maximum_queue_size_ > 0)
161  {
162  while ((int)queue_.size () > maximum_queue_size_)
163  queue_.erase (queue_.begin ());
164  }
165 
166 }
167 
170 
d
PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateFieldsSynchronizer, nodelet::Nodelet)
ros::Subscriber sub_input_
The input PointCloud subscriber.
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
#define NODELET_ERROR(...)
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
int maximum_queue_size_
The maximum number of messages that we can store in the queue.
std::map< ros::Time, std::vector< PointCloudConstPtr > > queue_
A queue for messages.
double maximum_seconds_
The maximum number of seconds to wait until we drop the synchronization.
boost::shared_ptr< ros::NodeHandle > pnh_
int input_messages_
The number of input messages that we expect on the input topic.
PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set ...
ros::Publisher pub_output_
The output PointCloud publisher.
void input_callback(const PointCloudConstPtr &cloud)
#define NODELET_DEBUG(...)


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Mon Jun 10 2019 14:19:18