protected override void SensorUpdate()
{
Camera.Render();
SegmentationCamera.Render();
minX.GetData(minXarr);
maxX.GetData(maxXarr);
minY.GetData(minYarr);
maxY.GetData(maxYarr);
DetectedObjects.Clear();
// 0 is reserved for clear color, 255 for non-agent segmentation
for (var i = 1; i < 255; ++i)
{
if (minXarr[i] == Width - 1 || maxXarr[i] == 0)
{
continue;
}
var detected = GetDetectedObject(i);
if (detected != null)
{
DetectedObjects.Add(detected);
}
}
MaxTracked = Math.Max(MaxTracked, DetectedObjects.Count);
if (Bridge != null && Bridge.Status == Status.Connected)
{
Publish(new Detected2DObjectData()
{
Frame = Frame,
Sequence = seqId++,
Time = SimulatorManager.Instance.CurrentTime,
Data = DetectedObjects.ToArray(),
});
}
foreach (var obj in DetectedObjects)
{
var min = obj.Position - obj.Scale / 2;
var max = obj.Position + obj.Scale / 2;
var color = string.Equals(obj.Label, "Pedestrian") ? Color.yellow : Color.green;
AAWireBoxes.DrawBox(min, max, color);
}
var cmd = CommandBufferPool.Get();
Camera.Render();
AAWireBoxes.Draw(cmd);
AAWireBoxes.Clear();
HDRPUtilities.ExecuteAndClearCommandBuffer(cmd);
CommandBufferPool.Release(cmd);
}
protected void RenderCamera()
{
var cmd = CommandBufferPool.Get();
var hd = HDCamera.GetOrCreate(SensorCamera);
if (renderTarget.IsCube && !HDAdditionalCameraData.hasCustomRender)
{
// HDRP renders cubemap as multiple separate images, each with different exposure.
// Locking exposure will force it to use the same value for all faces, removing inconsistencies.
hd.LockExposure();
SensorCamera.stereoSeparation = 0f;
SensorCamera.RenderToCubemap(renderTarget, faceMask, Camera.MonoOrStereoscopicEye.Left);
hd.UnlockExposure();
}
else
{
SensorCamera.Render();
}
if (Distorted)
{
if (Fisheye)
{
LensDistortion.UnifiedProjectionDistort(cmd, renderTarget, DistortedHandle);
}
else
{
LensDistortion.PlumbBobDistort(cmd, renderTarget, DistortedHandle);
}
cmd.SetGlobalVector(ScreenSizeProperty, new Vector4(Width, Height, 1.0f / Width, 1.0f / Height));
var ctx = new PostProcessPassContext(cmd, hd, DistortedHandle);
SimulatorManager.Instance.Sensors.PostProcessSystem.RenderLateForSensor(ctx, this);
}
FinalRenderTarget.BlitTo2D(cmd, hd);
HDRPUtilities.ExecuteAndClearCommandBuffer(cmd);
CommandBufferPool.Release(cmd);
}
public bool Save(string path)
{
int rotationCount = Mathf.CeilToInt(360.0f / HorizontalAngleLimit);
float minAngle = 360.0f / CurrentMeasurementsPerRotation;
int count = (int)(HorizontalAngleLimit / minAngle);
float angle = HorizontalAngleLimit / 2.0f;
var active = new ReadRequest[rotationCount];
var cmd = CommandBufferPool.Get();
try
{
for (int i = 0; i < rotationCount; i++)
{
var rotation = Quaternion.AngleAxis(angle, Vector3.up);
SensorCamera.transform.localRotation = rotation;
BeginReadRequest(count, ref active[i]);
angle += HorizontalAngleLimit;
if (angle >= 360.0f)
{
angle -= 360.0f;
}
}
for (int i = 0; i < rotationCount; i++)
{
EndReadRequest(cmd, active[i]);
}
}
finally
{
HDRPUtilities.ExecuteAndClearCommandBuffer(cmd);
CommandBufferPool.Release(cmd);
Array.ForEach(active, req => AvailableRenderTextures.Push(req.TextureSet));
}
PointCloudBuffer.GetData(Points);
var worldToLocal = LidarTransform;
if (Compensated)
{
worldToLocal = worldToLocal * transform.worldToLocalMatrix;
}
try
{
using (var writer = new PcdWriter(path))
{
for (int p = 0; p < Points.Length; p++)
{
var point = Points[p];
if (point != Vector4.zero)
{
writer.Write(worldToLocal.MultiplyPoint3x4(point), point.w);
}
}
;
}
return(true);
}
catch (Exception ex)
{
Debug.LogException(ex);
return(false);
}
}
public Vector4[] Capture()
{
Debug.Assert(Compensated); // points should be in world-space
int rotationCount = Mathf.CeilToInt(360.0f / HorizontalAngleLimit);
float minAngle = 360.0f / CurrentMeasurementsPerRotation;
int count = (int)(HorizontalAngleLimit / minAngle);
float angle = HorizontalAngleLimit / 2.0f;
var textures = new Texture2D[rotationCount];
var cmd = CommandBufferPool.Get();
var rt = RenderTexture.active;
try
{
for (int i = 0; i < rotationCount; i++)
{
var rotation = Quaternion.AngleAxis(angle, Vector3.up);
SensorCamera.transform.localRotation = rotation;
var req = new ReadRequest();
if (BeginReadRequest(count, ref req))
{
RenderTexture.active = req.TextureSet.ColorTexture;
Texture2D texture;
if (AvailableTextures.Count > 0)
{
texture = AvailableTextures.Pop();
}
else
{
texture = new Texture2D(RenderTextureWidth, RenderTextureHeight, TextureFormat.RGBA32, false, true);
}
texture.ReadPixels(new Rect(0, 0, RenderTextureWidth, RenderTextureHeight), 0, 0);
textures[i] = texture;
EndReadRequest(cmd, req);
AvailableRenderTextures.Push(req.TextureSet);
}
angle += HorizontalAngleLimit;
if (angle >= 360.0f)
{
angle -= 360.0f;
}
}
}
finally
{
HDRPUtilities.ExecuteAndClearCommandBuffer(cmd);
CommandBufferPool.Release(cmd);
RenderTexture.active = rt;
Array.ForEach(textures, AvailableTextures.Push);
}
PointCloudBuffer.GetData(Points);
return(Points);
}
public virtual void Update()
{
if (LaserCount != CurrentLaserCount ||
MeasurementsPerRotation != CurrentMeasurementsPerRotation ||
FieldOfView != CurrentFieldOfView ||
CenterAngle != CurrentCenterAngle ||
MinDistance != CurrentMinDistance ||
MaxDistance != CurrentMaxDistance ||
!Enumerable.SequenceEqual(VerticalRayAngles, CurrentVerticalRayAngles))
{
if (MinDistance > 0 && MaxDistance > 0 && LaserCount > 0 && MeasurementsPerRotation >= (360.0f / HorizontalAngleLimit))
{
Reset();
}
}
UpdateMarker.Begin();
var cmd = CommandBufferPool.Get();
foreach (var req in Active)
{
if (!req.TextureSet.IsValid(RenderTextureWidth, RenderTextureHeight))
{
// lost render texture, probably due to Unity window resize or smth
req.TextureSet.Release();
}
else
{
EndReadRequest(cmd, req);
AvailableRenderTextures.Push(req.TextureSet);
if (req.Index + req.Count >= CurrentMeasurementsPerRotation)
{
SendMessage();
}
}
}
HDRPUtilities.ExecuteAndClearCommandBuffer(cmd);
CommandBufferPool.Release(cmd);
Active.Clear();
if (IgnoreNewRquests > 0)
{
IgnoreNewRquests -= Time.unscaledDeltaTime;
}
else
{
float minAngle = 360.0f / CurrentMeasurementsPerRotation;
AngleDelta += Time.deltaTime * 360.0f * RotationFrequency;
int count = Mathf.CeilToInt(HorizontalAngleLimit / minAngle);
while (AngleDelta >= HorizontalAngleLimit)
{
float angle = AngleStart + HorizontalAngleLimit / 2.0f;
var rotation = Quaternion.AngleAxis(angle, Vector3.up);
SensorCamera.transform.localRotation = rotation;
if (Top != null)
{
Top.transform.localRotation = rotation;
}
var req = new ReadRequest();
if (BeginReadRequest(count, ref req))
{
req.AngleStart = AngleStart;
DateTime dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(SimulatorManager.Instance.CurrentTime * 1000.0)).UtcDateTime;
DateTime startHour = new DateTime(dt.Year, dt.Month,
dt.Day, dt.Hour, 0, 0);
TimeSpan timeOverHour = dt - startHour;
req.TimeStamp = (uint)(timeOverHour.Ticks / (TimeSpan.TicksPerMillisecond) * 1000);
Active.Add(req);
}
AngleDelta -= HorizontalAngleLimit;
AngleStart += HorizontalAngleLimit;
if (AngleStart >= 360.0f)
{
AngleStart -= 360.0f;
}
}
}
UpdateMarker.End();
}
