Example #1
0
    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];
    }
Example #2
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);
        }
    }
Example #3
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");
    }