private void SendPointCloud()
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
range_array.Clear();
intensity_array.Clear();
return;
}
int FOV_take_1 = (int)(Mathf.Abs(FOVLeftLimit - FOVRightLimit) / Mathf.Abs(rotationAnglePerStep));
int FOV_skip = (int)(Mathf.Abs(-270 - FOVLeftLimit) / Mathf.Abs(rotationAnglePerStep));
int FOV_take_2 = 0;
if (FOV_take_1 + FOV_skip != (int)(360 / rotationAnglePerStep))
{
FOV_take_2 = (int)(360 / rotationAnglePerStep) - FOV_skip - FOV_take_1;
}
var msg = new Ros.LaserScan()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seqId++,
frame_id = frame_id,
},
angle_min = FOVLeftLimit * Mathf.PI / 180.0f,
angle_max = FOVRightLimit * Mathf.PI / 180.0f,
angle_increment = rotationAnglePerStep * Mathf.PI / 180.0f,
time_increment = 1.0f * 360.0f / (rotationSpeedHz * rotationAnglePerStep),
scan_time = 1.0f / rotationSpeedHz,
range_min = minRange,
range_max = maxRange,
ranges = (range_array.Take(FOV_take_2).Concat(range_array.Skip(FOV_skip).Take(FOV_take_1))).ToArray(),
intensities = (intensity_array.Take(FOV_take_2).Concat(intensity_array.Skip(FOV_skip).Take(FOV_take_1))).ToArray(),
};
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1)
{
AutowareWriterLaserScan.Publish(msg);
}
range_array.Clear();
intensity_array.Clear();
}
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 != 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
}
