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 Ros.PointCloud2()
{
header = new Ros.Header()
{
stamp = Ros2Conversions.Convert(data.Time),
frame_id = data.Frame,
},
height = 1,
width = (uint)count,
fields = PointFields,
is_bigendian = false,
point_step = 32,
row_step = (uint)count * 32,
data = new PartialByteArray()
{
Array = Buffer,
Length = count * 32,
},
is_dense = true,
};
Writer.Write(msg, completed);
}
public void RegPublisher <DataType, BridgeType>(IBridgePlugin plugin, Func <DataType, BridgeType> converter)
{
plugin.AddType <DataType>(Ros2Utils.GetMessageType <BridgeType>());
plugin.AddPublisherCreator(
(instance, topic) =>
{
var ros2Instance = instance as Ros2BridgeInstance;
ros2Instance.AddPublisher <BridgeType>(topic);
var writer = new Ros2Writer <BridgeType>(ros2Instance, topic);
return(new Publisher <DataType>((data, completed) => writer.Write(converter(data), completed)));
}
);
}
