@@ -332,13 +332,14 @@ void log_point_cloud2(
332332 }
333333 has_z = true ;
334334 } else if (field.name == options.colormap_field .value_or (" rgb" )) {
335- rgb_offset = field.offset ;
336- if (field.datatype != sensor_msgs::msg::PointField::UINT32 &&
337- field.datatype != sensor_msgs::msg::PointField::FLOAT32 ) {
335+ if (field.datatype == sensor_msgs::msg::PointField::UINT32 ||
336+ field.datatype == sensor_msgs::msg::PointField::FLOAT32 ) {
337+ has_rgb = true ;
338+ rgb_offset = field.offset ;
339+ } else {
338340 rec.log (entity_path, rerun::TextLog (" Only UINT32 and FLOAT32 rgb field supported" ));
339- return ;
341+ continue ;
340342 }
341- has_rgb = true ;
342343 }
343344 }
344345
@@ -366,9 +367,9 @@ void log_point_cloud2(
366367 std::memcpy (&position.xyz .xyz [1 ], &msg->data [point_offset + y_offset], sizeof (float ));
367368 std::memcpy (&position.xyz .xyz [2 ], &msg->data [point_offset + z_offset], sizeof (float ));
368369 if (has_rgb) {
369- uint8_t rgba [4 ];
370- std::memcpy (&rgba , &msg->data [point_offset + rgb_offset], sizeof (uint32_t ));
371- colors.emplace_back (rerun::Color (rgba [2 ], rgba [1 ], rgba [0 ]));
370+ uint8_t bgra [4 ];
371+ std::memcpy (&bgra , &msg->data [point_offset + rgb_offset], sizeof (uint32_t ));
372+ colors.emplace_back (rerun::Color (bgra [2 ], bgra [1 ], bgra [0 ]));
372373 }
373374 points[i * msg->width + j] = position;
374375 }
0 commit comments