/// <summary>
/// Handles a scan, simply updates the landmark database and map if required
/// by extracting them using the EKF algorithm.
/// </summary>
/// <param name="sender">Sender.</param>
/// <param name="e">E.</param>
private void SerialProxy_Scan(object sender, ScanEventArgs e)
{
Gdk.Threads.Enter ();
try
{
// Extract any landmarks then update the slam database with any new landmarks.
ekfSlam.UpdateAndAddLineLandmarks (ekfSlam.ExtractLineLandmarks (e.Readings.ToArray (),
robot.Position));
ekfSlam.RemoveBadLandmarks (e.Readings.ToArray (), robot.Position);
// Now update the model.
map.UpdateLandmarks (ekfSlam.GetDB ());
}
finally
{
Gdk.Threads.Leave ();
}
}
private void SerialProxy_Scan(object sender, ScanEventArgs e)
{
Console.WriteLine(e.ToString());
}
protected virtual void OnScanPerformed(ScanEventArgs e)
{
if (Scanned != null)
{
Scanned (this, e);
}
}
private void Read()
{
while (serialPort.IsOpen)
{
try
{
string message = serialPort.ReadLine ();
switch (message [0])
{
case 'o':
string[] parameters = message.Split (',');
OdometryUpdateEventArgs args = new OdometryUpdateEventArgs (
Int32.Parse (parameters [1]),
Int32.Parse (parameters [2]),
Double.Parse (parameters [3]));
this.OnOdometryUpdate (args);
break;
case 's':
string[] stringReadings = message.Split (',');
List<double> readings = new List<double> ();
// Somewhat inefficient. Start from 1 to skip first character
// which is the message header.
for (int i = 1; i < stringReadings.Length; i++)
{
readings.Add (Double.Parse (stringReadings [i]));
}
readings.Reverse ();
ScanEventArgs args2 = new ScanEventArgs (readings);
OnScanPerformed (args2);
break;
default:
break;
}
}
catch (TimeoutException ex)
{
Console.WriteLine (ex.ToString ());
}
catch (FormatException ex)
{
Console.WriteLine (ex.ToString ());
}
catch (IndexOutOfRangeException ex)
{
Console.WriteLine (ex.ToString ());
}
catch (ThreadAbortException)
{
// Ignore it.
}
catch (Exception ex)
{
Console.WriteLine (ex.ToString ());
}
}
}
/// <summary>
/// Handles a scan, simply updates the landmark database and map if required
/// by extracting them using the EKF algorithm.
/// </summary>
/// <param name="sender">Sender.</param>
/// <param name="e">E.</param>
private void SerialProxy_Scan(object sender, ScanEventArgs e)
{
}
