class RosCommon:
def __init__(self, init_node=False):
self.init_node = init_node
if not self.init_node:
rospy.init_node('listener', disable_signals=True, anonymous=True)
@staticmethod
def process_pcd_data(msg):
message_cloud = PointCloud.from_msg(msg)
pcd_path = '/data/' + time.strftime("%Y%m%d%H%M",
time.localtime()) + '.pcd'
try:
parent_cloud = pypcd.point_cloud_from_path(pcd_path)
except Exception, ex:
import traceback
traceback.print_exc()
log.error(ex.message)
message_cloud.save(pcd_path)
return
if len(parent_cloud.pc_data) > 0:
log.info('Concatenate two point clouds into bigger point cloud')
# bigger cloud = a + b
parent_cloud = pypcd.cat_point_clouds(parent_cloud, message_cloud)
parent_cloud.save(pcd_path)
def test_cat_pointclouds(pcd_fname):
from pypcd import pypcd
pc = pypcd.PointCloud.from_path(pcd_fname)
pc2 = pc.copy()
pc2.pc_data['x'] += 0.1
pc3 = pypcd.cat_point_clouds(pc, pc2)
for fld, fld3 in zip(pc.fields, pc3.fields):
assert (fld == fld3)
assert (pc3.width == pc.width + pc2.width)
def test_cat_pointclouds(pcd_fname):
import pypcd
pc = pypcd.PointCloud.from_path(pcd_fname)
pc2 = pc.copy()
pc2.pc_data['x'] += 0.1
pc3 = pypcd.cat_point_clouds(pc, pc2)
for fld, fld3 in zip(pc.fields, pc3.fields):
assert(fld == fld3)
assert(pc3.width == pc.width+pc2.width)
