public int ObstacleDetected(LidarData Data, DetectedRectRegion AlarmRegion, DetectedRectRegion BanRegion, out ObstacleData AlarmObData, out ObstacleData BanObData)
{
int Result = 0;
AlarmObData = new ObstacleData();
BanObData = new ObstacleData();
double Distance = 0;
for (int index = 0; index < Data.X.Length; index++)
{
if (AlarmRegion.DetectRegionX1 < Data.X[index] && Data.X[index] < AlarmRegion.DetectRegionX2 && AlarmRegion.DetectRegionY1 < Data.Y[index] && Data.Y[index] < AlarmRegion.DetectRegionY2 && AlarmRegion.DetectRegionZ1 < Data.Z[index] && Data.Z[index] < AlarmRegion.DetectRegionZ2)
{
Distance = Math.Sqrt(Data.X[index] * Data.X[index] + Data.Y[index] * Data.Y[index] + Data.Z[index] * Data.Z[index]);
if (Data.X[index] != 0 && Data.Y[index] != 0 && Data.Z[index] != 0)
{
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)
{
BanObData.ObstaclePosX.Add(Data.X[index]);
BanObData.ObstaclePosY.Add(Data.Y[index]);
BanObData.ObstaclePosZ.Add(Data.Z[index]);
BanObData.ObstacleDistance.Add(Distance);
}
else
{
AlarmObData.ObstaclePosX.Add(Data.X[index]);
AlarmObData.ObstaclePosY.Add(Data.Y[index]);
AlarmObData.ObstaclePosZ.Add(Data.Z[index]);
AlarmObData.ObstacleDistance.Add(Distance);
}
}
}
}
}
if (BanObData.ObstaclePosX.Count != 0)
{
Result = 2;
}
else if (AlarmObData.ObstaclePosX.Count != 0)
{
Result = 1;
}
//回傳0 為無障礙物
//回傳1 為警告區域內有障礙物
//回傳2 為禁止區域有障礙物
return(Result);
}
public bool ObstacleDetected(LidarData Data, DetectedRectRegion Region, out ObstacleData ObData)
{
bool Isdetect = false;
ObData = new ObstacleData();
double Distance = 0;
for (int index = 0; index < Data.X.Length; index += 100)
{
if (Region.DetectRegionX1 < Data.X[index] && Data.X[index] < Region.DetectRegionX2 && Region.DetectRegionY1 < Data.Y[index] && Data.Y[index] < Region.DetectRegionY2 && Region.DetectRegionZ1 < Data.Z[index] && Data.Z[index] < Region.DetectRegionZ2)
{
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);
}
}
return(Isdetect);
}
