void SendPointCloud(List <VelodynePointCloudVertex> pointCloud)
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
return;
}
var pointCount = pointCloud.Count;
byte[] byteData = new byte[32 * pointCount];
for (int i = 0; i < pointCount; i++)
{
var local = sensorLocalspaceTransform.InverseTransformPoint(pointCloud[i].position);
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
local.Set(local.z, -local.x, local.y);
}
else
{
local.Set(local.x, local.z, local.y);
}
var scaledPos = local * exportScaleFactor;
var x = System.BitConverter.GetBytes(scaledPos.x);
var y = System.BitConverter.GetBytes(scaledPos.y);
var z = System.BitConverter.GetBytes(scaledPos.z);
//var intensity = System.BitConverter.GetBytes(pointCloud[i].color.maxColorComponent);
//var intensity = System.BitConverter.GetBytes((float)(((int)pointCloud[i].color.r) << 16 | ((int)pointCloud[i].color.g) << 8 | ((int)pointCloud[i].color.b)));
//var intensity = System.BitConverter.GetBytes((byte)pointCloud[i].distance);
var intensity = System.BitConverter.GetBytes((byte)(pointCloud[i].color.grayscale * 255));
var ring = System.BitConverter.GetBytes(pointCloud[i].ringNumber);
var ts = System.BitConverter.GetBytes((double)0.0);
System.Buffer.BlockCopy(x, 0, byteData, i * 32 + 0, 4);
System.Buffer.BlockCopy(y, 0, byteData, i * 32 + 4, 4);
System.Buffer.BlockCopy(z, 0, byteData, i * 32 + 8, 4);
System.Buffer.BlockCopy(intensity, 0, byteData, i * 32 + 16, 1);
System.Buffer.BlockCopy(ts, 0, byteData, i * 32 + 24, 8);
}
var msg = new Ros.PointCloud2()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seqId++,
frame_id = "velodyne", // needed for Autoware
},
height = 1,
width = (uint)pointCount,
fields = new Ros.PointField[] {
new Ros.PointField()
{
name = "x",
offset = 0,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "y",
offset = 4,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "z",
offset = 8,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "intensity",
offset = 16,
datatype = 2,
count = 1,
},
new Ros.PointField()
{
name = "timestamp",
offset = 24,
datatype = 8,
count = 1,
},
},
is_bigendian = false,
point_step = 32,
row_step = (uint)pointCount * 32,
data = byteData,
is_dense = true,
};
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
Bridge.Publish(topicName, msg);
}
if (targetEnv == ROSTargetEnvironment.APOLLO)
{
Bridge.Publish(ApolloTopicName, msg);
}
}
void SendMessage()
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
return;
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.BeginSample("SendMessage");
#endif
var worldToLocal = Matrix4x4.TRS(transform.position, transform.rotation, Vector3.one).inverse;
if (TargetEnvironment == ROSTargetEnvironment.APOLLO)
{
// local.Set(local.x, local.z, local.y);
worldToLocal = new Matrix4x4(new Vector4(1, 0, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(0, 1, 0, 0), Vector4.zero) * worldToLocal;
}
else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE)
{
// local.Set(local.z, -local.x, local.y);
worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero) * worldToLocal;
}
int count = 0;
unsafe
{
fixed(byte *ptr = RosPointCloud)
{
int offset = 0;
for (int i = 0; i < PointCloud.Length; i++)
{
var point = PointCloud[i];
if (point == Vector4.zero)
{
continue;
}
var worldPos = new Vector3(point.x, point.y, point.z);
float intensity = point.w;
*(Vector3 *)(ptr + offset) = worldToLocal.MultiplyPoint3x4(worldPos);
*(ptr + offset + 16) = (byte)(intensity * 255);
offset += 32;
count++;
}
}
}
var msg = new Ros.PointCloud2()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = "velodyne", // needed for Autoware
},
height = 1,
width = (uint)count,
fields = new Ros.PointField[] {
new Ros.PointField()
{
name = "x",
offset = 0,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "y",
offset = 4,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "z",
offset = 8,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "intensity",
offset = 16,
datatype = 2,
count = 1,
},
new Ros.PointField()
{
name = "timestamp",
offset = 24,
datatype = 8,
count = 1,
},
},
is_bigendian = false,
point_step = 32,
row_step = (uint)count * 32,
data = new Ros.PartialByteArray()
{
Base64 = System.Convert.ToBase64String(RosPointCloud, 0, count * 32),
},
is_dense = true,
};
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.BeginSample("Publish");
#endif
if (TargetEnvironment == ROSTargetEnvironment.APOLLO)
{
Bridge.Publish(ApolloTopicName, msg);
}
else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE)
{
Bridge.Publish(AutowareTopicName, msg);
}
else
{
Bridge.Publish(TopicName, msg);
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.EndSample();
#endif
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.EndSample();
#endif
}
void SendMessage(int currentEndIndex, float currentEndAngle)
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.BeginSample("SendMessage");
#endif
// Lidar x is forward, y is left, z is up
var worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero);
if (Compensated)
{
worldToLocal = worldToLocal * transform.worldToLocalMatrix;
}
if (CurrentRayCount != 1)
{
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
double measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
if (TargetEnvironment == ROSTargetEnvironment.APOLLO35)
{
var msg = new Apollo.Drivers.PointCloud()
{
Header = new Apollo.Common.Header()
{
// TimestampSec = measurement_time,
SequenceNum = Sequence++,
FrameId = FrameName,
},
FrameId = "lidar128",
IsDense = false,
MeasurementTime = measurement_time,
Height = 1,
Width = (uint)PointCloud.Length, // TODO is this right?
};
for (int i = 0; i < PointCloud.Length; i++)
{
var point = PointCloud[i];
if (point == Vector4.zero)
{
continue;
}
var pos = new Vector3(point.x, point.y, point.z);
float intensity = point.w;
pos = worldToLocal.MultiplyPoint3x4(pos);
msg.Point.Add(new Apollo.Drivers.PointXYZIT()
{
X = pos.x,
Y = pos.y,
Z = pos.z,
Intensity = (byte)(intensity * 255),
Timestamp = (ulong)measurement_time
});
}
;
Apollo35WriterPointCloud2.Publish(msg);
}
else
{
int count = 0;
unsafe
{
fixed(byte *ptr = RosPointCloud)
{
int offset = 0;
for (int i = 0; i < PointCloud.Length; i++)
{
var point = PointCloud[i];
if (point == Vector4.zero)
{
continue;
}
var pos = new Vector3(point.x, point.y, point.z);
float intensity = point.w;
*(Vector3 *)(ptr + offset) = worldToLocal.MultiplyPoint3x4(pos);
*(ptr + offset + 16) = (byte)(intensity * 255);
offset += 32;
count++;
}
}
}
var msg = new Ros.PointCloud2()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = FrameName,
},
height = 1,
width = (uint)count,
fields = new Ros.PointField[] {
new Ros.PointField()
{
name = "x",
offset = 0,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "y",
offset = 4,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "z",
offset = 8,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "intensity",
offset = 16,
datatype = 2,
count = 1,
},
new Ros.PointField()
{
name = "timestamp",
offset = 24,
datatype = 8,
count = 1,
},
},
is_bigendian = false,
point_step = 32,
row_step = (uint)count * 32,
data = new Ros.PartialByteArray()
{
Base64 = System.Convert.ToBase64String(RosPointCloud, 0, count * 32),
},
is_dense = true,
};
if (TargetEnvironment == ROSTargetEnvironment.APOLLO)
{
ApolloWriterPointCloud2.Publish(msg);
}
else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE)
{
AutowareWriterPointCloud2.Publish(msg);
}
else
{
WriterPointCloud2.Publish(msg);
}
}
}
else // for planar lidar only (single ray lidar) publish LaserScan instead of PoinCloud2
{
float fixup = currentEndAngle;
FixupAngle = currentEndAngle;
float LaserScanMinAngle = -Mathf.PI;
float LaserScanMaxAngle = Mathf.PI;
float LaserScanMinRange = 0.0f;
float LaserScanMaxRange = 40.0f;
float[] range_array = new float[CurrentMeasurementsPerRotation];
float[] intensity_array = new float[CurrentMeasurementsPerRotation];
int offset = (int)(currentEndIndex - currentEndAngle / 360.0f * CurrentMeasurementsPerRotation);
for (int i = 0; i < PointCloud.Length; i++)
{
var point = PointCloud[i];
if (point == Vector4.zero)
{
continue;
}
range_array[(i + offset + CurrentMeasurementsPerRotation) % CurrentMeasurementsPerRotation] = Vector3.Distance(new Vector3(point.x, point.y, point.z), transform.position);
intensity_array[(i + offset + CurrentMeasurementsPerRotation) % CurrentMeasurementsPerRotation] = point.w;
}
// Populate LaserScan message
var msg = new Ros.LaserScan()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = FrameName,
},
angle_min = LaserScanMinAngle,
angle_max = LaserScanMaxAngle,
angle_increment = Mathf.Abs(LaserScanMaxAngle - LaserScanMinAngle) / CurrentMeasurementsPerRotation,
time_increment = 1.0f / (CurrentMeasurementsPerRotation * RotationFrequency),
scan_time = 1.0f / RotationFrequency,
range_min = LaserScanMinRange,
range_max = LaserScanMaxRange,
ranges = range_array, // TODO: Should make sure only sending range between min and max
intensities = intensity_array,
};
WriterLaserScan.Publish(msg);
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.EndSample();
#endif
}
void SendMessage(int currentEndIndex, float currentEndAngle)
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
// Lidar x is forward, y is left, z is up
var worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero);
worldToLocal = worldToLocal * transform.worldToLocalMatrix;
int count = 0;
unsafe
{
fixed(byte *ptr = RosPointCloud)
{
int offset = 0;
for (int i = 0; i < PointCloud.Length; i++)
{
var point = PointCloud[i];
if (point == Vector4.zero)
{
continue;
}
var pos = new Vector3(point.x, point.y, point.z);
float intensity = point.w;
*(Vector3 *)(ptr + offset) = worldToLocal.MultiplyPoint3x4(pos);
*(ptr + offset + 16) = (byte)(intensity * 255);
offset += 32;
count++; //if (i == 0) { Debug.Log("<color=blue>:</color>"); }
}
}
}
var msg = new Ros.PointCloud2()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = FrameName,
},
height = 1,
width = (uint)count,
fields = new Ros.PointField[] {
new Ros.PointField()
{
name = "x",
offset = 0,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "y",
offset = 4,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "z",
offset = 8,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "intensity",
offset = 16,
datatype = 2,
count = 1,
},
new Ros.PointField()
{
name = "timestamp",
offset = 24,
datatype = 8,
count = 1,
},
},
is_bigendian = false,
point_step = 32,
row_step = (uint)count * 32,
data = new Ros.PartialByteArray()
{
Base64 = System.Convert.ToBase64String(RosPointCloud, 0, count * 32),
},
is_dense = true,
};
WriterPointCloud2.Publish(msg);
}
void SendMessage(int currentEndIndex, float currentEndAngle)
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
return;
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.BeginSample("SendMessage");
#endif
Matrix4x4 worldToLocal;
if (TargetEnvironment == ROSTargetEnvironment.APOLLO)
{
// local.Set(local.x, local.z, local.y);
worldToLocal = new Matrix4x4(new Vector4(1, 0, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(0, 1, 0, 0), Vector4.zero);
}
else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE)
{
// local.Set(local.z, -local.x, local.y);
worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero);
}
else
{
worldToLocal = Matrix4x4.identity;
}
if (Compensated)
{
worldToLocal = worldToLocal * transform.worldToLocalMatrix;
}
if (CurrentRayCount != 1)
{
int count = 0;
unsafe
{
fixed(byte *ptr = RosPointCloud)
{
int offset = 0;
for (int i = 0; i < PointCloud.Length; i++)
{
var point = PointCloud[i];
if (point == Vector4.zero)
{
continue;
}
var pos = new Vector3(point.x, point.y, point.z);
float intensity = point.w;
*(Vector3 *)(ptr + offset) = worldToLocal.MultiplyPoint3x4(pos);
*(ptr + offset + 16) = (byte)(intensity * 255);
offset += 32;
count++;
}
}
}
var msg = new Ros.PointCloud2()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = FrameName,
},
height = 1,
width = (uint)count,
fields = new Ros.PointField[] {
new Ros.PointField()
{
name = "x",
offset = 0,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "y",
offset = 4,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "z",
offset = 8,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "intensity",
offset = 16,
datatype = 2,
count = 1,
},
new Ros.PointField()
{
name = "timestamp",
offset = 24,
datatype = 8,
count = 1,
},
},
is_bigendian = false,
point_step = 32,
row_step = (uint)count * 32,
data = new Ros.PartialByteArray()
{
Base64 = System.Convert.ToBase64String(RosPointCloud, 0, count * 32),
},
is_dense = true,
};
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.BeginSample("Publish");
#endif
if (TargetEnvironment == ROSTargetEnvironment.APOLLO)
{
Bridge.Publish(ApolloTopicName, msg);
}
else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE)
{
Bridge.Publish(AutowareTopicName, msg);
}
else
{
Bridge.Publish(TopicName, msg);
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.EndSample();
#endif
}
else // for planar lidar only (single ray lidar) publish LaserScan instead of PoinCloud2
{
float fixup = currentEndAngle;
FixupAngle = currentEndAngle;
float LaserScanMinAngle = -Mathf.PI;
float LaserScanMaxAngle = Mathf.PI;
float LaserScanMinRange = 0.0f;
float LaserScanMaxRange = 40.0f;
float[] range_array = new float[CurrentMeasurementsPerRotation];
float[] intensity_array = new float[CurrentMeasurementsPerRotation];
int offset = (int)(currentEndIndex - currentEndAngle / 360.0f * CurrentMeasurementsPerRotation);
for (int i = 0; i < PointCloud.Length; i++)
{
var point = PointCloud[i];
if (point == Vector4.zero)
{
continue;
}
range_array[(i + offset + CurrentMeasurementsPerRotation) % CurrentMeasurementsPerRotation] = Vector3.Distance(new Vector3(point.x, point.y, point.z), transform.position);
intensity_array[(i + offset + CurrentMeasurementsPerRotation) % CurrentMeasurementsPerRotation] = point.w;
}
// Populate LaserScan message
var msg = new Ros.LaserScan()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = FrameName,
},
angle_min = LaserScanMinAngle,
angle_max = LaserScanMaxAngle,
angle_increment = Mathf.Abs(LaserScanMaxAngle - LaserScanMinAngle) / CurrentMeasurementsPerRotation,
time_increment = 1.0f / (CurrentMeasurementsPerRotation * RotationFrequency),
scan_time = 1.0f / RotationFrequency,
range_min = LaserScanMinRange,
range_max = LaserScanMaxRange,
ranges = range_array, // TODO: Should make sure only sending range between min and max
intensities = intensity_array,
};
Bridge.Publish(TopicName, msg);
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.EndSample();
#endif
}
