public void Write(PointCloudData data, Action completed)
{
if (Buffer == null || Buffer.Length != data.Points.Length)
{
Buffer = new byte[32 * data.Points.Length];
}
int count = 0;
unsafe
{
fixed(byte *ptr = Buffer)
{
int offset = 0;
for (int i = 0; i < data.Points.Length; i++)
{
var point = data.Points[i];
if (point == UnityEngine.Vector4.zero)
{
continue;
}
var pos = new UnityEngine.Vector3(point.x, point.y, point.z);
float intensity = point.w;
*(UnityEngine.Vector3 *)(ptr + offset) = data.Transform.MultiplyPoint3x4(pos);
*(ptr + offset + 16) = (byte)(intensity * 255);
offset += 32;
count++;
}
}
}
var msg = new PointCloud2()
{
header = new Header()
{
seq = data.Sequence,
stamp = Conversions.ConvertTime(data.Time),
frame_id = data.Frame,
},
height = 1,
width = (uint)count,
fields = new []
{
new PointField()
{
name = "x",
offset = 0,
datatype = 7,
count = 1,
},
new PointField()
{
name = "y",
offset = 4,
datatype = 7,
count = 1,
},
new PointField()
{
name = "z",
offset = 8,
datatype = 7,
count = 1,
},
new PointField()
{
name = "intensity",
offset = 16,
datatype = 2,
count = 1,
},
new PointField()
{
name = "timestamp",
offset = 24,
datatype = 8,
count = 1,
},
},
is_bigendian = false,
point_step = 32,
row_step = (uint)count * 32,
data = new PartialByteArray()
{
Array = Buffer,
Length = count * 32,
},
is_dense = true,
};
OriginalWriter.Write(msg, completed);
}
