Exemplo n.º 1
0
        private static void addRadarPoint()
        {
            // 线段序列为空
            if (proSeg == null)
            {
                return;
            }
            // 数据不满足要求,则返回
            KeyPoint p  = PortManager.drPort.getPosition();
            UrgModel um = PortManager.urgPort.getUrgData();

            if (um.Distance.Count < UrgInfo.ang180Index)
            {
                return;
            }
            // 货物激光雷达位置信息
            KeyPoint kp = CarInfo.getRadarPosition(p);

            um.Distance[UrgInfo.ang000Index] = (um.Distance[UrgInfo.ang000Index] < 100) ? UrgInfo.maxDist : um.Distance[UrgInfo.ang000Index];
            um.Distance[UrgInfo.ang180Index] = (um.Distance[UrgInfo.ang180Index] < 100) ? UrgInfo.maxDist : um.Distance[UrgInfo.ang180Index];
            // 添加线段
            proSeg.addPoint(
                new SimPoint()
            {
                x = kp.x + Math.Cos(p.w + Math.PI) * um.Distance[UrgInfo.ang180Index] / 1000,
                y = kp.y + Math.Sin(p.w + Math.PI) * um.Distance[UrgInfo.ang180Index] / 1000
            },
                new SimPoint()
            {
                x = kp.x + Math.Cos(p.w) * um.Distance[UrgInfo.ang000Index] / 1000,
                y = kp.y + Math.Sin(p.w) * um.Distance[UrgInfo.ang000Index] / 1000
            });
        }
Exemplo n.º 2
0
        /////////////////////////////////////////////// private attribute ////////////////////////////////////////////

        private bool getUrgPoint(UrgPort urgPort)
        {
            config.pointsH = new List <URG_POINT>();
            if (!urgPort.IsOpen)
            {
                return(false);
            }

            UrgModel urgModel = urgPort.getUrgData();

            if (urgModel.Distance == null)
            {
                return(false);
            }
            int BG = (int)((75 - urgPort.AngleStart) / urgPort.AnglePace);
            int ED = (int)((105 - urgPort.AngleStart) / urgPort.AnglePace);

            if (urgModel.Distance.Count < ED)
            {
                return(false);
            }

            config.pointsH = new List <URG_POINT>();

            for (int i = BG; i < ED; i++)
            {
                if (urgModel.Distance[i] == 0)
                {
                    continue;
                }

                URG_POINT point = new URG_POINT();
                point.d = urgModel.Distance[i];

                double angle = urgPort.AngleStart + i * urgPort.AnglePace;

                point.a = angle;
                point.x = point.d * Math.Cos(angle * Math.PI / 180);
                point.y = point.d * Math.Sin(angle * Math.PI / 180);

                config.pointsH.Add(point);
            }

            return(true);
        }
Exemplo n.º 3
0
        private List <CONFIG.URG_POINT> getUrgPoint(double angleBG, double angleED)
        {
            List <CONFIG.URG_POINT> points = new List <CONFIG.URG_POINT>();

            if (!PortManager.urgPort.IsOpen)
            {
                return(points);
            }

            UrgModel urgModel = PortManager.urgPort.getUrgData();

            while (urgModel.Distance == null || urgModel.Distance.Count == 0)
            {
                urgModel = PortManager.urgPort.getUrgData();
            }

            int BG = (int)((angleBG - -30) / (360.0 / 1024.0));
            int ED = (int)((angleED - -30) / (360.0 / 1024.0));

            if (urgModel.Distance.Count < ED)
            {
                return(points);
            }

            for (int i = BG; i < ED; i++)
            {
                if (urgModel.Distance[i] == 0)
                {
                    continue;
                }

                CONFIG.URG_POINT point = new CONFIG.URG_POINT();
                point.d = urgModel.Distance[i];

                double angle = -30.0 + i * 360.0 / 1024.0;

                point.a = angle;
                point.x = point.d * Math.Cos(angle * Math.PI / 180);
                point.y = point.d * Math.Sin(angle * Math.PI / 180);

                points.Add(point);
            }

            return(points);
        }
Exemplo n.º 4
0
        public UrgModel getUrgData()
        {
            while (config.IsSetting)
            {
                ;
            }
            config.IsGetting = true;

            List <long> Distance = new List <long>();

            foreach (long idis in config.Distance)
            {
                Distance.Add(idis);
            }

            config.IsGetting = false;

            UrgModel urgModel = new UrgModel();

            urgModel.Distance = Distance;
            return(urgModel);
        }