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