point_cloud_conversion.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, 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$
35  *
36  */
37 
38 #ifndef SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
39 #define SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
40 
41 #include <sensor_msgs/PointCloud.h>
42 #include <sensor_msgs/PointCloud2.h>
44 
49 namespace sensor_msgs
50 {
52 
56 static inline int getPointCloud2FieldIndex (const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
57 {
58  // Get the index we need
59  for (size_t d = 0; d < cloud.fields.size (); ++d)
60  if (cloud.fields[d].name == field_name)
61  return (d);
62  return (-1);
63 }
64 
66 
70 static inline bool convertPointCloudToPointCloud2 (const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
71 {
72  output.header = input.header;
73  output.width = input.points.size ();
74  output.height = 1;
75  output.fields.resize (3 + input.channels.size ());
76  // Convert x/y/z to fields
77  output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z";
78  int offset = 0;
79  // All offsets are *4, as all field data types are float32
80  for (size_t d = 0; d < output.fields.size (); ++d, offset += 4)
81  {
82  output.fields[d].offset = offset;
83  output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
84  output.fields[d].count = 1;
85  }
86  output.point_step = offset;
87  output.row_step = output.point_step * output.width;
88  // Convert the remaining of the channels to fields
89  for (size_t d = 0; d < input.channels.size (); ++d)
90  output.fields[3 + d].name = input.channels[d].name;
91  output.data.resize (input.points.size () * output.point_step);
92  output.is_bigendian = false; // @todo ?
93  output.is_dense = false;
94 
95  // Copy the data points
96  for (size_t cp = 0; cp < input.points.size (); ++cp)
97  {
98  memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (float));
99  memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (float));
100  memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (float));
101  for (size_t d = 0; d < input.channels.size (); ++d)
102  {
103  if (input.channels[d].values.size() == input.points.size())
104  {
105  memcpy (&output.data[cp * output.point_step + output.fields[3 + d].offset], &input.channels[d].values[cp], sizeof (float));
106  }
107  }
108  }
109  return (true);
110 }
111 
113 
117 static inline bool convertPointCloud2ToPointCloud (const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud &output)
118 {
119 
120  output.header = input.header;
121  output.points.resize (input.width * input.height);
122  output.channels.resize (input.fields.size () - 3);
123  // Get the x/y/z field offsets
124  int x_idx = getPointCloud2FieldIndex (input, "x");
125  int y_idx = getPointCloud2FieldIndex (input, "y");
126  int z_idx = getPointCloud2FieldIndex (input, "z");
127  if (x_idx == -1 || y_idx == -1 || z_idx == -1)
128  {
129  std::cerr << "x/y/z coordinates not found! Cannot convert to sensor_msgs::PointCloud!" << std::endl;
130  return (false);
131  }
132  int x_offset = input.fields[x_idx].offset;
133  int y_offset = input.fields[y_idx].offset;
134  int z_offset = input.fields[z_idx].offset;
135  uint8_t x_datatype = input.fields[x_idx].datatype;
136  uint8_t y_datatype = input.fields[y_idx].datatype;
137  uint8_t z_datatype = input.fields[z_idx].datatype;
138 
139  // Convert the fields to channels
140  int cur_c = 0;
141  for (size_t d = 0; d < input.fields.size (); ++d)
142  {
143  if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
144  continue;
145  output.channels[cur_c].name = input.fields[d].name;
146  output.channels[cur_c].values.resize (output.points.size ());
147  cur_c++;
148  }
149 
150  // Copy the data points
151  for (size_t cp = 0; cp < output.points.size (); ++cp)
152  {
153  // Copy x/y/z
154  output.points[cp].x = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + x_offset], x_datatype);
155  output.points[cp].y = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + y_offset], y_datatype);
156  output.points[cp].z = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + z_offset], z_datatype);
157  // Copy the rest of the data
158  int cur_c = 0;
159  for (size_t d = 0; d < input.fields.size (); ++d)
160  {
161  if ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)input.fields[d].offset == z_offset)
162  continue;
163  output.channels[cur_c++].values[cp] = sensor_msgs::readPointCloud2BufferValue<float>(&input.data[cp * input.point_step + input.fields[d].offset], input.fields[d].datatype);
164  }
165  }
166  return (true);
167 }
168 }
169 #endif// SENSOR_MSGS_POINT_CLOUD_CONVERSION_H
d
static int getPointCloud2FieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
static bool convertPointCloud2ToPointCloud(const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud &output)
Convert a sensor_msgs::PointCloud2 message to a sensor_msgs::PointCloud message.
Tools for manipulating sensor_msgs.
static bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
Convert a sensor_msgs::PointCloud message to a sensor_msgs::PointCloud2 message.


sensor_msgs
Author(s):
autogenerated on Sat Aug 15 2020 03:19:47