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