Ask Your Question
1

PointCloud2 parse to xyz array in ROS2

asked 2022-03-31 11:24:46 -0500

nigeno gravatar image

updated 2022-03-31 12:24:48 -0500

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2022-04-05 10:33:11 -0500

ChuiV gravatar image

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;
    }
}
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2022-03-31 11:24:46 -0500

Seen: 106 times

Last updated: Apr 05