MRPT  1.9.9
point_cloud2.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
13 #include <mrpt/ros1bridge/time.h>
14 #include <mrpt/version.h>
15 #include <ros/console.h>
16 #include <sensor_msgs/PointCloud2.h>
17 #include <sensor_msgs/PointField.h>
18 
19 using namespace mrpt::maps;
20 
21 namespace mrpt::ros1bridge
22 {
23 static bool check_field(
24  const sensor_msgs::PointField& input_field, std::string check_name,
25  const sensor_msgs::PointField** output)
26 {
27  bool coherence_error = false;
28  if (input_field.name == check_name)
29  {
30  if (input_field.datatype != sensor_msgs::PointField::FLOAT32 &&
31  input_field.datatype != sensor_msgs::PointField::FLOAT64 &&
32  input_field.datatype != sensor_msgs::PointField::UINT16)
33  {
34  *output = nullptr;
35  coherence_error = true;
36  }
37  else
38  {
39  *output = &input_field;
40  }
41  }
42  return coherence_error;
43 }
44 
46  const sensor_msgs::PointField* field, const unsigned char* data,
47  float& output)
48 {
49  if (field != nullptr)
50  {
51  if (field->datatype == sensor_msgs::PointField::FLOAT32)
52  output = *(reinterpret_cast<const float*>(&data[field->offset]));
53  else
54  output = (float)(*(
55  reinterpret_cast<const double*>(&data[field->offset])));
56  }
57  else
58  output = 0.0;
59 }
61  const sensor_msgs::PointField* field, const unsigned char* data,
62  uint16_t& output)
63 {
64  if (field != nullptr)
65  {
66  if (field->datatype == sensor_msgs::PointField::UINT16)
67  output = *(reinterpret_cast<const uint16_t*>(&data[field->offset]));
68  }
69  else
70  output = 0;
71 }
72 
73 std::set<std::string> extractFields(const sensor_msgs::PointCloud2& msg)
74 {
75  std::set<std::string> lst;
76  for (const auto& f : msg.fields) lst.insert(f.name);
77  return lst;
78 }
79 
80 /** Convert sensor_msgs/PointCloud2 -> mrpt::slam::CSimplePointsMap
81  *
82  * \return true on sucessful conversion, false on any error.
83  */
84 bool fromROS(const sensor_msgs::PointCloud2& msg, CSimplePointsMap& obj)
85 {
86  // Copy point data
87  unsigned int num_points = msg.width * msg.height;
88  obj.clear();
89  obj.reserve(num_points);
90 
91  bool incompatible = false;
92  const sensor_msgs::PointField *x_field = nullptr, *y_field = nullptr,
93  *z_field = nullptr;
94 
95  for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++)
96  {
97  incompatible |= check_field(msg.fields[i], "x", &x_field);
98  incompatible |= check_field(msg.fields[i], "y", &y_field);
99  incompatible |= check_field(msg.fields[i], "z", &z_field);
100  }
101 
102  if (incompatible || (!x_field || !y_field || !z_field)) return false;
103 
104  // If not, memcpy each group of contiguous fields separately
105  for (unsigned int row = 0; row < msg.height; ++row)
106  {
107  const unsigned char* row_data = &msg.data[row * msg.row_step];
108  for (uint32_t col = 0; col < msg.width; ++col)
109  {
110  const unsigned char* msg_data = row_data + col * msg.point_step;
111 
112  float x, y, z;
113  get_float_from_field(x_field, msg_data, x);
114  get_float_from_field(y_field, msg_data, y);
115  get_float_from_field(z_field, msg_data, z);
116  obj.insertPoint(x, y, z);
117  }
118  }
119 
120  return true;
121 }
122 
123 bool fromROS(const sensor_msgs::PointCloud2& msg, CPointsMapXYZI& obj)
124 {
125  // Copy point data
126  unsigned int num_points = msg.width * msg.height;
127  obj.clear();
128  obj.reserve(num_points);
129 
130  bool incompatible = false;
131  const sensor_msgs::PointField *x_field = nullptr, *y_field = nullptr,
132  *z_field = nullptr, *i_field = nullptr;
133 
134  for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++)
135  {
136  incompatible |= check_field(msg.fields[i], "x", &x_field);
137  incompatible |= check_field(msg.fields[i], "y", &y_field);
138  incompatible |= check_field(msg.fields[i], "z", &z_field);
139  incompatible |= check_field(msg.fields[i], "intensity", &i_field);
140  }
141 
142  if (incompatible || (!x_field || !y_field || !z_field || !i_field))
143  return false;
144 
145  // If not, memcpy each group of contiguous fields separately
146  for (unsigned int row = 0; row < msg.height; ++row)
147  {
148  const unsigned char* row_data = &msg.data[row * msg.row_step];
149  for (uint32_t col = 0; col < msg.width; ++col)
150  {
151  const unsigned char* msg_data = row_data + col * msg.point_step;
152 
153  float x, y, z, i;
154  get_float_from_field(x_field, msg_data, x);
155  get_float_from_field(y_field, msg_data, y);
156  get_float_from_field(z_field, msg_data, z);
157  get_float_from_field(i_field, msg_data, i);
158  obj.insertPoint(x, y, z);
159 
160  obj.setPointIntensity(obj.size() - 1, i / 255.0);
161  }
162  }
163  return true;
164 }
165 
166 /** Convert mrpt::slam::CSimplePointsMap -> sensor_msgs/PointCloud2
167  * The user must supply the "msg_header" field to be copied into the output
168  * message object, since that part does not appear in MRPT classes.
169  *
170  * Since CSimplePointsMap only contains (x,y,z) data,
171  * sensor_msgs::PointCloud2::channels will be empty.
172  * \return true on sucessful conversion, false on any error.
173  */
174 bool toROS(
175  const CSimplePointsMap& obj, const std_msgs::Header& msg_header,
176  sensor_msgs::PointCloud2& msg)
177 {
178  throw ros::Exception("not implemented yet.");
179 }
180 
181 /** Convert sensor_msgs/PointCloud2 -> mrpt::obs::CObservationRotatingScan */
182 bool fromROS(
183  const sensor_msgs::PointCloud2& msg,
185  const mrpt::poses::CPose3D& sensorPoseOnRobot,
186  unsigned int num_azimuth_divisions)
187 {
188  // Copy point data
189  obj.timestamp = mrpt::ros1bridge::fromROS(msg.header.stamp);
190 
191  bool incompatible = false;
192  const sensor_msgs::PointField *x_field = nullptr, *y_field = nullptr,
193  *z_field = nullptr, *i_field = nullptr,
194  *ring_field = nullptr;
195 
196  for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++)
197  {
198  incompatible |= check_field(msg.fields[i], "x", &x_field);
199  incompatible |= check_field(msg.fields[i], "y", &y_field);
200  incompatible |= check_field(msg.fields[i], "z", &z_field);
201  incompatible |= check_field(msg.fields[i], "ring", &ring_field);
202  check_field(msg.fields[i], "intensity", &i_field);
203  }
204 
205  if (incompatible || (!x_field || !y_field || !z_field || !ring_field))
206  return false;
207 
208  // 1st: go through the scan and find ring count:
209  uint16_t ring_min = 0, ring_max = 0;
210 
211  for (unsigned int row = 0; row < msg.height; ++row)
212  {
213  const unsigned char* row_data = &msg.data[row * msg.row_step];
214  for (uint32_t col = 0; col < msg.width; ++col)
215  {
216  const unsigned char* msg_data = row_data + col * msg.point_step;
217  uint16_t ring_id = 0;
218  get_uint16_from_field(ring_field, msg_data, ring_id);
219 
220  mrpt::keep_min(ring_min, ring_id);
221  mrpt::keep_max(ring_max, ring_id);
222  }
223  }
224  ASSERT_NOT_EQUAL_(ring_min, ring_max);
225 
226  obj.rowCount = ring_max - ring_min + 1;
227  obj.columnCount = num_azimuth_divisions;
228 
229  obj.rangeImage.resize(obj.rowCount, obj.columnCount);
230  obj.rangeImage.fill(0);
231 
232  // Default unit: 1cm
233  if (obj.rangeResolution == 0) obj.rangeResolution = 1e-2;
234 
235  if (i_field)
236  {
237  obj.intensityImage.resize(obj.rowCount, obj.columnCount);
238  obj.intensityImage.fill(0);
239  }
240  else
241  obj.intensityImage.resize(0, 0);
242 
243  // If not, memcpy each group of contiguous fields separately
244  for (unsigned int row = 0; row < msg.height; ++row)
245  {
246  const unsigned char* row_data = &msg.data[row * msg.row_step];
247  for (uint32_t col = 0; col < msg.width; ++col)
248  {
249  const unsigned char* msg_data = row_data + col * msg.point_step;
250 
251  float x, y, z;
252  uint16_t ring_id = 0;
253  get_float_from_field(x_field, msg_data, x);
254  get_float_from_field(y_field, msg_data, y);
255  get_float_from_field(z_field, msg_data, z);
256  get_uint16_from_field(ring_field, msg_data, ring_id);
257 
258  const mrpt::math::TPoint3D localPt =
259  sensorPoseOnRobot.inverseComposePoint(
261  const double azimuth = std::atan2(localPt.y, localPt.x);
262 
263  const auto az_idx = lround(
264  (num_azimuth_divisions - 1) * (azimuth + M_PI) / (2 * M_PI));
265  ASSERT_ABOVEEQ_(az_idx, 0);
266  ASSERT_BELOWEQ_(az_idx, num_azimuth_divisions - 1);
267 
268  // Store in matrix form:
269  obj.rangeImage(ring_id, az_idx) =
270  lround(localPt.norm() / obj.rangeResolution);
271 
272  if (i_field)
273  {
274  float intensity;
275  get_float_from_field(i_field, msg_data, intensity);
276  ASSERT_BELOWEQ_(intensity, 255.0f);
277  obj.intensityImage(ring_id, az_idx) = lround(intensity);
278  }
279  }
280  }
281  return true;
282 }
283 
284 } // namespace mrpt::ros1bridge
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt) const
Computes the 3D point L such as .
double norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:177
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
static void get_uint16_from_field(const sensor_msgs::PointField *field, const unsigned char *data, uint16_t &output)
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
std::set< std::string > extractFields(const sensor_msgs::PointCloud2 &msg)
Extract a list of fields found in the point cloud.
#define ASSERT_NOT_EQUAL_(__A, __B)
Definition: exceptions.h:143
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
static void get_float_from_field(const sensor_msgs::PointField *field, const unsigned char *data, float &output)
GLsizei const GLchar ** string
Definition: glext.h:4116
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
A map of 3D points with reflectance/intensity (float).
bool toROS(const mrpt::obs::CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Convert mrpt::obs::CObservationGPS -> sensor_msgs/NavSatFix The user must supply the "msg_header" fie...
Definition: gps.cpp:48
ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html MRPT message: https://github...
Definition: gps.h:28
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
GLenum GLenum GLvoid * row
Definition: glext.h:3580
bool fromROS(const sensor_msgs::NavSatFix &msg, mrpt::obs::CObservationGPS &obj)
Convert sensor_msgs/NavSatFix -> mrpt::obs::CObservationGPS.
Definition: gps.cpp:20
GLenum GLint GLint y
Definition: glext.h:3542
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:161
static bool check_field(const sensor_msgs::PointField &input_field, std::string check_name, const sensor_msgs::PointField **output)
A CObservation-derived class for raw range data from a 2D or 3D rotating scanner. ...
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3550



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 0cbd40372 Sun Nov 17 09:43:05 2019 +0100 at dom nov 17 09:45:09 CET 2019