Question

I'm trying to find a solution of converting PointCloud2 message data into xyz array for further analysis. Though, in my case I'm interested only in 2d application.

The information is either scarce or confusing.

Each point data in the PointCloud2 is stored as a binary blob. For example: First 1-4 numbers represent x as a 32 bit float, 5-8 represents y, 9-12 represents z, 13-16 represents the intensity.

So, my question is, is there any ROS2 library to make the conversion? Alternatively, any example of implementing it in C++.

For both cases, please provide a full code snippet or a link to an example.

Asked by nigeno on 2022-03-31 11:24:46 UTC

Answer 1

You're looking for a point cloud2 iterator:

#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>

void PointCloud2Callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
    const size_t number_of_points = msg->height * msg->width;
    sensor_msgs::PointCloud2Iterator<float> iter_x(*msg, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
    for (size_t i = 0; i < number_of_points; ++i, ++iter_x, ++iter_y)
    {
        double x = *iter_x;
        double y = *iter_y;
    }
}

Asked by ChuiV on 2022-04-05 10:33:11 UTC

Comments

Worked here. Thank you a lot.

Asked by Lucas Marins on 2023-05-05 14:59:59 UTC

PointCloud2 parse to xyz array in ROS2

Comments

Answers

Comments