//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);
}
}
}
public void OnMessageReceivedEvent(object sender, LidarMessageArgs e)
{
if (r2000 != null)
{
r2000.DisplayMessage(e.Line, e.Value);
}
}