/// <summary>
/// 自己位置推定 開始位置セット
/// </summary>
/// <param name="stWldX">マップ座標X</param>
/// <param name="stWldY">マップ座標Y</param>
/// <param name="stDir">角度</param>
public void SetStartPostion(int stWldX, int stWldY, double stDir)
{
startMapX = stWldX;
startMapY = stWldY;
startMapDir = stDir;
MRF = new MapRangeFinder(LRF_Ctrl.LRFmaxRange_mm / MapToRealScale); // 仮想Map用 LRFクラス
worldMap.UpdateAreaCenter(stWldX, stWldY);
AreaBmp = worldMap.AreaGridMap.UpdateBitmap();
//bAreaMapUpdateReqest = false;
MRF.SetMap(worldMap.AreaGridMap);
// ロボットマーカ設定
R1 = new MarkPoint(stWldX, stWldY, stDir); // 実ロボット位置 R.E,Compass,GPSでの位置+ LRF補正
E1 = new MarkPoint(stWldX, stWldY, stDir); // R.Encoderの位置
oldE1 = new MarkPoint(stWldX, stWldY, stDir); //
E2 = new MarkPoint(stWldX, stWldY, stDir); // R.E Pluse & Compus の位置
V1 = new MarkPoint(stWldX, stWldY, stDir); // 推定位置ロボット
C1 = new MarkPoint(0, 0, stDir);
G1 = new MarkPoint(0, 0, 0); // GPS
oldG1 = new MarkPoint(stWldX, stWldY, stDir); //
A1 = new MarkPoint(0, 0, 0); // AMCL
// パーティクルフィルター初期化
{
// サンプル数(ParticleSizeと同じで、すべてのパーティクルを対象とする)
// LRFの有効距離を半径に変換(/2.0)、20%の距離で散らばる
// +-5度の範囲
ParticleFilter = new ParticleFilter(ParticleSize, (((LRF_Ctrl.LRFmaxRange_mm / 2.0) * 0.20) / MapToRealScale), 30.0); // サンプリング数、パーティクルのレンジ
Particles = new List <Particle>();
for (int i = 0; i < ParticleSize; ++i)
{
// パーティクルマーカー
Particles.Add(new Particle(new MarkPoint(0, 0, 0), 0));
}
}
// ローパスフィルター初期化
ResetLPF_V1(V1);
}
/// <summary>
/// エリアの移動
/// </summary>
private bool MoveAreaCheck()
{
// エリアの端に近づいたか？
if ((R1.X < worldMap.AreaGridSize.w / 4 || R1.X > worldMap.AreaGridSize.w * 3 / 4) ||
(R1.Y < worldMap.AreaGridSize.h / 4 || R1.Y > worldMap.AreaGridSize.h * 3 / 4))
{
lock (AreaBmp)
{
// R1の位置を新しいエリアの中心とする
worldMap.UpdateAreaCenter((int)(R1.X + 0.5), (int)(R1.Y + 0.5));
// エリアマップ更新
AreaBmp = worldMap.AreaGridMap.UpdateBitmap();
}
//bAreaMapUpdateReqest = true;
MRF.SetMap(worldMap.AreaGridMap);
return(true);
}
return(false);
}
/// <summary>
/// 自己位置推定 開始位置セット
/// </summary>
/// <param name="stWldX">マップ座標X</param>
/// <param name="stWldY">マップ座標Y</param>
/// <param name="stDir">角度</param>
public void SetStartPostion(int stWldX, int stWldY, double stDir)
{
MRF = new MapRangeFinder(LRF_Ctrl.LRFmaxRange_mm / MapToRealScale); // 仮想Map用 LRFクラス
worldMap.UpdateAreaCenter(stWldX, stWldY);
AreaBmp = worldMap.AreaGridMap.UpdateBitmap();
//bAreaMapUpdateReqest = false;
MRF.SetMap(worldMap.AreaGridMap);
// ロボットマーカ設定
R1 = new MarkPoint(stWldX, stWldY, stDir); // 実ロボット位置 R.E,Compass,GPSでの位置+ LRF補正
E1 = new MarkPoint(stWldX, stWldY, stDir); // R.Encoderの位置
oldE1 = new MarkPoint(stWldX, stWldY, stDir); //
E2 = new MarkPoint(stWldX, stWldY, stDir); // R.E Pluse & Compus の位置
V1 = new MarkPoint(stWldX, stWldY, stDir); // 推定位置ロボット
C1 = new MarkPoint(0, 0, stDir);
G1 = new MarkPoint(0, 0, 0); // GPS
oldG1 = new MarkPoint(stWldX, stWldY, stDir); //
// パーティクルフィルター初期化
{
// サンプル数(ParticleSizeと同じで、すべてのパーティクルを対象とする)
// LRFの有効距離を半径に変換(/2.0)、20%の距離で散らばる
// +-5度の範囲
ParticleFilter = new ParticleFilter(ParticleSize, (((LRF_Ctrl.LRFmaxRange_mm / 2.0) * 0.20) / MapToRealScale), 30.0 ); // サンプリング数、パーティクルのレンジ
Particles = new List<Particle>();
for (int i = 0; i < ParticleSize; ++i)
{
// パーティクルマーカー
Particles.Add(new Particle(new MarkPoint(0,0,0), 0));
}
}
// ローパスフィルター初期化
ResetLPF_V1(V1);
}