public Boolean init()
{
close();
isRunning = true;
Boolean is_connected = false;
try
{
semaphoreDriver.WaitOne();
rplidar = new RplidarNative();
driver = RplidarNative.createDriver();
if (driver > 0)
{
//is_connected = RplidarNative.connect(driver, Constants.LIDAR_DEFAULT_PORT);
if (is_connected)
{
RplidarNative.startScan(driver);
}
else
{
errorMessage = "Failed to bind serial port " + Constants.LIDAR_DEFAULT_PORT + " for RpLidar driver.";
}
}
else
{
errorMessage = "Failed to create RpLidar driver.";
}
semaphoreDriver.Release();
}
catch (ThreadInterruptedException e)
{
Console.WriteLine(e.Message);
}
return(is_connected);
}
public Boolean isHealthy()
{
Boolean isLidarHealthy = false;
try
{
semaphoreDriver.WaitOne();
if (driver > 0)
{
isLidarHealthy = RplidarNative.checkHealth(driver);
if (!isLidarHealthy)
{
errorMessage = "RpLidar is unhealthy.";
}
}
else
{
errorMessage = "Failed to create RpLidar driver.";
}
semaphoreDriver.Release();
}
catch (ThreadInterruptedException e)
{
Console.WriteLine(e.Message);
}
return(isLidarHealthy);
}
static void Main(String[] args)
{
RplidarNative rplidar = new RplidarNative();
long driver = RplidarNative.createDriver();
if (driver == 0)
{
Console.WriteLine("Failed to create driver...");
return;
}
StringBuilder serialPort = new StringBuilder("\\\\.\\COM3");
bool is_connected = RplidarNative.connect(driver, serialPort);
if (!is_connected)
{
Console.WriteLine("Failed to bind " + serialPort);
RplidarNative.disposeDriver(driver);
return;
}
bool is_healthy = RplidarNative.checkHealth(driver);
if (!is_healthy)
{
Console.WriteLine("RpLidar is not healthy.");
RplidarNative.disposeDriver(driver);
return;
}
RplidarNative.startScan(driver);
MeasurementNode[] nodes = RplidarNative.getScanData(driver);
if (nodes == null)
{
Console.WriteLine("No scan result.");
}
else
{
foreach (MeasurementNode node in nodes)
{
Console.WriteLine(node.angle + " " + node.distance + " " + node.sync_quality);
}
}
RplidarNative.stopScan(driver);
RplidarNative.disposeDriver(driver);
}
public void run()
{
while (Thread.CurrentThread.ThreadState != ThreadState.Suspended && isRunning)
{
try
{
Thread.Sleep(Constants.LIDAR_SLEEP_INTERVAL);
}
catch (ThreadInterruptedException e)
{
Console.WriteLine(e.Message);
continue;
}
// Restart driver if necessary.
// if (!isHealthy() && !init()) {
// logger.debug("LidarRunnable is not healthy: " + errorMessage());
// continue;
// }
MeasurementNode[] oneScan = null;
try
{
semaphoreDriver.WaitOne();
if (rplidar != null && driver > 0)
{
oneScan = RplidarNative.getScanData(driver);
}
// oneScan = getScanDataForTesting();
semaphoreDriver.Release();
}
catch (ThreadInterruptedException e)
{
Console.WriteLine(e.Message);
}
if (oneScan == null)
{
errorMessage = "RpLidar reports no scan result.";
continue;
}
addOneScanToCache(oneScan);
reportSuccess();
}
}
public void close()
{
try
{
semaphoreDriver.WaitOne();
if (rplidar != null && driver > 0)
{
RplidarNative.stopScan(driver);
RplidarNative.disposeDriver(driver);
}
rplidar = null;
driver = 0;
semaphoreDriver.Release();
}
catch (ThreadInterruptedException e)
{
Console.WriteLine(e.Message);
}
isRunning = false;
}
public MeasurementNode(int sync_quality, float angle, float distance)
{
this.sync_quality = RplidarNative.CopySync_quality();
this.angle = RplidarNative.CopyAngle();
this.distance = RplidarNative.CopyDistance();
}
