// bool donescan = false; private void insert(float distance, int j, float currentangle) { blockLoc = 0 + block * (16 * 6 + 4); if (blockLoc >= 1200) { ranges[1205] = 34; ranges[1204] = 54; System.Buffer.BlockCopy(BitConverter.GetBytes(fireTime), 0, ranges, 1200, 4); fireTime += 1333; // Debug.Log(BitConverter.GetBytes(fireTime)[3]); sendObj.sendData(ranges); // Debug.Log(ranges); // Debug.Log(BitConverter.IsLittleEndian); blockLoc = 0; block = 0; // donescan = true; encstamped = false; // Debug.Log("sent"); } // ushort dist = 22921; ushort dist = (ushort)Mathf.RoundToInt(distance / 0.002f); // ushort dist = (ushort)2000; if (j == 0 && !encstamped) //block stamp { ranges[blockLoc] = 255; ranges[blockLoc + 1] = 238; System.Buffer.BlockCopy(BitConverter.GetBytes((ushort)Mathf.RoundToInt(currentangle * 100)), 0, ranges, blockLoc + 2, 2); // Debug.Log((ushort)Mathf.RoundToInt(currentangle * 100)); encstamped = true; } int putInArray = j <= 7 ? j * 2 : (j - 8) * 2 + 1; if (block < 12) { System.Buffer.BlockCopy(BitConverter.GetBytes(dist), 0, ranges, blockLoc + 4 + putInArray * 3 + dataIndex * 16 * 3, 2); ranges[blockLoc + 6 + putInArray * 3 + dataIndex * 16 * 3] = 0; // intensity } if (j == 15 && dataIndex == 0) { dataIndex = 1; } else if (j == 15 && dataIndex == 1) { encstamped = false; dataIndex = 0; block++; } }
// Update is called once per frame void FixedUpdate() { ScannerLoc = myref.position; velocity = (ScannerLoc - prevScannerLoc) / Time.fixedDeltaTime; //message floats format: [number of rays,number of columns, sensor Angle, min Vert Angle, vertRes,HorRes,x,y,z,rx,ry,rz[ranges]] in ros coordinates x forward y left ranges = new float[NoOfScansPerFrame * lasercount + dataOffset]; byte[] data = new byte[ranges.Length * 4];//char[ranges.Length * 4]; if (currentangle > HorScanAngRange / 2) { currentangle = -HorScanAngRange / 2; SensorRotator.localEulerAngles = new Vector3(0, -HorScanAngRange / 2, 0); // if (!ROS && !UDP) Points = PointsTemp; // if (!ROS && !UDP) PointsTemp.Clear(); // if (ROS){ // SimulationManager.instance.ros.Publish("velodyne/string", rosMsg); // } // rosData = ""; } //completed horizontal scan ranges[0] = lasercount; ranges[1] = NoOfScansPerFrame; ranges[2] = currentangle * Mathf.Deg2Rad; ranges[3] = StartVerticalAngle * Mathf.Deg2Rad; ranges[4] = ((VertScanAngRange) / (lasercount - 1)) * Mathf.Deg2Rad; ranges[5] = HorRes * Mathf.Deg2Rad; Vector3 rosVector = RosUnityCoordinates.UnityToRosPositionAxisConversion(myref.position); Quaternion rosRot = RosUnityCoordinates.UnityToRosRotationAxisConversion(myref.rotation); ranges[6] = rosVector.x; ranges[7] = rosVector.y; ranges[8] = rosVector.z; ranges[9] = rosRot.x; ranges[10] = rosRot.y; ranges[11] = rosRot.z; ranges[12] = rosRot.w; ranges[13] = HorScanAngRange * Mathf.Deg2Rad; intLoc = ScannerLoc + velocity * Time.fixedDeltaTime; for (int i = 0; i < NoOfScansPerFrame; i++) { // multiple horizontal scans in 1 physics step in order to achieve the full range in the desired rate if (InterpolateLocation) { ScannerLoc = Vector3.Lerp(myref.position, intLoc, (float)i / NoOfScansPerFrame); } if (currentangle > HorScanAngRange / 2) { currentangle = -HorScanAngRange / 2; SensorRotator.localEulerAngles = new Vector3(0, -HorScanAngRange / 2, 0); } //rotate horizontally emitter.localEulerAngles = new Vector3(-StartVerticalAngle, 0, 0); for (int j = 0; j < lasercount; j++) //the lazer column { pitchAngle = (StartVerticalAngle + j * VertScanAngRange / (lasercount - 1)); emitter.localEulerAngles = new Vector3(pitchAngle, 0, 0); shootLaserDir = (emitter.forward); RaycastHit hit; if (Physics.Raycast(ScannerLoc, shootLaserDir, out hit, Range)) { Vector3 p = hit.point; if (!ROS) { PointsTemp.Add(p); } if (DebugDraw) { Debug.DrawLine(p, SensorRotator.position, Color.red, DrawTime, true); } else if (DebugDrawDots) { Debug.DrawLine(p, p + 0.1f * Vector3.up, Color.red, DrawTime, true); } if (ROS || UDP) { ranges[j + i * lasercount + dataOffset] = hit.distance; } } else { if (ROS || UDP) { ranges[j + i * lasercount + dataOffset] = 0; } if (DebugDraw) { Debug.DrawLine(SensorRotator.position, SensorRotator.position + shootLaserDir * Range, Color.blue, 0.3f, true); } } // datacolumn[j]=hit.distance; } // WriteData(datacolumn); currentangle += HorRes; SensorRotator.localEulerAngles = new Vector3(0, currentangle, 0); } if (ROS) { System.Buffer.BlockCopy(ranges, 0, data, 0, data.Length); string d = (data.ToString()); Debug.Log(d); wsc.Publish("velodyne", d); } if (UDP && !ROS) { System.Buffer.BlockCopy(ranges, 0, data, 0, data.Length); sendObj.sendData(data); // Debug.Log(data.Length); } prevScannerLoc = SensorRotator.position; }