/// <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;
}
public void SetMap(GridMap map)
{
Map = map;
}
/// <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;
}
public MapRangeFinder(double rangeMax, double pixelScale, Bitmap srcBmpMap)
: this(rangeMax, pixelScale)
{
Bitmap bmpMap = new Bitmap(srcBmpMap);
Map = new GridMap(bmpMap);
}
/// <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);
}
/// <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;
}
