/// <summary>
/// Does the task work.
/// </summary>
/// <param name="token">The token.</param>
private void LidarTaskDoWork(CancellationToken token)
{
try
{
// Check if on start is canceled
token.ThrowIfCancellationRequested();
IsRunning = true;
while (true)
{
if (token.IsCancellationRequested)
{
token.ThrowIfCancellationRequested();
}
Thread.Sleep(400);
if (RpLidarInterface.LidarIsConnected() != 1)
{
return;
}
var scanResult = RpLidarInterface.GetCurrentVectors();
OnScanDataReceived?.Invoke(this, new LidarDataReceivedEventArgs(scanResult, ++_ScanNumber));
}
}
catch (OperationCanceledException)
{
throw;
}
catch (AggregateException e)
{
foreach (var v in e.InnerExceptions)
{
Console.WriteLine(v);
}
}
catch (Exception ex)
{
Console.WriteLine(ex);
}
finally
{
// Stop device and dispose driver
try
{
// Exception happens when closing an connection
// where someone has reconnect USB cable during usage
RpLidarInterface.LidarDisposeDriver();
}
catch (Exception ex)
{
Console.WriteLine(ex);
}
IsRunning = false;
}
}
/// <summary>
/// Stops this instance.
/// </summary>
/// <returns><c>true</c> if successfully, <c>false</c> otherwise.</returns>
public bool LidarStop()
{
if (!IsRunning ||
IsDisposed)
{
return(false);
}
_TokenSource?.Cancel();
try
{
_Task?.Wait(_Token);
}
catch (OperationCanceledException)
{
}
catch (AggregateException e)
{
foreach (var v in e.InnerExceptions)
{
Console.WriteLine(v);
}
}
finally
{
_TokenSource?.Dispose();
}
try
{
// Stop device and dispose driver
RpLidarInterface.LidarDisposeDriver();
if (RpLidarInterface.LidarIsConnected() != 1)
{
return(true);
}
else
{
return(false);
}
}
finally
{
if (RpLidarInterface.LidarIsConnected() != 1)
{
OnDeviceDisconnected?.Invoke(this, EventArgs.Empty);
}
}
}
/// <summary>
/// Releases unmanaged and - optionally - managed resources.
/// </summary>
/// <param name="disposing"><c>true</c> to release both managed and unmanaged resources; <c>false</c> to release only unmanaged resources.</param>
protected virtual void LidarDispose(bool disposing)
{
if (!_Disposed)
{
if (!IsRunning)
{
return;
}
try
{ // Signal exit
_TokenSource?.Cancel();
}
catch
{
}
try
{
// Wait until task is ended
_Task.Wait(5000, _Token);
}
catch (OperationCanceledException)
{
// Task tell us it's ended
}
catch (AggregateException e)
{
foreach (var v in e.InnerExceptions)
{
Console.WriteLine(v);
}
}
finally
{
// Dispose token source
_TokenSource?.Dispose();
// Stop motor, disconnect device and dispose driver
RpLidarInterface.LidarDisposeDriver();
_Disposed = true;
}
}
}
/// <summary>
/// Starts this instance.
/// </summary>
/// <param name="scanMode">The scan mode.</param>
/// <returns><c>true</c> if successfully, <c>false</c> otherwise.</returns>
public bool LidarStart(EScanMode scanMode = EScanMode.Boost)
{
try
{
if (IsRunning ||
IsDisposed)
{
return(false);
}
_ScanNumber = 0;
// Load driver
if (!NativeModuleManager.InitializeNativeModule(NativeModuleNames.NativeRpLidar))
{
Console.WriteLine("Can not load lidar native driver library.");
return(false);
}
// Initialize driver
if (RpLidarInterface.LidarInitializeDriver() != 0)
{
Console.WriteLine("Can not initialize lidar driver.");
}
// Connect device
if (RpLidarInterface.LidarConnect(_SerialPort, _SerialPortSpeed) != 0)
{
Console.WriteLine($"Can not connect to serial port \"{_SerialPort}:{_SerialPortSpeed}\".");
}
// Get device info
_DeviceInfo = new rplidar_response_device_info_t
{
serialNum = new byte[16]
};
if (RpLidarInterface.LidarGetDeviceInfo(ref _DeviceInfo) != 0)
{
Console.WriteLine("Can not get device info from device.");
}
// Get device health info
_DeviceHealthInfo = new rplidar_response_device_health_t();
if (RpLidarInterface.LidarGetHealth(ref _DeviceHealthInfo) != 0)
{
Console.WriteLine($"Can not get device health info from device.");
}
// Reset device on error state
if (_DeviceHealthInfo.Status == (byte)EDeviceStatus.RPLIDAR_STATUS_ERROR)
{
Console.WriteLine($@"Reset device due to error device health status: {_DeviceHealthInfo.Status}");
RpLidarInterface.LidarReset();
}
// Start spinning motor
if (RpLidarInterface.LidarStartMotor() != 0)
{
Console.WriteLine($"Can not start device motor.");
}
// Start scanning
_RpLidarScanMode = new RplidarScanMode
{
scan_mode = new char[64]
};
if (RpLidarInterface.LidarStartScan(false, (ushort)scanMode, ref _RpLidarScanMode) != 0)
{
Console.WriteLine($"Can not start scan mode.");
}
// Initialize task
_TokenSource = new CancellationTokenSource();
_Token = _TokenSource.Token;
_Task = Task.Factory.StartNew(() => LidarTaskDoWork(_Token), _Token);
if (RpLidarInterface.LidarIsConnected() == 1)
{
return(true);
}
else
{
return(false);
}
}
finally
{
if (RpLidarInterface.LidarIsConnected() == 1)
{
OnDeviceConnected?.Invoke(this, EventArgs.Empty);
}
}
}
