private void ProcessReceivedData(TiMDataMessage TimData)
{
lock (LidarPoints)
{
LidarPoints.Clear();
LidarPolarPointsList.Clear();
double angleLidar = Toolbox.DegToRad(Location.Theta);
for (int i = 0; i < TimData.AngleData.Count; i++)
{
if (TimData.AngleData[i] >= AngleMin && TimData.AngleData[i] <= AngleMax) // Filtre d'angle
{
double distance = TimData.DistanceData[i] / 1000.0;
double angle = Toolbox.DegToRad(TimData.AngleData[i]);
//On trouve les coordonnées du point brut en cartésien dans le ref du Lidar
double xRefRobot = Location.X + distance * Math.Cos(angle + angleLidar);
double yRefRobot = Location.Y + distance * Math.Sin(angle + angleLidar);
//On trouve les coordonnées du point en polaire dans le ref du robot
double distanceCentreRobot =
Math.Sqrt(Math.Pow(xRefRobot, 2) +
Math.Pow(yRefRobot, 2));
double angleAxeRobot;
if (yRefRobot != 0 && xRefRobot != 0)
{
angleAxeRobot = Math.Atan2(yRefRobot, xRefRobot);
}
else
{
angleAxeRobot = angle;
}
PolarPointRssi pt = new PolarPointRssi(angleAxeRobot, distance, TimData.RssiData[i]);
//LidarPoint point = new LidarPoint(distanceCentreRobot, angleAxeRobot, TimData.RssiData[i]);
// Supression des points en dehors des limites hardware
//if (distance >= 0.01 && distance <= 10.01) // Filtre de distance
{
//LidarPoints.Add(point);
LidarPolarPointsList.Add(pt);
}
}
}
}
//OnLidarPointsReady();
OnLidarDecodedFrame(robotId, LidarPolarPointsList);
}
/// <summary>
/// RAN IN A SEPERATE THREAD
/// </summary>
private void PollLidarPoints()
{
try
{
ObjectDescriptor[] lidarSensorDescriptions = new ObjectDescriptor[lidarDisplays.Length];
for (int i = 0; i < lidarDisplays.Length; i++)
{
lidarSensorDescriptions[i] = anvelConnection.GetObjectDescriptorByName(lidarDisplays[i].sensorName);
}
ObjectDescriptor vehicle = anvelConnection.GetObjectDescriptorByName(vehicleName);
LidarPoints[] allPoints = new LidarPoints[lidarDisplays.Length];
Vector3[] offsets = new Vector3[lidarDisplays.Length];
int totalNumberOfPoints = 0;
float lowestPoint = float.MaxValue;
float highestPoint = float.MinValue;
while (true)
{
Point3 vehiclePosition = anvelConnection.GetPoseAbs(vehicle.ObjectKey).Position;
// anvelConnection.
totalNumberOfPoints = 0;
Vector3 lastPos = Vector3.forward * 1000000;
for (int i = 0; i < lidarDisplays.Length; i++)
{
allPoints[i] = anvelConnection.GetLidarPoints(lidarSensorDescriptions[i].ObjectKey, 0);
totalNumberOfPoints += allPoints[i].Points.Count;
if (anvelConnection.GetProperty(lidarSensorDescriptions[i].ObjectKey, "Lidar Global Frame") == "true")
{
offsets[i] = new Vector3((float)vehiclePosition.Y, (float)vehiclePosition.Z, (float)vehiclePosition.X);
}
}
var newParticles = new ParticleSystem.Particle[totalNumberOfPoints];
int particleIndex = 0;
for (int lidarIndex = 0; lidarIndex < lidarDisplays.Length; lidarIndex++)
{
for (int pointIndex = 0; pointIndex < allPoints[lidarIndex].Points.Count; pointIndex++)
{
Vector3 position = ModifiedPositionFromRotationalOffset(new Vector3(
-(float)allPoints[lidarIndex].Points[pointIndex].Y,
(float)allPoints[lidarIndex].Points[pointIndex].Z,
(float)allPoints[lidarIndex].Points[pointIndex].X
) - offsets[lidarIndex], Vector3.zero, rotationOffset) + centerOffset;
if (position.y > highestPoint)
{
highestPoint = position.y;
}
if (position.y < lowestPoint)
{
lowestPoint = position.y;
}
var colorToRender = lidarDisplays[lidarIndex].renderColor;
if (Mathf.Abs(highestPoint - lowestPoint) > 0.001f)
{
float H;
float S;
float V;
Color.RGBToHSV(colorToRender, out H, out S, out V);
var p = (position.y - lowestPoint) / (highestPoint - lowestPoint);
colorToRender = Color.HSVToRGB(H, p, p);
colorToRender.a = lidarDisplays[lidarIndex].renderColor.a;
}
if ((position - lastPos).sqrMagnitude > .1)
{
newParticles[particleIndex] = new ParticleSystem.Particle
{
remainingLifetime = float.MaxValue,
position = position,
startSize = .5f,
startColor = colorToRender
};
}
lastPos = position;
particleIndex++;
}
}
particles = newParticles;
}
}
catch (AnvelException e)
{
Debug.Log(string.Format("Anvel Exception: {0} at {1}", e.ErrorMessage, e.Source));
throw;
}
catch (System.Exception e)
{
Debug.LogFormat("{0}:{1}", e.GetType(), e.Message);
throw;
}
}
