//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();
}
}
