def cloud_cb(cloud_msg): rospy.loginfo('Filtering cloud') cloud_arr = pointclouds.pointcloud2_to_array(cloud_msg, split_rgb=True) # filter points by color filtered_cloud_arr = cloud_arr[(cloud_arr['r'] < 128) * (cloud_arr['g'] < 50) * (cloud_arr['b'] < 50)] # filter out far away points filtered_cloud_arr = filtered_cloud_arr[filtered_cloud_arr['z'] < 1.5] filtered_cloud_msg = pointclouds.array_to_pointcloud2( filtered_cloud_arr, stamp=cloud_msg.header.stamp, frame_id=cloud_msg.header.frame_id, merge_rgb=True) cloud_pub.publish(filtered_cloud_msg)
def test_roundtrip(): from python_msg_conversions import pointclouds import numpy as np npoints = 100 points_arr = np.zeros((npoints,), dtype=[ ('x', np.float32), ('y', np.float32), ('z', np.float32), ('r', np.uint8), ('g', np.uint8), ('b', np.uint8)]) points_arr['x'] = np.random.random((npoints,)) points_arr['y'] = np.random.random((npoints,)) points_arr['z'] = np.random.random((npoints,)) points_arr['r'] = np.floor(np.random.random((npoints,))*255) points_arr['g'] = 0 points_arr['b'] = 255 cloud_msg = pointclouds.array_to_pointcloud2(points_arr, merge_rgb=True) new_points_arr = pointclouds.pointcloud2_to_array(cloud_msg, split_rgb=True) assert(point_arrays_equal(points_arr, new_points_arr))
import rospy from sensor_msgs.msg import PointCloud2 from python_msg_conversions import pointclouds rospy.init_node('pub_random_clouds', anonymous=True) pub = rospy.Publisher('/random_clouds', PointCloud2) npoints = 1000 while not rospy.is_shutdown(): points_arr = np.zeros((npoints,), dtype=[ ('x', np.float32), ('y', np.float32), ('z', np.float32), ('r', np.uint8), ('g', np.uint8), ('b', np.uint8)]) points_arr['x'] = np.random.random((npoints,)) points_arr['y'] = np.random.random((npoints,)) points_arr['z'] = np.random.random((npoints,)) points_arr['r'] = np.floor(np.random.random((npoints,))*255) points_arr['g'] = 0 points_arr['b'] = 255 #print points_arr cloud_msg = pointclouds.array_to_pointcloud2(points_arr, stamp=rospy.Time.now(), frame_id='/base_link', merge_rgb=True) pub.publish(cloud_msg) rospy.sleep(0.001)