/**
* 扫描点云
*/
public LiDarData LaserScan()
{
LiDarData phitPoints = new LiDarData();
// 手动控制扫描点云数据
// 计算旋转角度
// rotationAnglePerStep = 0;
float phorizontalAngle = 0;
int lineId = 0;
for (int step = 0; step < 120; step++)
{
// 进行旋转
// rotationAnglePerStep
transform.Rotate(0, rotationAnglePerStep, 0);
phorizontalAngle += rotationAnglePerStep;
LiDarLine line = new LiDarLine();
line.LineID = lineId++;
int laserid = 0;
foreach (Laser laser in lasers)
{
RaycastHit hit = laser.ShootRay();
float distance = hit.distance;
if (distance != 0) // Didn't hit anything, don't add to list.
{
float verticalAngle = laser.GetVerticalAngle();
LiDarMsg.Laser laserVal = new LiDarMsg.Laser();
laserVal.LaId = laserid++;
laserVal.Radius = distance;
laserVal.Pitch = verticalAngle;
laserVal.Yaw = phorizontalAngle;
line.LaserData.Add(laserVal);
}
}
phitPoints.LineData.Add(line);
}
return(phitPoints);
}
private void FixedUpdate()
{
hits = new LinkedList <SphericalCoordinate>();
hitPoints = new LiDarData();
// Do nothing, if the simulator is paused.
if (!isPlaying)
{
return;
}
// return;
// Check if number of steps is greater than possible calculations by unity.
float numberOfStepsNeededInOneLap = 360 / Mathf.Abs(rotationAnglePerStep);
float numberOfStepsPossible = 1 / Time.fixedDeltaTime / 5;
float precalculateIterations = 1;
// Check if we need to precalculate steps.
if (numberOfStepsNeededInOneLap > numberOfStepsPossible)
{
precalculateIterations = (int)(numberOfStepsNeededInOneLap / numberOfStepsPossible);
if (360 % precalculateIterations != 0)
{
precalculateIterations += 360 % precalculateIterations;
}
}
// Check if it is time to step. Example: 2hz = 2 rotations in a second.
if (Time.fixedTime - lastUpdate > (1 / (numberOfStepsNeededInOneLap) / rotationSpeedHz) * precalculateIterations)
{
// Update current execution time.
lastUpdate = Time.fixedTime;
for (int i = 0; i < precalculateIterations; i++)
{
// Perform rotation.
transform.Rotate(0, rotationAnglePerStep, 0);
// 进行旋转
horizontalAngle += rotationAnglePerStep; // Keep track of our current rotation.
if (horizontalAngle >= 360)
{
horizontalAngle -= 360;
//GameObject.Find("RotSpeedText").GetComponent<Text>().text = "" + (1/(Time.fixedTime - lastLapTime));
lastLapTime = Time.fixedTime;
HitPointsTrans(lastLapTime, hitPointsStore);
hitPointsStore = new LiDarData();
lineId = 0;
}
LiDarLine line = new LiDarLine();
line.LineID = lineId++;
// Execute lasers.
int laserid = 0;
foreach (Laser laser in lasers)
{
RaycastHit hit = laser.ShootRay();
float distance = hit.distance;
if (distance != 0) // Didn't hit anything, don't add to list.
{
float verticalAngle = laser.GetVerticalAngle();
LiDarMsg.Laser laserVal = new LiDarMsg.Laser();
laserVal.LaId = laserid++;
laserVal.Radius = distance;
laserVal.Pitch = verticalAngle;
laserVal.Yaw = horizontalAngle;
line.LaserData.Add(laserVal);
hits.AddLast(new SphericalCoordinate(distance, verticalAngle, horizontalAngle, hit.point, laser.GetLaserId()));
}
}
hitPoints.LineData.Add(line);
}
hitPointsStore.LineData.AddRange(hitPoints.LineData);
// Notify listeners that the lidar sensor have scanned points.
if (OnScanned != null && pointCloudObject != null && pointCloudObject.activeInHierarchy)
{
OnScanned(lastLapTime, hits);
}
}
}
