コード例 #1
0
ファイル: ObstacleFunc.cs プロジェクト: ChiFang/AGV_Sys
        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);
        }
コード例 #2
0
ファイル: ObstacleFunc.cs プロジェクト: ChiFang/AGV_Sys
        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);
        }
コード例 #3
0
ファイル: ObstacleFunc.cs プロジェクト: ChiFang/AGV_Sys
        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;
        }