ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

PointCloud2 parse to xyz array in ROS2

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

nigeno gravatar image

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

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
5

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

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

Comments

Worked here. Thank you a lot.

Lucas Marins gravatar image Lucas Marins  ( 2023-05-05 14:59:59 -0600 )edit

Question Tools

2 followers

Stats

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

Seen: 1,015 times

Last updated: Apr 05 '22