private void VelodyneCalcAndSendData()
{
hAng = -horizontalFOV / 2.0f;
for (int i = 0; i < m_ResWidth; i++) //columns
{
float vAng = HigherAngle;
for (int j = 0; j < Channels; j++) //rows
{
float range = (m_Ranges[j * m_ResWidth + i].r * MeasurementRange) / (Mathf.Cos(hAng * Mathf.Deg2Rad) * Mathf.Cos(vAng * Mathf.Deg2Rad));
if (range >= MeasurementRange)
{
range = 0;
}
if (sendDataOnICD)
{
vel16ICDinterface.SetChannel((double)range, 0);
}
if (DrawLidar)
{
Vector3 rangePointPos = Sensor.transform.position + Quaternion.AngleAxis(vAng, Sensor.transform.right) * Quaternion.AngleAxis(hAng, Sensor.transform.up) * Sensor.transform.forward * range;
Debug.DrawLine(rangePointPos, rangePointPos + Vector3.up * drawSize, drawColor, drawTime);
}
vAng = vAng - m_VerticalAngularResolution;
}
hAng = hAng + AngularResolution;
if (sendDataOnICD)
{
float columnAng = Mathf.Repeat(-horizontalFOV / 2.0f + cameraRotationAngle + i * AngularResolution, 360.0f);
// displayText.text += " " + columnAng.ToString();
vel16ICDinterface.SetAzimuth((double)columnAng);
vel16ICDinterface.SetTimeStamp(Time.fixedTime);
vel16ICDinterface.CloseBlock();
m_BlocksCounter++;
if (m_BlocksCounter == BLOCKS_ON_PACKET)
{
vel16ICDinterface.SendData();
m_BlocksCounter = 0;
// displayText.text = "";
}
}
}
m_Ranges = new Color[0];
}
// Update is called once per frame
void FixedUpdate()
{
physics_StepTime = Time.fixedDeltaTime;
horFOV_ScanTime = 1.0f / ScaningFreqHZ;
ScanColumnsPerPhysicStep = (int)((HorFOV / HorRes) * (physics_StepTime / horFOV_ScanTime));
verRes = VerFOV / (ScaningPlaines - 1);
if (horCurrentAngle > HorFOV) //completed horizontal scan
{
horCurrentAngle = 0;
SensorRotator.localEulerAngles = new Vector3(0, 0, 0);
}
Vector3 ScannerVel = myref.InverseTransformVector(rb.velocity);
Vector3 scanerPos = myref.position;
Vector3 ScanerLinearStep = ScannerVel * physics_StepTime;
for (int i = 0; i < ScanColumnsPerPhysicStep; i++) // multiple horizontal scans in 1 physics step in order to achieve the full range in the desired rate
{
if (InterpolateLocation)
{
scanerPos = scanerPos + ScanerLinearStep * i / ScanColumnsPerPhysicStep;
}
if (horCurrentAngle > HorFOV)
{
horCurrentAngle = 0;
SensorRotator.localEulerAngles = new Vector3(0, 0, 0);
}
for (int j = ScaningPlaines - 1; j >= 0; j--) //the lazer column
{
float verCurentAngle = (StartVerticalAngle + j * verRes);
emitter.localEulerAngles = new Vector3(verCurentAngle, 0, 0);
Vector3 shootLaserDir = (emitter.forward);
RaycastHit hit;
if (Physics.Raycast(scanerPos, shootLaserDir, out hit, MaxRange))
{
vc.SetChannel(hit.distance, 0);
if (DebugDraw)
{
Debug.DrawLine(hit.point, hit.point + 0.1f * Vector3.up, Color.red, DrawTime, true);
}
}
else
{
vc.SetChannel(0, 0);
}
}
vc.SetAzimuth(horCurrentAngle);
vc.SetTimeStamp(Time.fixedTime);
vc.CloseBlock();
blocksCounter++;
if (blocksCounter == BLOCKS_ON_PACKET)
{
vc.SendData();
blocksCounter = 0;
}
horCurrentAngle = horCurrentAngle + HorRes;
SensorRotator.localEulerAngles = new Vector3(0, horCurrentAngle, 0);
}
}
void FixedUpdate()
{
RenderTexture currentActiveRT = RenderTexture.active;
for (int s = 0; s < SuperSample; s++)
{
RenderTexture.active = depthCam.targetTexture; // When a RenderTexture becomes active its hardware rendering context is created
depthCam.Render();
RangesSamples.ReadPixels(new Rect(0, 0, ResWidth, ResHeight), 0, 0); // copy a rectangular pixel area from the currently active RenderTexture
RangesSamples.Apply();
ranges = RangesSamples.GetPixels();
float hAng = -horizontalFOV / 2.0f;
for (int i = 0; i < ResWidth; i++) //columns
{
float vAng = HigherAngle;
for (int j = 0; j < Channels; j++) //rows
{
float range = (ranges[j * ResWidth + i].r * MeasurementRange) / (Mathf.Cos(hAng * Mathf.Deg2Rad) * Mathf.Cos(vAng * Mathf.Deg2Rad));
if (range >= MeasurementRange)
{
range = 0;
}
if (sendDataOnICD)
{
vel16ICDinterface.SetChannel((double)range, 0);
}
if (DrawLidar)
{
Vector3 rangePointPos = Sensor.transform.position + Quaternion.AngleAxis(vAng, Sensor.transform.right) * Quaternion.AngleAxis(hAng, Sensor.transform.up) * Sensor.transform.forward * range;
Debug.DrawLine(rangePointPos, rangePointPos + Vector3.up * drawSize, drawColor, drawTime);
}
vAng = vAng - verticalAngularResolution;
}
hAng = hAng + AngularResolution;
if (sendDataOnICD)
{
float columnAng = Mathf.Repeat(-horizontalFOV / 2.0f + cameraRotationAngle + i * AngularResolution, 360.0f);
// displayText.text += " " + columnAng.ToString();
vel16ICDinterface.SetAzimuth((double)columnAng);
vel16ICDinterface.SetTimeStamp(Time.fixedTime);
vel16ICDinterface.CloseBlock();
blocksCounter++;
if (blocksCounter == BLOCKS_ON_PACKET)
{
vel16ICDinterface.SendData();
blocksCounter = 0;
// displayText.text = "";
}
}
}
if (Rotate)
{
cameraRotationAngle += horizontalFOV;
}
Sensor.transform.localEulerAngles = new Vector3(-(HigherAngle + LowerAngle) / 2.0f, cameraRotationAngle, 0);
}
RenderTexture.active = currentActiveRT;
displayText.text = "Velodyne: Freq[Hz]=" + RotateFrequency.ToString() + " \n" +
"vFOV[deg]=" + verticalFOV.ToString("0.00") + " vRes[deg]= " + verticalAngularResolution.ToString("0.00") + "\n" +
"hFOV[deg]=" + RotationAngle.ToString("0.00") + " hRes[deg]= " + AngularResolution.ToString("0.00");
}