def from_msg(msg, squeeze=True):
""" from pointcloud2 msg
squeeze: fix when clouds get 1 as first dim
"""
if not HAS_SENSOR_MSGS:
raise NotImplementedError("ROS sensor_msgs not found")
md = {
"version": 0.7,
"fields": [],
"size": [],
"count": [],
"width": msg.width,
"height": msg.height,
"viewpoint": [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
"points": 0,
"type": [],
"data": "binary_compressed",
}
for field in msg.fields:
md["fields"].append(field.name)
t, s = pc2_type_to_pcd_type[field.datatype]
md["type"].append(t)
md["size"].append(s)
# TODO handle multicount correctly
if field.count > 1:
warnings.warn("fields with count > 1 are not well tested")
md["count"].append(field.count)
pc_array = numpy_pc2.pointcloud2_to_array(msg)
pc_data = pc_array.reshape(-1)
md["height"], md["width"] = pc_array.shape
md["points"] = len(pc_data)
pc = PointCloud(md, pc_data)
return pc
def from_msg(msg, squeeze=True):
""" from pointcloud2 msg
squeeze: fix when clouds get 1 as first dim
"""
if not HAS_SENSOR_MSGS:
raise NotImplementedError('ROS sensor_msgs not found')
md = {
'version': .7,
'fields': [],
'size': [],
'count': [],
'width': 0,
'height': 1,
'viewpoint': [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
'points': 0,
'type': [],
'data': 'binary_compressed'
}
for field in msg.fields:
md['fields'].append(field.name)
t, s = pc2_type_to_pcd_type[field.datatype]
md['type'].append(t)
md['size'].append(s)
# TODO handle multicount correctly
if field.count > 1:
warnings.warn('fields with count > 1 are not well tested')
md['count'].append(field.count)
pc_data = np.squeeze(numpy_pc2.pointcloud2_to_array(msg))
md['width'] = len(pc_data)
md['points'] = len(pc_data)
pc = PointCloud(md, pc_data)
return pc
def from_msg(msg, squeeze=True):
""" from pointcloud2 msg
squeeze: fix when clouds get 1 as first dim
"""
if not HAS_SENSOR_MSGS:
raise NotImplementedError('ROS sensor_msgs not found')
md = {'version': .7,
'fields': [],
'size': [],
'count': [],
'width': 0,
'height': 1,
'viewpoint': [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
'points': 0,
'type': [],
'data': 'binary_compressed'}
for field in msg.fields:
md['fields'].append(field.name)
t, s = pc2_type_to_pcd_type[field.datatype]
md['type'].append(t)
md['size'].append(s)
# TODO handle multicount correctly
if field.count > 1:
warnings.warn('fields with count > 1 are not well tested')
md['count'].append(field.count)
pc_data = np.squeeze(numpy_pc2.pointcloud2_to_array(msg))
md['width'] = len(pc_data)
md['points'] = len(pc_data)
pc = PointCloud(md, pc_data)
return pc
