Example #1
0
        //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);
                }
            }
        }
Example #2
0
        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);
            }
        }
Example #3
0
        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();
            }
        }