12

I'm trying to do some segmentation on a pointcloud from a kinect one in ROS. As of now i have this:

import rospy
import pcl
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
def on_new_point_cloud(data):
    pc = pc2.read_points(data, skip_nans=True, field_names=("x", "y", "z"))
    pc_list = []
    for p in pc:
        pc_list.append( [p[0],p[1],p[2]] )

    p = pcl.PointCloud()
    p.from_list(pc_list)
    seg = p.make_segmenter()
    seg.set_model_type(pcl.SACMODEL_PLANE)
    seg.set_method_type(pcl.SAC_RANSAC)
    indices, model = seg.segment()

rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/kinect2/hd/points", PointCloud2, on_new_point_cloud)
rospy.spin()

This seems to work but is very slow because of the for loop. My questions are:

1) How do i effeciently convert from the PointCloud2 message to a pcl pointcloud

2) How do i visualize the clouds.

3 Answers 3

9
import rospy
import pcl
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import ros_numpy

def callback(data):
    pc = ros_numpy.numpify(data)
    points=np.zeros((pc.shape[0],3))
    points[:,0]=pc['x']
    points[:,1]=pc['y']
    points[:,2]=pc['z']
    p = pcl.PointCloud(np.array(points, dtype=np.float32))

rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/velodyne_points", PointCloud2, callback)
rospy.spin()

I would prefer using ros_numpy module to first convert to numpy array and initialize Point Cloud from that array.

2
  • This wouldn't work for ROS melodic since there is no ros_numpy for it. Could we somehow use something else to interpret the variable data? Or is this the only way?
    – Schütze
    Feb 18, 2019 at 10:32
  • where is the module pcl
    – raaj
    Jan 16, 2020 at 22:48
2

On Ubuntu 20.04 with Python3 I use the following:

import numpy as np
import pcl  # pip3 install python-pcl
import ros_numpy  # apt install ros-noetic-ros-numpy
import rosbag
import sensor_msgs

def convert_pc_msg_to_np(pc_msg):
    # Fix rosbag issues, see: https://github.com/eric-wieser/ros_numpy/issues/23
    pc_msg.__class__ = sensor_msgs.msg._PointCloud2.PointCloud2
    offset_sorted = {f.offset: f for f in pc_msg.fields}
    pc_msg.fields = [f for (_, f) in sorted(offset_sorted.items())]

    # Conversion from PointCloud2 msg to np array.
    pc_np = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(pc_msg, remove_nans=True)
    pc_pcl = pcl.PointCloud(np.array(pc_np, dtype=np.float32))
    return pc_np, pc_pcl  # point cloud in numpy and pcl format

# Use a ros subscriber as you already suggested or is shown in the other
# answers to run it online :)

# To run it offline on a rosbag use:
for topic, msg, t in rosbag.Bag('/my/rosbag.bag').read_messages():
    if topic == "/my/cloud":
        pc_np, pc_pcl = convert_pc_msg_to_np(msg)


0

This works for me. I just resize the point cloud since mine is an ordered pc (512x x 512px). My code is adapted from @Abdulbaki Aybakan - thanks for this!

My code:

pc = ros_numpy.numpify(pointcloud)
height = pc.shape[0]
width = pc.shape[1]
np_points = np.zeros((height * width, 3), dtype=np.float32)
np_points[:, 0] = np.resize(pc['x'], height * width)
np_points[:, 1] = np.resize(pc['y'], height * width)
np_points[:, 2] = np.resize(pc['z'], height * width)

To use ros_numpy one has to clone the repo: http://wiki.ros.org/ros_numpy

2
  • Does this work if pointcloud has holes in it? Will these just become NaNs in np_points?
    – tsbertalan
    Aug 28, 2019 at 15:24
  • Not sure if this works with an ordered pointcloud2, but it might work if you set every value of each point that is a hole to NaN. The "is_dense" flag should then be set to False, see: docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud2.html
    – kame
    Sep 3, 2019 at 7:58

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.