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)
