Exemplo n.º 1
0
        /// <summary>
        /// LRFセンサーデータをGridMapから生成(代用)
        /// </summary>
        /// <param name="robot"></param>
        /// <returns></returns>
        public double[] Sense(GridMap map, double posX, double posY, double robotTheta)
        {
            double[] result = new double[AngleRange];
            int i;
            int angRng = AngleRange / 2;

            int iroboTheta = (int)(robotTheta+0.5);
            iroboTheta = iroboTheta - ((iroboTheta / 360) * 360);

            LocPreSumpSystem.swCNT_MRF.Start();
            try
            {
                for (i = -angRng; i < angRng; ++i)
                {
                    int theta = (iroboTheta + -i + 360 * 2) % 360;

                    // 障害物までの距離を取得
                    result[i + angRng] = map.MeasureDist(
                        posX,
                        posY,
                        DeltaX[theta],
                        DeltaY[theta],
                        RangeMax);
                }
            }
            catch (Exception e)
            {
                throw e;
            }
            finally
            {
                LocPreSumpSystem.swCNT_MRF.Stop();
            }

            return result;
        }
Exemplo n.º 2
0
 public void SetMap(GridMap map)
 {
     Map = map;
 }
Exemplo n.º 3
0
        /// <summary>
        /// LRFセンサーデータをGridMapから生成(代用)
        /// </summary>
        /// <param name="robot"></param>
        /// <returns></returns>
        public double[] Sense(GridMap map, double posX, double posY, double robotTheta)
        {
            double[] result = new double[AngleRange];
            //int i;
            int angRng = AngleRange / 2;

            int iroboTheta = (int)(robotTheta+0.5);
            iroboTheta = iroboTheta - ((iroboTheta / 360) * 360);

            //LocPreSumpSystem.swCNT_MRF.Start();
            try
            {
                Parallel.For(-angRng, angRng, i => // こう書くだけで、並行して処理が行われる
                {
                    //for (i = -angRng; i < angRng; ++i) {

                    int theta = (iroboTheta + -i + 360 * 2) % 360;

                    // 障害物までの距離を取得
                    result[i + angRng] = map.MeasureDist(posX / PixelScale,
                                                          posY / PixelScale,
                                                          DeltaX[theta],
                                                          DeltaY[theta],
                                                          RangeMax) * PixelScale;
                });
            }
            catch (Exception e)
            {
                throw e;
            }
            finally
            {
                //LocPreSumpSystem.swCNT_MRF.Stop();
            }

            return result;
        }
Exemplo n.º 4
0
 public MapRangeFinder(double rangeMax, double pixelScale, Bitmap srcBmpMap)
     : this(rangeMax, pixelScale)
 {
     Bitmap bmpMap = new Bitmap(srcBmpMap);
     Map = new GridMap(bmpMap);
 }
Exemplo n.º 5
0
        /// <summary>
        /// GridMap同時生成
        /// </summary>
        /// <param name="rangeMax"></param>
        /// <param name="bmpMapFilename"></param>
        public MapRangeFinder(double rangeMax, double pixelScale, string bmpMapFilename)
            : this(rangeMax, pixelScale)
        {
            Bitmap bmpMap = new Bitmap(bmpMapFilename);

            Map = new GridMap(bmpMap);
        }
Exemplo n.º 6
0
        /// <summary>
        /// ワールドBMPからエリアのGridMapを作成
        /// </summary>
        /// <returns></returns>
        public GridMap MakeAreaGridMap()
        {
            GridMap gm = new GridMap(AreaGridSize.w, AreaGridSize.h);

            BitmapAccess bmpAp = new BitmapAccess(mapBmp);
            bmpAp.BeginAccess();

            // bmpからGridMapを生成
            for (int x = 0; x < gm.W; ++x)
            {
                for (int y = 0; y < gm.H; ++y)
                {
                    Color c;
                    int adX = WldOffset.x + x;
                    int adY = WldOffset.y + y;

                    if (adX < 0 || adX >= mapBmp.Width ||
                        adY < 0 || adY >= mapBmp.Height)
                    {
                        // レンジ外
                        c = GridMap.GridColor_Fill;
                    }
                    else
                    {
                        c = bmpAp[adX, adY];

                        // 色数を減らして、微妙な誤差をなくす
                        //c = Color.FromArgb((((c.R+4) / 5) * 5), (((c.G+4) / 5) * 5), (((c.B+4) / 5) * 5));
                    }

                    //if (c == GridMap.GridColor_Fill)
                    if ( GridMap.IsLess(c,GridMap.GridColor_Fill,20))
                    {
                        // 障害物
                        gm.M[x, y] = Grid.Fill;
                    }
                    //else if (c == GridMap.GridColor_Free)
                    else if (GridMap.IsUpper(c, GridMap.GridColor_Free, -20))
                    {
                        // 通行可能
                        gm.M[x, y] = Grid.Free;
                    }
                    else if (c == GridMap.GridColor_RedArea)
                    {
                        // 通行不可 壁の中
                        gm.M[x, y] = Grid.RedArea;
                    }
                    else if (c == GridMap.GridColor_GreenArea)
                    {
                        // 通行可能 位置補正推奨
                        gm.M[x, y] = Grid.GreenArea;
                    }
                    else if (c == GridMap.GridColor_BlueArea)
                    {
                        // 通行可能 スローダウン
                        gm.M[x, y] = Grid.BlueArea;
                    }
                    else
                    {
                        // それ以外
                        gm.M[x, y] = Grid.Unknown;
                    }
                }
            }

            bmpAp.EndAccess();

            AreaGridMap = gm;

            return gm;
        }