point_cloud2.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <sensor_msgs/PointField.h>
11 #include <sensor_msgs/PointCloud2.h>
12 #include <ros/console.h>
13 
15 
16 #include <mrpt/version.h>
17 #include <mrpt/maps/CSimplePointsMap.h>
18 #include <mrpt/maps/CColouredPointsMap.h>
19 using namespace mrpt::maps;
20 
21 namespace mrpt_bridge
22 {
23 inline 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  {
33  *output = NULL;
34  coherence_error = true;
35  }
36  else
37  {
38  *output = &input_field;
39  }
40  }
41  return coherence_error;
42 }
43 
45  const sensor_msgs::PointField* field, const unsigned char* data,
46  float& output)
47 {
48  if (field != NULL)
49  {
50  if (field->datatype == sensor_msgs::PointField::FLOAT32)
51  output = *(reinterpret_cast<const float*>(&data[field->offset]));
52  else
53  output = (float)(*(
54  reinterpret_cast<const double*>(&data[field->offset])));
55  }
56  else
57  output = 0.0;
58 }
59 
65 {
66  // Copy point data
67  unsigned int num_points = msg.width * msg.height;
68  obj.clear();
69  obj.reserve(num_points);
70 
71  bool incompatible_clouds = false;
72  const sensor_msgs::PointField *x_field = NULL, *y_field = NULL,
73  *z_field = NULL;
74 
75  for (unsigned int i = 0; i < msg.fields.size() && !incompatible_clouds; i++)
76  {
77  incompatible_clouds |= check_field(msg.fields[i], "x", &x_field);
78  incompatible_clouds |= check_field(msg.fields[i], "y", &y_field);
79  incompatible_clouds |= check_field(msg.fields[i], "z", &z_field);
80  }
81 
82  if (incompatible_clouds ||
83  (x_field == NULL && y_field == NULL && z_field == NULL))
84  return false;
85 
86  // If not, memcpy each group of contiguous fields separately
87  for (unsigned int row = 0; row < msg.height; ++row)
88  {
89  const unsigned char* row_data = &msg.data[row * msg.row_step];
90  for (uint32_t col = 0; col < msg.width; ++col)
91  {
92  const unsigned char* msg_data = row_data + col * msg.point_step;
93 
94  float x, y, z;
95  get_float_from_field(x_field, msg_data, x);
96  get_float_from_field(y_field, msg_data, y);
97  get_float_from_field(z_field, msg_data, z);
98  obj.insertPoint(x, y, z);
99  }
100  }
101 
102  return true;
103 }
104 
113 bool copy(
114  const CSimplePointsMap& obj, const std_msgs::Header& msg_header,
116 {
117  MRPT_TODO("Implement pointcloud2 mrpt2ros");
118  throw ros::Exception("not implemented yet.");
119  return true;
120 }
121 
122 } // namespace mrpt_bridge
GLint GLint GLint GLint GLint GLint y
GLenum GLenum GLvoid * row
void get_float_from_field(const sensor_msgs::PointField *field, const unsigned char *data, float &output)
unsigned int uint32_t
MRPT_TODO("Modify ping to run on Windows + Test this")
virtual void reserve(size_t newLength) MRPT_OVERRIDE
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types...
GLint GLint GLint GLint GLint x
GLhandleARB obj
GLint GLenum GLsizei GLint GLsizei const GLvoid * data
bool copy(const sensor_msgs::PointCloud2 &msg, mrpt::maps::CSimplePointsMap &obj)
GLdouble GLdouble z
void insertPoint(float x, float y, float z=0)
bool check_field(const sensor_msgs::PointField &input_field, std::string check_name, const sensor_msgs::PointField **output)


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Fri Feb 28 2020 03:22:14