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); }
private TiMDataMessage ParseTimDataMessage(string[] argList) { TiMDataMessage tms = new TiMDataMessage(); int pos = 1; if (argList[pos++] != "LMDscandata") { return(tms); } tms.VersionNumber = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.DeviceNumber = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.SerialNumber = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.DeviceStatus1 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.DeviceStatus2 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.TelegramCounter = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.ScanCounter = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.TimeSinceStartUp = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.TimeOfTransmission = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.InputStatus1 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.InputStatus2 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.OutputStatus1 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.OutputStatus2 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.ReservedByteA = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.ScanningFrequency = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.MeasurementFrequency = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.NumberOfEncoders = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.NumberOf16BitsChannels = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.MeasuredDataContents = argList[pos++]; tms.ScalingFactor = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.ScalingOffset = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.StartingAngle = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.AngularStepWidth = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.NumberOfData = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.DistanceData = new List <int>(); tms.AngleData = new List <double>(); tms.RssiData = new List <int>(); tms.RssiDataPercent = new List <double>(); double angleOffset = -900000; // Pour placer le zéro d'angle en face du LIDAR for (int i = 0; i < tms.NumberOfData; i++) { double angle = (tms.StartingAngle + angleOffset + i * tms.AngularStepWidth) / 10000.0; if (IsUpsideDown) // Inverse les angles si le Lidar est retourné { angle *= -1; } tms.AngleData.Add(angle); tms.DistanceData.Add(int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber)); } tms.RssiDataType = argList[pos++]; tms.ScalingFactor = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.ScalingOffset = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.OutputStatus1 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.OutputStatus2 = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.NumberOfDataRssi = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); for (int i = 0; i < tms.NumberOfDataRssi; i++) { tms.RssiData.Add(int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber)); double calculatePercentRssi = tms.RssiData[i] / 65536.0 * 100.0;//(((byte)(tms.RssiData[i])) / 255.0) * 100; tms.RssiDataPercent.Add(calculatePercentRssi); } //double maxvalue = tms.RssiData.Max(); int outputOfPosition = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); //RSSI missing int deviceName = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber);; //RSSI missing : not really clear in the datasheet tms.Comment = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); if (tms.Comment == 1) { pos++; //int CommentLenght = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); pos++; //int CommentTransmit = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); } pos++;//tms.TimeInformation = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); tms.EventInformation = int.Parse(argList[pos++], System.Globalization.NumberStyles.HexNumber); return(tms); }