//執行檢測
public void RunObstacleDetect()
{
Obstacle_Ini.Read_ini_Cfg3();
rWarningCircleRegion = new DetectCircleRegion(0f, 0f, -0.3f, 0.8f, Obstacle_Ini.Lidar_Radius);
if (Obstacle_Ini.Lidar_Radius / 3 <= 1)
{
rAlarmCircleRegion = new DetectCircleRegion(0f, 0f, -0.3f, 0.8f, 1.1f);
}
else
{
rAlarmCircleRegion = new DetectCircleRegion(0f, 0f, -0.3f, 0.8f, (Obstacle_Ini.Lidar_Radius / 3));
}
while (IsContinueDetected)
{
//bool HaveObject = ObstacleDetected(LidarFunc.rLidarData, rDetectRegion, out ObjectData);
Information_Obstacle = CircleObstacleDetected(LidarFunc.rLidarData, rWarningCircleRegion, rAlarmCircleRegion, out AlarmObjectData, out ObjectData);
//MainForm.BeginInvoke(new ShowResult(ShowDetectResult), HaveObject);
MainForm.BeginInvoke(new ShowGradingResult(ShowDetectResult), Information_Obstacle);
//Console.WriteLine("Is there an obstacle ? " + HaveObject.ToString());
/*int result = ObstacleDetected(LidarFunc.rLidarData, rAlarmRegion, rDetectRegion, out AlarmObjectData, out ObjectData);
* MainForm.BeginInvoke(new ShowGradingResult(ShowDetectResult), result);*/
}
}
public int CircleObstacleDetected(LidarData Data, DetectCircleRegion WarningCircleRegion, DetectCircleRegion AlarmCircleRegion, out ObstacleData WarningObData, out ObstacleData AlarmObData)
{
//bool Isdetect = false;
int Result = 0;
WarningObData = new ObstacleData();
AlarmObData = new ObstacleData();
double Distance = 0;
double radius = 0;
for (int index = 0; index < Data.X.Length; index++)
{
radius = Math.Sqrt((Data.X[index] - WarningCircleRegion.CenterX) * (Data.X[index] - WarningCircleRegion.CenterX) + (Data.Y[index] - WarningCircleRegion.CenterY) * (Data.Y[index] - WarningCircleRegion.CenterY));
if (Data.X[index] != 0 && Data.Y[index] != 0 && Data.Z[index] != 0)
{
if (WarningCircleRegion.ZDown <= Data.Z[index] && Data.Z[index] <= WarningCircleRegion.ZUp && radius <= WarningCircleRegion.Radius)
{
if (Data.Z[index] >= Obstacle_Ini.Lidar_Obstacle_Height)
{
//if (BanRegion.DetectRegionX1 < Data.X[index] && Data.X[index] < BanRegion.DetectRegionX2 && BanRegion.DetectRegionY1 < Data.Y[index] && Data.Y[index] < BanRegion.DetectRegionY2 && BanRegion.DetectRegionZ1 < Data.Z[index] && Data.Z[index] < BanRegion.DetectRegionZ2)
if (radius < WarningCircleRegion.Radius && radius < AlarmCircleRegion.Radius && radius > 1)
{
//當在警戒範圍內時(降速)
AlarmObData.ObstaclePosX.Add(Data.X[index]);
AlarmObData.ObstaclePosY.Add(Data.Y[index]);
AlarmObData.ObstaclePosZ.Add(Data.Z[index]);
AlarmObData.ObstacleDistance.Add(Distance);
}
else if (radius < WarningCircleRegion.Radius && radius > AlarmCircleRegion.Radius && radius > 1)
{
//當在危險範圍內時(停止)
WarningObData.ObstaclePosX.Add(Data.X[index]);
WarningObData.ObstaclePosY.Add(Data.Y[index]);
WarningObData.ObstaclePosZ.Add(Data.Z[index]);
WarningObData.ObstacleDistance.Add(Distance);
}
/*Distance = Math.Sqrt(Data.X[index] * Data.X[index] + Data.Y[index] * Data.Y[index] + Data.Z[index] * Data.Z[index]);
* Isdetect = true;
* ObData.ObstaclePosX.Add(Data.X[index]);
* ObData.ObstaclePosY.Add(Data.Y[index]);
* ObData.ObstaclePosZ.Add(Data.Z[index]);
* ObData.ObstacleDistance.Add(Distance);*/
}
}
}
}
if (AlarmObData.ObstaclePosX.Count != 0)
{
Result = 2;
}
else if (WarningObData.ObstaclePosX.Count != 0)
{
Result = 1;
}
return(Result);
//return Isdetect;
}
