protected override string[] ToRenderType(Message message)
{
if (_isFirstMessage && Renderer is not null)
{
Renderer.SetHeader(RosMessageConvertExtender.GetMessagePropertyNames(message.GetType()));
_isFirstMessage = false;
}
return(message.GetData());
}
public static SlamPoint[] ToSlamPoints(this PointCloud2Message cloud)
{
var data = cloud.data.ToArray();
var res = new SlamPoint[data.Length / cloud.point_step];
var xOffset = cloud.fields.First(f => f.Name == "x").Offset;
var yOffset = cloud.fields.First(f => f.Name == "y").Offset;
var zOffset = cloud.fields.First(f => f.Name == "z").Offset;
var intensityOffset = cloud.fields.FirstOrDefault(f => f.Name == "intensity")?.Offset ??
cloud.point_step + 1;
var rgbOffset = cloud.fields.FirstOrDefault(f => f.Name == "rgb")?.Offset ?? cloud.point_step + 1;
var rgbaOffset = cloud.fields.FirstOrDefault(f => f.Name == "rgba")?.Offset ??
cloud.point_step + 1;
for (int i = 0; i < data.Length / cloud.point_step; i++)
{
var x = BitConverter.ToSingle(data, (int)(i * cloud.point_step + xOffset));
var y = BitConverter.ToSingle(data, (int)(i * cloud.point_step + yOffset));
var z = BitConverter.ToSingle(data, (int)(i * cloud.point_step + zOffset));
Color color = Color.black;
if (intensityOffset < cloud.point_step)
{
var intensity = BitConverter.ToSingle(data, (int)(i * cloud.point_step + intensityOffset));
color = RosMessageConvertExtender.ColorFromIntensity(intensity);
}
else if (rgbOffset < cloud.point_step)
{
byte r = data[i * cloud.point_step + rgbOffset + 0];
byte g = data[i * cloud.point_step + rgbOffset + 1];
byte b = data[i * cloud.point_step + rgbOffset + 2];
color = new Color(r / (float)byte.MaxValue, g / (float)byte.MaxValue, b / (float)byte.MaxValue);
}
else if (rgbaOffset < cloud.point_step)
{
byte a = data[i * cloud.point_step + rgbaOffset + 0];
byte r = data[i * cloud.point_step + rgbaOffset + 1];
byte g = data[i * cloud.point_step + rgbaOffset + 2];
byte b = data[i * cloud.point_step + rgbaOffset + 3];
color = new Color(r / (float)byte.MaxValue, g / (float)byte.MaxValue, b / (float)byte.MaxValue,
a / (float)byte.MaxValue);
}
var pos = new Vector3(x, y, z);
RosMessageConvertExtender.Converter?.Convert(ref pos);
res[i] = new SlamPoint(i, pos, color);
}
return(res);
}
