how can I see the result after pointcloud-segmentation?

1

I'm processing point cloud data on a topic. After segmentation, I want to see the result. I want to publish this point cloud and then use Rviz. But there is an Error: Data size (32 bytes) does not match width (4371) times height (1) times point_step (32). Dropping message.

Code is here:

seg = cloud_filtered.make_segmenter_normals(ksearch=50)
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_distance_threshold(0.01)
seg.set_normal_distance_weight(0.01)
seg.set_max_iterations(100)
inliers, coefficients = seg.segment()
point_seg = cloud_filtered.extract(inliers)

ros_msg = PointCloud2()
ros_msg.header.stamp = rospy.Time.now()
ros_msg.header.frame_id = "laser"
ros_msg.height = 1
ros_msg.width = point_seg.size
ros_msg.fields.append(PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1))
ros_msg.fields.append(PointField(name="rgb", offset=16, datatype=PointField.FLOAT32, count=1))
ros_msg.is_bigendian = False
ros_msg.point_step = 32
ros_msg.row_step = ros_msg.point_step * ros_msg.width * ros_msg.height
ros_msg.is_dense = False
buffer = []
for data in point_seg:
    s = struct.pack('>f', data[3])
    i = struct.unpack('>l', s)[0]
    pack = ctypes.c_uint32(i).value
    r = (pack & 0x00FF0000) >> 16
    g = (pack & 0x0000FF00) >> 8
    b = (pack & 0x000000FF)
    buffer.append(struct.pack('ffffBBBBIII', data[0], data[1], data[2], 1.0, b, g, r, 0, 0, 0, 0))
ros_msg.data = "".join(buffer)
self.pub.publish(ros_msg)

2 or is there another way to see the result? thanks for the help!

python
ros
point-clouds
asked on Stack Overflow Oct 1, 2019 by Yuhhh • edited Oct 2, 2019 by Benyamin Jafari

0 Answers

Nobody has answered this question yet.


User contributions licensed under CC BY-SA 3.0