//執行檢測 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; }