// 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;
}
