14 #include <mrpt/version.h>
15 #include <ros/console.h>
16 #include <sensor_msgs/PointCloud2.h>
17 #include <sensor_msgs/PointField.h>
24 const sensor_msgs::PointField& input_field, std::string check_name,
25 const sensor_msgs::PointField** output)
27 bool coherence_error =
false;
28 if (input_field.name == check_name)
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)
35 coherence_error =
true;
39 *output = &input_field;
42 return coherence_error;
46 const sensor_msgs::PointField* field,
const unsigned char*
data,
51 if (field->datatype == sensor_msgs::PointField::FLOAT32)
52 output = *(
reinterpret_cast<const float*
>(&
data[field->offset]));
55 reinterpret_cast<const double*
>(&
data[field->offset])));
61 const sensor_msgs::PointField* field,
const unsigned char*
data,
66 if (field->datatype == sensor_msgs::PointField::UINT16)
67 output = *(
reinterpret_cast<const uint16_t*
>(&
data[field->offset]));
73 std::set<std::string>
extractFields(
const sensor_msgs::PointCloud2& msg)
75 std::set<std::string> lst;
76 for (
const auto& f : msg.fields) lst.insert(f.name);
87 unsigned int num_points = msg.width * msg.height;
91 bool incompatible =
false;
92 const sensor_msgs::PointField *x_field =
nullptr, *y_field =
nullptr,
95 for (
unsigned int i = 0; i < msg.fields.size() && !incompatible; i++)
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);
102 if (incompatible || (!x_field || !y_field || !z_field))
return false;
105 for (
unsigned int row = 0; row < msg.height; ++row)
107 const unsigned char* row_data = &msg.data[row * msg.row_step];
108 for (uint32_t col = 0; col < msg.width; ++col)
110 const unsigned char* msg_data = row_data + col * msg.point_step;
126 unsigned int num_points = msg.width * msg.height;
130 bool incompatible =
false;
131 const sensor_msgs::PointField *x_field =
nullptr, *y_field =
nullptr,
132 *z_field =
nullptr, *i_field =
nullptr;
134 for (
unsigned int i = 0; i < msg.fields.size() && !incompatible; i++)
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);
142 if (incompatible || (!x_field || !y_field || !z_field || !i_field))
146 for (
unsigned int row = 0; row < msg.height; ++row)
148 const unsigned char* row_data = &msg.data[row * msg.row_step];
149 for (uint32_t col = 0; col < msg.width; ++col)
151 const unsigned char* msg_data = row_data + col * msg.point_step;
176 sensor_msgs::PointCloud2& msg)
178 throw ros::Exception(
"not implemented yet.");
183 const sensor_msgs::PointCloud2& msg,
186 unsigned int num_azimuth_divisions)
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;
196 for (
unsigned int i = 0; i < msg.fields.size() && !incompatible; i++)
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);
205 if (incompatible || (!x_field || !y_field || !z_field || !ring_field))
209 uint16_t ring_min = 0, ring_max = 0;
211 for (
unsigned int row = 0; row < msg.height; ++row)
213 const unsigned char* row_data = &msg.data[row * msg.row_step];
214 for (uint32_t col = 0; col < msg.width; ++col)
216 const unsigned char* msg_data = row_data + col * msg.point_step;
217 uint16_t ring_id = 0;
226 obj.
rowCount = ring_max - ring_min + 1;
244 for (
unsigned int row = 0; row < msg.height; ++row)
246 const unsigned char* row_data = &msg.data[row * msg.row_step];
247 for (uint32_t col = 0; col < msg.width; ++col)
249 const unsigned char* msg_data = row_data + col * msg.point_step;
252 uint16_t ring_id = 0;
261 const double azimuth = std::atan2(localPt.
y, localPt.
x);
263 const auto az_idx = lround(