public static LGSVL.Detection3DArray ConvertFrom(Detected3DObjectData data)
{
return(new LGSVL.Detection3DArray()
{
header = new Header()
{
seq = data.Sequence,
stamp = Conversions.ConvertTime(data.Time),
frame_id = data.Frame,
},
detections = data.Data.Select(d => new Detection3D()
{
id = d.Id,
label = d.Label,
score = d.Score,
bbox = new BoundingBox3D()
{
position = new Pose()
{
position = ConvertToPoint(d.Position),
orientation = Convert(d.Rotation),
},
size = ConvertToVector(d.Scale),
},
velocity = new Twist()
{
linear = ConvertToVector(d.LinearVelocity),
angular = ConvertToVector(d.AngularVelocity),
}
}).ToList(),
});
}
public static LGSVL.Detection2DArray ConvertFrom(Detected2DObjectData data)
{
return(new LGSVL.Detection2DArray()
{
header = new Header()
{
seq = data.Sequence,
stamp = Conversions.ConvertTime(data.Time),
frame_id = data.Frame,
},
detections = data.Data.Select(d => new Detection2D()
{
id = d.Id,
label = d.Label,
score = d.Score,
bbox = new BoundingBox2D()
{
x = d.Position.x,
y = d.Position.y,
width = d.Scale.x,
height = d.Scale.y
},
velocity = new Twist()
{
linear = ConvertToVector(d.LinearVelocity),
angular = ConvertToVector(d.AngularVelocity),
}
}).ToList(),
});
}
public static LGSVL.SignalArray ConvertFrom(SignalDataArray data)
{
return(new LGSVL.SignalArray()
{
header = new Header()
{
seq = data.Sequence,
stamp = Conversions.ConvertTime(data.Time),
frame_id = data.Frame,
},
signals = data.Data.Select(d => new Signal()
{
id = d.Id,
label = d.Label,
score = d.Score,
bbox = new BoundingBox3D()
{
position = new Pose()
{
position = ConvertToPoint(d.Position),
orientation = Convert(d.Rotation),
},
size = ConvertToVector(d.Scale),
}
}).ToList(),
});
}
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()
{
seq = data.Sequence,
stamp = Conversions.ConvertTime(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 static Lgsvl.Detection3DArray ConvertFrom(Detected3DObjectData data)
{
var arr = new Lgsvl.Detection3DArray()
{
header = new Ros.Header()
{
seq = data.Sequence,
stamp = Conversions.ConvertTime(data.Time),
frame_id = data.Frame,
},
detections = new List <Lgsvl.Detection3D>(),
};
foreach (var d in data.Data)
{
// Transform from (Right/Up/Forward) to (Forward/Left/Up)
var position = d.Position;
position.Set(position.z, -position.x, position.y);
var orientation = d.Rotation;
orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w);
var size = d.Scale;
size.Set(size.z, size.x, size.y);
d.AngularVelocity.z = -d.AngularVelocity.z;
var det = new Lgsvl.Detection3D()
{
id = d.Id,
label = d.Label,
score = d.Score,
bbox = new Lgsvl.BoundingBox3D()
{
position = new Ros.Pose()
{
position = ConvertToPoint(position),
orientation = Convert(orientation),
},
size = ConvertToVector(size),
},
velocity = new Ros.Twist()
{
linear = ConvertToVector(d.LinearVelocity),
angular = ConvertToVector(d.AngularVelocity),
},
};
arr.detections.Add(det);
}
return(arr);
}
public void Write(GpsData message, Action completed)
{
char latitudeS = message.Latitude < 0 ? 'S' : 'N';
char longitudeS = message.Longitude < 0 ? 'W' : 'E';
double lat = Math.Abs(message.Latitude);
double lon = Math.Abs(message.Longitude);
lat = Math.Floor(lat) * 100 + (lat % 1) * 60.0f;
lon = Math.Floor(lon) * 100 + (lon % 1) * 60.0f;
var dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(message.Time * 1000.0)).LocalDateTime;
var utc = dt.ToString("HHmmss.fff");
var gga = string.Format("GPGGA,{0},{1:0.000000},{2},{3:0.000000},{4},{5},{6},{7},{8:0.000000},M,{9:0.000000},M,,",
utc,
lat, latitudeS,
lon, longitudeS,
1, // GPX fix
10, // sattelites tracked
Accuracy,
message.Altitude,
Height);
var angles = message.Orientation.eulerAngles;
float roll = -angles.z;
float pitch = -angles.x;
float yaw = angles.y;
var qq = string.Format("QQ02C,INSATT,V,{0},{1:0.000},{2:0.000},{3:0.000},",
utc,
roll,
pitch,
yaw);
// http://www.plaisance-pratique.com/IMG/pdf/NMEA0183-2.pdf
// 5.2.3 Checksum Field
byte ggaChecksum = 0;
for (int i = 0; i < gga.Length; i++)
{
ggaChecksum ^= (byte)gga[i];
}
byte qqChecksum = 0;
for (int i = 0; i < qq.Length; i++)
{
qqChecksum ^= (byte)qq[i];
}
var ggaMessage = new Sentence()
{
header = new Header()
{
stamp = Conversions.ConvertTime(message.Time),
seq = 2 * message.Sequence + 0,
frame_id = message.Frame,
},
sentence = "$" + gga + "*" + ggaChecksum.ToString("X2"),
};
Writer.Write(ggaMessage);
var qqMessage = new Sentence()
{
header = new Header()
{
stamp = ggaMessage.header.stamp,
seq = 2 * message.Sequence + 1,
frame_id = message.Frame,
},
sentence = qq + "@" + qqChecksum.ToString("X2"),
};
Writer.Write(qqMessage, completed);
}
