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