Skip to content

Commit 51f0550

Browse files
AndreasR30AndreasR30
committed
Use FLOAT64 for point fields with large integers instead of UINT32
Since UINT64 is not supported in PointCloud2 message we use FLOAT64, which can represent integers up to 2^52 exactly. UINT32 only counts up to 2^32
1 parent fe0da51 commit 51f0550

File tree

2 files changed

+36
-21
lines changed

2 files changed

+36
-21
lines changed

‎include/continuous_clustering/ros/ros_utils.hpp

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -22,11 +22,15 @@ enum ProcessingStage
2222

2323
struct PointCloud2Iterators
2424
{
25+
// Some point fields below should be actually UINT64. Unfortunately, this type is not available for a PointCloud2
26+
// message: http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html. Therefore, we use FLOAT64 which
27+
// is able to store higher integers than UINT32 (2^52 vs 2^32); these fields are marked with (*)
28+
2529
// raw points (from sensor)
2630
std::optional<sensor_msgs::PointCloud2Iterator<float>> iter_x_out;
2731
std::optional<sensor_msgs::PointCloud2Iterator<float>> iter_y_out;
2832
std::optional<sensor_msgs::PointCloud2Iterator<float>> iter_z_out;
29-
std::optional<sensor_msgs::PointCloud2Iterator<uint32_t>> iter_f_out;
33+
std::optional<sensor_msgs::PointCloud2Iterator<double>> iter_f_out;
3034
std::optional<sensor_msgs::PointCloud2Iterator<uint8_t>> iter_i_out;
3135
std::optional<sensor_msgs::PointCloud2Iterator<double>> iter_gpi_out;
3236
std::optional<sensor_msgs::PointCloud2Iterator<uint32_t>> iter_time_sec_out;
@@ -37,7 +41,7 @@ struct PointCloud2Iterators
3741
std::optional<sensor_msgs::PointCloud2Iterator<float>> iter_a_out;
3842
std::optional<sensor_msgs::PointCloud2Iterator<float>> iter_ia_out;
3943
std::optional<sensor_msgs::PointCloud2Iterator<double>> iter_ca_out;
40-
std::optional<sensor_msgs::PointCloud2Iterator<uint32_t>> iter_gc_out;
44+
std::optional<sensor_msgs::PointCloud2Iterator<double>> iter_gc_out;
4145
std::optional<sensor_msgs::PointCloud2Iterator<uint16_t>> iter_lc_out;
4246
std::optional<sensor_msgs::PointCloud2Iterator<uint16_t>> iter_r_out;
4347

@@ -50,10 +54,10 @@ struct PointCloud2Iterators
5054
std::optional<sensor_msgs::PointCloud2Iterator<double>> iter_finished_at_azimuth_angle;
5155
std::optional<sensor_msgs::PointCloud2Iterator<uint16_t>> iter_num_child_points;
5256
std::optional<sensor_msgs::PointCloud2Iterator<uint16_t>> iter_tree_root_row_index;
53-
std::optional<sensor_msgs::PointCloud2Iterator<uint32_t>> iter_tree_root_column_index;
57+
std::optional<sensor_msgs::PointCloud2Iterator<double>> iter_tree_root_column_index; // no uin64_t available
5458
std::optional<sensor_msgs::PointCloud2Iterator<uint32_t>> iter_number_of_visited_neighbors;
55-
std::optional<sensor_msgs::PointCloud2Iterator<uint32_t>> iter_tree_id;
56-
std::optional<sensor_msgs::PointCloud2Iterator<uint32_t>> iter_id;
59+
std::optional<sensor_msgs::PointCloud2Iterator<double>> iter_tree_id;
60+
std::optional<sensor_msgs::PointCloud2Iterator<double>> iter_id;
5761
};
5862

5963
sensor_msgs::PointCloud2Ptr clusterToPointCloud(const std::vector<Point>& cluster_points,

‎src/ros/ros_utils.cpp

Lines changed: 27 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -121,6 +121,10 @@ PointCloud2Iterators prepareMessageAndCreateIterators(sensor_msgs::PointCloud2&
121121
else if (fill_fields_up_to_stage == CONTINUOUS_CLUSTERING)
122122
up_to_field = 25;
123123

124+
// Some point fields below should be actually UINT64. Unfortunately, this type is not available for a PointCloud2
125+
// message: http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html. Therefore, we use FLOAT64 which
126+
// is able to store higher integers than UINT32 (2^52 vs 2^32); these fields are marked with (*)
127+
124128
sensor_msgs::PointCloud2Modifier output_modifier(msg);
125129
output_modifier.setPointCloud2Fields(up_to_field,
126130
"x",
@@ -134,13 +138,13 @@ PointCloud2Iterators prepareMessageAndCreateIterators(sensor_msgs::PointCloud2&
134138
sensor_msgs::PointField::FLOAT32,
135139
"firing_index",
136140
1,
137-
sensor_msgs::PointField::UINT32,
141+
sensor_msgs::PointField::FLOAT64, // (*)
138142
"intensity",
139143
1,
140144
sensor_msgs::PointField::UINT8,
141145
"globally_unique_point_index",
142146
1,
143-
sensor_msgs::PointField::FLOAT64,
147+
sensor_msgs::PointField::FLOAT64, // (*)
144148
"time_sec",
145149
1,
146150
sensor_msgs::PointField::UINT32,
@@ -161,7 +165,7 @@ PointCloud2Iterators prepareMessageAndCreateIterators(sensor_msgs::PointCloud2&
161165
sensor_msgs::PointField::FLOAT64,
162166
"global_column_index",
163167
1,
164-
sensor_msgs::PointField::UINT32,
168+
sensor_msgs::PointField::FLOAT64, // (*)
165169
"local_column_index",
166170
1,
167171
sensor_msgs::PointField::UINT16,
@@ -188,16 +192,16 @@ PointCloud2Iterators prepareMessageAndCreateIterators(sensor_msgs::PointCloud2&
188192
sensor_msgs::PointField::UINT16,
189193
"tree_root_column_index",
190194
1,
191-
sensor_msgs::PointField::UINT8,
195+
sensor_msgs::PointField::FLOAT64, // (*)
192196
"number_of_visited_neighbors",
193197
1,
194198
sensor_msgs::PointField::UINT32,
195199
"tree_id",
196200
1,
197-
sensor_msgs::PointField::UINT32,
201+
sensor_msgs::PointField::FLOAT64, // (*)
198202
"id",
199203
1,
200-
sensor_msgs::PointField::UINT32);
204+
sensor_msgs::PointField::FLOAT64); // (*)
201205

202206
PointCloud2Iterators iterators;
203207
iterators.iter_x_out = {msg, "x"};
@@ -243,14 +247,17 @@ void addPointToMessage(PointCloud2Iterators& container,
243247
ros::Time stamp;
244248
stamp.fromNSec(point.stamp);
245249

250+
// Some point fields below should be actually UINT64. Unfortunately, this type is not available for a PointCloud2
251+
// message: http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html. Therefore, we use FLOAT64 which
252+
// is able to store higher integers than UINT32 (2^52 vs 2^32); these fields are marked with (*)
253+
246254
// raw point
247255
*(*container.iter_x_out + data_index_message) = point.xyz.x;
248256
*(*container.iter_y_out + data_index_message) = point.xyz.y;
249257
*(*container.iter_z_out + data_index_message) = point.xyz.z;
250-
*(*container.iter_f_out + data_index_message) = static_cast<uint32_t>(point.firing_index);
258+
*(*container.iter_f_out + data_index_message) = static_cast<double>(point.firing_index); // (*)
251259
*(*container.iter_i_out + data_index_message) = point.intensity;
252-
*(*container.iter_gpi_out + data_index_message) =
253-
*reinterpret_cast<const double*>(&point.globally_unique_point_index); // PointCloud2 does not support UINT64
260+
*(*container.iter_gpi_out + data_index_message) = static_cast<double>(point.globally_unique_point_index); // (*)
254261
*(*container.iter_time_sec_out + data_index_message) = stamp.sec;
255262
*(*container.iter_time_nsec_out + data_index_message) = stamp.nsec;
256263
if (fill_fields_up_to_stage == RAW_POINT)
@@ -261,7 +268,7 @@ void addPointToMessage(PointCloud2Iterators& container,
261268
*(*container.iter_a_out + data_index_message) = point.azimuth_angle;
262269
*(*container.iter_ia_out + data_index_message) = point.inclination_angle;
263270
*(*container.iter_ca_out + data_index_message) = point.continuous_azimuth_angle;
264-
*(*container.iter_gc_out + data_index_message) = point.global_column_index;
271+
*(*container.iter_gc_out + data_index_message) = static_cast<double>(point.global_column_index); // (*)
265272
*(*container.iter_lc_out + data_index_message) = point.local_column_index;
266273
*(*container.iter_r_out + data_index_message) = point.row_index;
267274
if (fill_fields_up_to_stage == RANGE_IMAGE_GENERATION)
@@ -278,22 +285,26 @@ void addPointToMessage(PointCloud2Iterators& container,
278285
*(*container.iter_finished_at_azimuth_angle + data_index_message) = point.finished_at_continuous_azimuth_angle;
279286
*(*container.iter_num_child_points + data_index_message) = point.child_points.size();
280287
*(*container.iter_tree_root_row_index + data_index_message) = point.tree_root_.row_index;
281-
*(*container.iter_tree_root_column_index + data_index_message) = point.tree_root_.column_index;
288+
*(*container.iter_tree_root_column_index + data_index_message) =
289+
static_cast<double>(point.tree_root_.column_index); // (*)
282290
*(*container.iter_number_of_visited_neighbors + data_index_message) = point.number_of_visited_neighbors;
283291
*(*container.iter_tree_id + data_index_message) =
284-
static_cast<uint32_t>(point.tree_root_.column_index * num_rows + point.tree_root_.row_index);
285-
*(*container.iter_id + data_index_message) = point.id;
292+
static_cast<double>(point.tree_root_.column_index * num_rows + point.tree_root_.row_index); // (*)
293+
*(*container.iter_id + data_index_message) = static_cast<double>(point.id); // (*)
286294
}
287295

288296
void addRawPointToMessage(PointCloud2Iterators& container, int data_index_message, const RawPoint& point)
289297
{
298+
// Some point fields below should be actually UINT64. Unfortunately, this type is not available for a PointCloud2
299+
// message: http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html. Therefore, we use FLOAT64 which
300+
// is able to store higher integers than UINT32 (2^52 vs 2^32); these fields are marked with (*)
301+
290302
*(*container.iter_x_out + data_index_message) = point.x;
291303
*(*container.iter_y_out + data_index_message) = point.y;
292304
*(*container.iter_z_out + data_index_message) = point.z;
293-
*(*container.iter_f_out + data_index_message) = static_cast<uint32_t>(point.firing_index);
305+
*(*container.iter_f_out + data_index_message) = static_cast<double>(point.firing_index); // (*)
294306
*(*container.iter_i_out + data_index_message) = point.intensity;
295-
*(*container.iter_gpi_out + data_index_message) =
296-
*reinterpret_cast<const double*>(&point.globally_unique_point_index); // PointCloud2 does not support UINT64
307+
*(*container.iter_gpi_out + data_index_message) = static_cast<double>(point.globally_unique_point_index); // (*)
297308

298309
ros::Time t;
299310
t.fromNSec(point.stamp);

0 commit comments

Comments
 (0)