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); }