pointcloud2 to map

subscribe_to_pointcloud2.py

#!/usr/bin/env python

## PYTHON2
# subscribes to PointCloud2, transforms to "map" and saves as npy


import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import numpy as np
import matplotlib.pyplot as plt
import tf2_ros
import tf2_py as tf2
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud

file_i = 0
rospy.init_node('subscribe_to_pointcloud2', anonymous=True)
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)


def callback(data):
    global run_once, first_run, tf_buffer, tf_listener, file_i 
    first_run = False
    print(data.height, "*", data.width)
    # transform to "map" frame
    trans = tf_buffer.lookup_transform("map", data.header.frame_id, rospy.get_rostime()+rospy.Duration(0.05), rospy.Duration(1))
    cloud_out = do_transform_cloud(data, trans)
    points_n = np.zeros([data.height * data.width, 3])
    i = 0
    for point in pc2.read_points(cloud_out, skip_nans=True):
        points_n[i, 0] = point[0] #x
        points_n[i, 1] = point[1] #y
        points_n[i, 2] = point[2] #z
        i += 1
    print(np.max(points_n))
    with open("p" + str(file_i) + ".npy", 'ab') as f:
        np.save(f, points_n)
    file_i += 1

#rospy.Subscriber("/left_os1/os1_cloud_node/points", PointCloud2, callback)
rospy.Subscriber("/points_raw", PointCloud2, callback)
rospy.spin()

view_npy.py

#!/usr/bin/env python

# PYTHON3

import numpy as np
import matplotlib.pyplot as plt

db = 10
points_n = np.zeros([db, 64 * 512, 3])
for i in range(db):
    print("p" + str(i) + ".npy")
    with open("p" + str(i) + ".npy", "rb") as f:
        points_n[i] = np.load(f)

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
for i in range(db):
    ax.scatter(points_n[i, :, 0], points_n[i][:, 1], points_n[i][:, 2], c=points_n[i][:, 2], cmap='hsv', s = 0.01)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')

plt.show()