//double angleIncrement; private void LidarStartAndAcquire(double freq, R2000SamplingRate samplingRate) { using (r2000 = new R2000Scanner(IPAddress.Parse("169.254.235.44"), R2000ConnectionType.TCPConnection)) { r2000.Connect(); r2000.SetSamplingRate(R2000SamplingRate._8kHz); r2000.SetScanFrequency(freq); r2000.SetSamplingRate(samplingRate); r2000.DisplayMessage(1, "Points :"); r2000.DisplayMessage(2, "50 points"); //angleIncrement = 2 * Math.PI/((double)R2000SamplingRate._252kHz / 20); r2000.OnlyStatusEvents().Subscribe(ev => { var oldColor = Console.ForegroundColor; Console.ForegroundColor = ConsoleColor.Blue; Console.WriteLine($"Event: {ev.Level.ToString()} / {ev.Message}"); Console.ForegroundColor = oldColor; }); r2000.OnlyLidarPoints() .BufferByScan() .Subscribe(x => { //lastLidarPtList = new List<PolarPoint>(); //lastLidarPtList.Add(new PolarPoint(x.Points[0].Distance / 1000, Utilities.Toolbox.DegToRad(x.Points[0].Azimuth))); //for (int i=1; i< x.Points.Count; i++ ) //{ // if() //} lastLidarPtList = new List <PolarPointRssi>(x.Points.Select(pt => new PolarPointRssi(Utilities.Toolbox.DegToRad(pt.Azimuth), pt.Distance / 1000, pt.Amplitude))); lastScanNumber = (int)x.Scan; newLidarDataAvailable = true; //Console.WriteLine($"Got {x.Count} points for scan {x.Scan} / Min {x.Points.Min(pt => pt.Azimuth)} :: Max {x.Points.Max(pt => pt.Azimuth)}"); }); r2000.StartScan(); while (true) { Thread.Sleep(5); } } }
private void LidarStartAndAcquire(double freq, R2000SamplingRate samplingRate) { while (true) { /// Si le lidar n'est pas connecté, on tente de le connecter if (!isLidarConnected) { Console.WriteLine("Trying to connect Lidar R2000"); try { var r2000 = new R2000Scanner(IPAddress.Parse("169.254.235.44"), R2000ConnectionType.TCPConnection); //using (r2000 = new R2000Scanner(IPAddress.Parse("169.254.235.44"), R2000ConnectionType.TCPConnection)) { var isLidarReachable = r2000.Connect(); if (isLidarReachable) { r2000.SetSamplingRate(R2000SamplingRate._8kHz); r2000.SetScanFrequency(freq); r2000.SetSamplingRate(samplingRate); //angleIncrement = 2 * Math.PI/((double)R2000SamplingRate._252kHz / 20); r2000.OnlyStatusEvents().Subscribe(ev => { var oldColor = Console.ForegroundColor; Console.ForegroundColor = ConsoleColor.Blue; Console.WriteLine($"Event: {ev.Level.ToString()} / {ev.Message}"); Console.ForegroundColor = oldColor; }); r2000.OnlyLidarPoints() .BufferByScan() .Subscribe(x => { lastLidarPtList = new List <PolarPointRssi>(x.Points.Select(pt => new PolarPointRssi(Utilities.Toolbox.DegToRad(pt.Azimuth), pt.Distance / 1000, pt.Amplitude))); lastScanNumber = (int)x.Scan; newLidarDataAvailable = true; }); r2000.StartScan(); Console.WriteLine("Lidar R2000 Connected"); isLidarConnected = true; } } } catch (IOException e) { Console.WriteLine(e); isLidarConnected = false; } //using (r2000 = new R2000Scanner(IPAddress.Parse("169.254.235.44"), R2000ConnectionType.TCPConnection)) //{ // r2000.Connect(); // r2000.SetSamplingRate(R2000SamplingRate._8kHz); // r2000.SetScanFrequency(freq); // r2000.SetSamplingRate(samplingRate); // r2000.DisplayMessage(1, "Points :"); // r2000.DisplayMessage(2, "50 points"); // r2000.OnlyStatusEvents().Subscribe(ev => // { // var oldColor = Console.ForegroundColor; // Console.ForegroundColor = ConsoleColor.Blue; // Console.WriteLine($"Event: {ev.Level.ToString()} / {ev.Message}"); // Console.ForegroundColor = oldColor; // }); // r2000.OnlyLidarPoints() // .BufferByScan() // .Subscribe(x => // { // lastLidarPtList = new List<PolarPointRssi>(x.Points.Select(pt => new PolarPointRssi(Utilities.Toolbox.DegToRad(pt.Azimuth), pt.Distance / 1000, pt.Amplitude))); // lastScanNumber = (int)x.Scan; // newLidarDataAvailable = true; // }); // r2000.StartScan(); // while (true) // { // Thread.Sleep(5); // } //} } Thread.Sleep(2000); } }
static void Main(string[] args) { //SweepTest(); using (var r2000 = new R2000Scanner(IPAddress.Parse("192.168.1.214"), R2000ConnectionType.TCPConnection)) { r2000.Connect(); r2000.SetSamplingRate(R2000SamplingRate._8kHz); r2000.SetScanFrequency(10); r2000.SetSamplingRate(R2000SamplingRate._180kHz); r2000.OnlyStatusEvents().Subscribe(ev => { var oldColor = Console.ForegroundColor; Console.ForegroundColor = ConsoleColor.Yellow; Console.WriteLine($"Event: {ev.Level.ToString()} / {ev.Message}"); Console.ForegroundColor = oldColor; }); r2000.OfType <R2000Status>().Subscribe(_ => { Console.WriteLine($"R2000 status:"); Console.WriteLine($"\tScan Frequency {_.CurrentScanFrequency} Hz"); Console.WriteLine($"\tUptime {_.Uptime} min"); Console.WriteLine($"\tTemperature {_.CurrentTemperature} °C"); Console.WriteLine($"\tSystem load {_.SystemLoad}%"); Console.WriteLine("--------------------------------------------"); }); r2000.OnlyLidarPoints() .BufferByScan() .Subscribe(x => { //Console.WriteLine($"Scans per second: {x.Count}"); Console.WriteLine($"Got {x.Count} points for scan {x.Scan} / Min {x.Points.Min(pt => pt.Azimuth)} :: Max {x.Points.Max(pt => pt.Azimuth)}"); }); /* * r2000.OnlyLidarPoints() * .BufferByScan() * .Where(x => x.Count < 25200) * .Subscribe(x => * { * //Console.WriteLine($"Scans per second: {x.Count}"); * Console.WriteLine($"Got {x.Count} points for scan {x.Scan} / Min {x.Points.Min(pt => pt.Azimuth)} :: Max {x.Points.Max(pt => pt.Azimuth)}"); * * var gaps = new List<LidarPoint>(); * * LidarPoint lastPoint = x.Points.First(); * var expectedIncr = 360f / 25150; * * foreach(var pt in x.Points.Skip(1)) * { * var diff = pt.Azimuth - lastPoint.Azimuth - expectedIncr; * * if (diff > 0) * { * gaps.Add(lastPoint); * gaps.Add(pt); * } * * lastPoint = pt; * } * * if (gaps.Count > 0) * { * Console.WriteLine("Gaps: " + * gaps.Skip(1).Aggregate( * gaps.First().Azimuth.ToString(), * (acc, pt) => acc + ", " + pt.Azimuth)); * * } * });*/ r2000.OnlyLidarPoints() .Where(x => x.Distance >= 400 && x.Distance <= 1200) .PointsInAzimuthRange(-45, 45) .BufferByScan() .Subscribe(x => { Console.WriteLine($"Distance: {x.Points.Average(y => y.Distance)} / points {x.Count}"); }); r2000.StartScan(); while (true) { var line = Console.ReadLine(); if (line == "q") { break; } else if (line == "t") { if (r2000.IsScanning) { r2000.StopScan(); } else { r2000.StartScan(); } } } r2000.Disconnect(); } }