/// <summary>
/// コンストラクタ
/// </summary>
/// <param name="areaMap"></param>
public ActiveDetour(LocationPresumption.GridMap areaMap, double[] lrfData)
{
// GridMapの1/5 [100mm間隔から、500mmに変換]
hexMap = new HexPixelMap(areaMap.W / dotScale, areaMap.H / dotScale);
// GridMap[100mm間隔]からhexMap[500mm間隔]に変換
for (int iH = 0; iH < areaMap.H / dotScale; iH++)
{
for (int iW = 0; iW < areaMap.W / dotScale; iW++)
{
bool bFree = true;
// 500mm間隔の中に1つでも通行不可ならば、通行できない
for (int nH = 0; nH < dotScale; nH++)
{
for (int nW = 0; nW < dotScale; nW++)
{
if (areaMap[iW * dotScale + nW, iH *dotScale + nH] != LocationPresumption.Grid.Free)
{
bFree = false;
}
}
}
if (!bFree)
{
// 障害物あり
hexMap[iW, iH].powVal = 2.0;
}
}
}
// LRFから現在の障害物をマップに落とし込む
{
double lrfScale = URG_LRF.getScale(); // スケール変換値
double rPI = Math.PI / 180.0;
double mmToPix = 1.0 / 500.0;
// LRFの値を描画
for (int i = 0; i < lrfData.Length; i++)
{
double val = lrfData[i] * lrfScale;
double rad = (-i + (270.0 / 2.0) - 90) * rPI;
// LRFは右下から左回り
int iX = (int)(val * Math.Cos(rad) * mmToPix);
int iY = (int)(val * Math.Sin(rad) * mmToPix);
// 障害物あり
if (iX >= 0 && iX < hexMap.W &&
iY >= 0 && iY < hexMap.H)
{
hexMap[iX, iY].powVal = 2.0;
}
}
}
}
/// <summary>
/// LRF初期化 ＬＡＮモード
/// </summary>
/// <param name="IPAddr"></param>
/// <param name="IPPort"></param>
public void Open(string IPAddr, int IPPort)
{
urgLRF = new URG_LRF();
if (!urgLRF.IpOpen(IPAddr, IPPort))
{
// Open失敗
urgLRF = null;
}
}
/// <summary>
/// 緊急時ハンドルコントロール
/// </summary>
/// <param name="lrfData">270度＝個数のデータを想定</param>
/// <returns></returns>
public double CheckEHS(double[] lrfData)
{
double rtHandleVal = 0.0;
Result = EHS_MODE.None;
// 指定の扇状角度の指定の距離範囲内に、障害物があればハンドルを切る。
{
double lrfScale = URG_LRF.getScale(); // スケール変換値
const int rangeCenterAng = 270 / 2; // 中央(角度)
double LHitLength = 0.0;
double LHitDir = 0.0;
double RHitLength = 0.0;
double RHitDir = 0.0;
double minScaledRange = (MinRange * lrfScale);
double maxScaleRange = (MaxRange * lrfScale);
double minDistance;
// LRFの値を調べる
// 右側
minDistance = maxScaleRange;
for (int i = (stRAng + rangeCenterAng); i < (edRAng + rangeCenterAng); i++)
{
// 範囲内なら反応
if (lrfData[i] > minScaledRange && lrfData[i] < maxScaleRange)
{
// もっとも近い障害物を選定
if (lrfData[i] < minDistance)
{
minDistance = lrfData[i];
RHitLength = lrfData[i];
RHitDir = (double)i;
}
}
}
// 左側
minDistance = maxScaleRange;
for (int i = (edLAng + rangeCenterAng); i >= (stLAng + rangeCenterAng); i--)
{
// 範囲内なら反応
if (lrfData[i] > minScaledRange && lrfData[i] < maxScaleRange)
{
if (lrfData[i] < minDistance)
{
minDistance = lrfData[i];
LHitLength = lrfData[i];
LHitDir = (double)i;
}
}
}
// ハンドル角を計算
#if false
if (LHitLength != 0.0 && RHitLength != 0.0)
{
// 左右がHIT
double Lx, Ly;
double Rx, Ry;
Lx = Ly = 0.0;
LrfValToPosition(ref Lx, ref Ly, LHitDir, LHitLength);
Rx = Ry = 0.0;
LrfValToPosition(ref Rx, ref Ry, RHitDir, RHitLength);
double tgtAng = Rooting.CalcPositionToAngle((Lx - Rx) * 0.5, (Ly - Ry) * 0.5);
rtHandleVal = Rooting.CalcHandleValueFromAng(tgtAng, 0.0);
Result = EHS_MODE.CenterPass;
}
else if (LHitLength != 0.0)
#else
if (LHitLength != 0.0 && RHitLength != 0.0)
{
// 両方ならば、近いほうを優先する
if (LHitLength < RHitLength)
{
RHitLength = 0.0;
}
else
{
LHitLength = 0.0;
}
}
if (LHitLength != 0.0)
#endif
{
// 左側がHit
double Lx, Ly;
Lx = Ly = 0.0;
LrfValToPosition(ref Lx, ref Ly, LHitDir, LHitLength);
// 壁から離れる距離を足しこむ
Lx += WallRange;
double tgtAng = 45.0;// Rooting.CalcPositionToAngle(Lx, Ly);
rtHandleVal = Rooting.CalcHandleValueFromAng(tgtAng, 0.0);
Result = EHS_MODE.LeftWallHit;
}
else if (RHitLength != 0.0)
{
// 右側がHit
double Rx, Ry;
Rx = Ry = 0.0;
LrfValToPosition(ref Rx, ref Ry, RHitDir, RHitLength);
Rx -= WallRange;
double tgtAng = -45.0;//Rooting.CalcPositionToAngle(Rx, Ry);
rtHandleVal = Rooting.CalcHandleValueFromAng(tgtAng, 0.0);
Result = EHS_MODE.RightWallHit;
}
}
// 極小値は切り捨て
rtHandleVal = (double)((int)(rtHandleVal * 100.0)) / 100.0;
return(rtHandleVal);
}
/// <summary>
/// LRF初期化 エミュレーションモード
/// </summary>
public void Open()
{
urgLRF = null;
}
/// <summary>
/// LRF初期化 ログデータモード
/// </summary>
/// <param name="logFilename"></param>
public void Open(string logFilename)
{
urgLRF = new URG_LRF();
urgLRF.LogFileOpen(logFilename);
}
/// <summary>
/// 緊急ブレーキ判定
/// </summary>
/// <param name="lrfData">270度＝個のデータ</param>
/// <returns>ブレーキLv 0..問題なし 9...非常停止</returns>
public int CheckEBS(double[] lrfData, long UpdateCnt)
{
int rangeAngHalf = 270 / 2; // 中央(角度)
int stAng = EmergencyBrake.stAng;
int edAng = EmergencyBrake.edAng;
bool bCauntion = false;
bool bStop = false;
int cntCautuon = 0;
int cntStop = 0;
// 時間経過で注意Lv引き下げ
if ((UpdateCnt - hzUCNT) > LvDownCnt)
{
hzUCNT = UpdateCnt;
if (CautionLv > 0)
{
CautionLv--;
}
}
// ハンドル値によって、角度をずらす
#if EBS_HANDLE_LINK
{
// ハンドル切れ角３０度 0.4は角度を控えめに40%
int diffDir = (int)((-handleValue * 30.0) * 0.3);
stAng += diffDir;
edAng += diffDir;
HandleDiffAngle = diffDir;
}
#endif
double BrakeScaledRange = BrakeRange * URG_LRF.getScale();
double SlowScaledRange = SlowRange * URG_LRF.getScale();
// LRFの値を調べる
for (int i = (stAng + rangeAngHalf); i < (edAng + rangeAngHalf); i++)
{
// ブレーキレンジ内か？
if (lrfData[i] < BrakeScaledRange)
{
cntStop++;
bStop = true;
}
// 徐行レンジ内か？
if (lrfData[i] < SlowScaledRange)
{
cntCautuon++;
bCauntion = true;
}
}
// 注意Lv引き上げ
if (bStop)
{
// 停止
CautionLv = CautionLvMax;
hzUCNT = UpdateCnt;
}
else if (bCauntion && CautionLv < (StopLv - 1))
{
// 徐行の最大まで(注意状態だけなら止まりはしない)
CautionLv += cntCautuon;
hzUCNT = UpdateCnt;
}
return(CautionLv);
}
