def setUp(self):
self.point_cloud_in = point_cloud2.PointCloud2()
self.point_cloud_in.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
self.point_cloud_in.point_step = 4 * 3
self.point_cloud_in.height = 1
# we add two points (with x, y, z to the cloud)
self.point_cloud_in.width = 2
self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
points = [1, 2, 0, 10, 20, 30]
self.point_cloud_in.data = struct.pack('%sf' % len(points), *points)
self.transform_translate_xyz_300 = TransformStamped()
self.transform_translate_xyz_300.transform.translation.x = 300
self.transform_translate_xyz_300.transform.translation.y = 300
self.transform_translate_xyz_300.transform.translation.z = 300
self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w
assert (list(point_cloud2.read_points(self.point_cloud_in)) == [
(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)
])
def setUp(self):
self.point_cloud_in = point_cloud2.PointCloud2()
self.point_cloud_in.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('index', 12, PointField.INT32, 1)
]
self.point_cloud_in.point_step = 4 * 4
self.point_cloud_in.height = 1
# we add two points (with x, y, z to the cloud)
self.point_cloud_in.width = 2
self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
self.points = [(1.0, 2.0, 0.0, 123), (10.0, 20.0, 30.0, 456)]
for point in self.points:
self.point_cloud_in.data += struct.pack('3fi', *point)
self.transform_translate_xyz_300 = TransformStamped()
self.transform_translate_xyz_300.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w
assert (list(point_cloud2.read_points(
self.point_cloud_in)) == self.points)
def create_cloud(header, fields, points):
"""
Create a L{sensor_msgs.msg.PointCloud2} message.
@param header: The point cloud header.
@type header: L{std_msgs.msg.Header}
@param fields: The point cloud fields.
@type fields: iterable of L{sensor_msgs.msg.PointField}
@param points: The point cloud points.
@type points: list of iterables, i.e. one iterable for each point, with the
elements of each iterable being the values of the fields for
that point (in the same order as the fields parameter)
@return: The point cloud.
@rtype: L{sensor_msgs.msg.PointCloud2}
"""
cloud_struct = struct.Struct(_get_struct_fmt(False, fields))
buff = ctypes.create_string_buffer(cloud_struct.size * len(points))
point_step, pack_into = cloud_struct.size, cloud_struct.pack_into
offset = 0
for p in points:
pack_into(buff, offset, *p)
offset += point_step
return pc2.PointCloud2(header=header,
height=1,
width=len(points),
is_dense=False,
is_bigendian=False,
fields=fields,
point_step=cloud_struct.size,
row_step=cloud_struct.size * len(points),
data=buff.raw)