ROS接收颜色点云并转换为图像和颜色点云

1. 点云转换

#!/usr/bin/env python3
# -*- coding: UTF-8 -*-
import numpy as np
import cv2.cv2 as cv

import rospy
from sensor_msgs.msg import PointCloud2
import ros_numpy

def main():
    rospy.init_node("subscriber")
    pub = rospy.Publisher('/stone/points', PointCloud2, queue_size=5)

    watch_points = np.zeros((480,640), dtype=[('x', np.float32), ('y', np.float32), ('z', np.float32), (('rgb', np.float32))])
    watch_rgb = np.zeros((480,640), dtype=[('r', np.uint8), ('g', np.uint8), ('b', np.uint8)])
    
    while True:
        msg = rospy.wait_for_message("/camera_03/depth_registered/points", PointCloud2)
        data_points = ros_numpy.numpify(msg)

        # rgb
        data_rgb = ros_numpy.point_cloud2.split_rgb_field(data_points)
        b, g, r = data_rgb['b'][:, :, np.newaxis], data_rgb['g'][:, :, np.newaxis], data_rgb['r'][:, :, np.newaxis]
        bgr = np.concatenate([b, g, r], axis=2)
        print(bgr.dtype, np.unique(bgr))
        # xyz
        x, y, z = data_points['x'], data_points['y'], data_points['z']
        x, y, z = x[:, :,   np.newaxis], y[:, :, np.newaxis], z[:, :, np.newaxis]
        xyz = np.concatenate([x, y, z], axis=2)
        print("XYZ: ", xyz.shape)

        # 显示
        cv.imshow("rgb", bgr)
        cv.waitKey(1)

        watch_points['x'] = xyz[:, :, 0]
        watch_points['y'] = xyz[:, :, 1]
        watch_points['z'] = xyz[:, :, 2]

        watch_rgb['r'] = bgr[:, :, 2]
        watch_rgb['g'] = bgr[:, :, 1]
        watch_rgb['b'] = bgr[:, :, 0]

        watch_points['rgb'] = ros_numpy.point_cloud2.merge_rgb_fields(watch_rgb)

        msg = ros_numpy.msgify(PointCloud2, watch_points, frame_id="camera_03_rgb_frame")
        pub.publish(msg)


if __name__ == "__main__":
    main()