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