/// <summary>
/// 方眼紙モード
/// </summary>
/// <param name="g"></param>
/// <param name="BrainCtrl"></param>
public void AreaMap_Draw_Ruler(Graphics g, ref Brain BrainCtrl, int canvWidth, int canvHeight)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
int toRulerSize = 50; // 5mのピクセル数
int dfX = LocSys.worldMap.GetWorldX(0) % toRulerSize;
int dfY = LocSys.worldMap.GetWorldY(0) % toRulerSize;
// 横線
for (int iy = 0; iy < canvHeight / toRulerSize; iy++)
{
var P1 = new PointF(0.0f, (float)(iy * toRulerSize));
var P2 = new PointF((float)canvWidth, (float)(iy * toRulerSize));
g.DrawLine(Pens.LightGray, P1, P2);
}
// 縦線
for (int ix = 0; ix < canvWidth / toRulerSize; ix++)
{
var P1 = new PointF((float)(ix * toRulerSize), 0.0f);
var P2 = new PointF((float)(ix * toRulerSize), (float)canvHeight);
g.DrawLine(Pens.LightGray, P1, P2);
}
}
/// <summary>
/// エリアマップ描画
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
public void AreaMap_Draw_Area(Graphics g, ref CersioCtrl CersioCt, ref Brain BrainCtrl)
{
// 書き換えＢＭＰ（追加障害物）描画
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
// エリアマップ描画
//g.FillRectangle(Brushes.Black, 0, 0, picbox_AreaMap.Width, picbox_AreaMap.Height);
g.DrawImage(LocSys.AreaOverlayBmp, 0, 0);
// ターゲット描画
if (null != CersioCt)
{
int tgtPosX, tgtPosY;
double dir = 0;
tgtPosX = tgtPosY = 0;
float olScale = (float)LocSys.AreaOverlayBmp.Width / (float)LocSys.AreaBmp.Width;
BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
BrainCtrl.RTS.getNowTargetDir(ref dir);
MarkPoint tgtMk = new MarkPoint(LocSys.worldMap.GetAreaX(tgtPosX), LocSys.worldMap.GetAreaY(tgtPosY), dir);
DrawMaker(g, olScale, tgtMk, Brushes.GreenYellow, 8);
// ターゲットまでのライン
DrawMakerLine(g, olScale,
new MarkPoint(LocSys.worldMap.GetAreaX(LocSys.R1.X), LocSys.worldMap.GetAreaY(LocSys.R1.Y), 0),
tgtMk,
Pens.Olive, 1);
}
}
/// <summary>
/// GPSから現在位置設定に代入
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void btn_GetGPStoR1_Click(object sender, EventArgs e)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
num_R1X.Value = (int)LocSys.G1.X;
num_R1Y.Value = (int)LocSys.G1.Y;
}
/// <summary>
/// 自律走行モード
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void cb_Autonomous_CheckedChanged(object sender, EventArgs e)
{
if (cb_StartAutonomous.Checked)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
// 現在座標から開始
// 開始時のリセット
LocSys.SetStartPostion((int)(LocSys.R1.X + 0.5),
(int)(LocSys.R1.Y + 0.5),
LocSys.R1.Theta);
// GPS情報があれば GPSの初期値をセット
if (CersioCt.bhwGPS)
{
// 起点情報をセット
LocPreSumpSystem.Set_GPSStart(CersioCt.hwGPS_LandX,
CersioCt.hwGPS_LandY,
(int)(LocSys.R1.X + 0.5),
(int)(LocSys.R1.Y + 0.5));
}
bRunAutonomous = true;
cb_StartAutonomous.BackColor = Color.LimeGreen;
}
else
{
// 停止
bRunAutonomous = false;
CersioCt.SendCommand_Stop();
cb_StartAutonomous.BackColor = SystemColors.Window;
}
}
/// <summary>
/// 現在位置設定をR1にセット
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void btn_ResetR1_Click(object sender, EventArgs e)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
LocSys.R1.X = (double)num_R1X.Value;
LocSys.R1.Y = (double)num_R1Y.Value;
LocSys.R1.Theta = (double)num_R1Dir.Value;
}
/// <summary>
/// タブ切り替え時
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void tabControl_SelectedIndexChanged(object sender, EventArgs e)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
num_R1X.Value = (int)LocSys.R1.X;
num_R1Y.Value = (int)LocSys.R1.Y;
num_R1Dir.Value = (int)LocSys.R1.Theta;
}
/// <summary>
/// 向き入力元変更
/// ラジオボタンクリック
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void rb_Dir_CheckedChanged(object sender, EventArgs e)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
// 向き入力元 センサー切り替え
LocSys.Setting.bDirSrcRePlot = rb_DirREPlot.Checked;
LocSys.Setting.bDirSrcGPS = rb_DirGPS.Checked;
LocSys.Setting.bDirSrcSVO = rb_DirSVO.Checked;
LocSys.Setting.bDirSrcCompus = rb_DirCompus.Checked;
LocSys.Setting.bDirAMCL = rb_DirAMCL.Checked;
}
/// <summary>
/// 移動量入力元変更
/// ラジオボタンクリック
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void rb_Move_CheckedChanged(object sender, EventArgs e)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
// 移動入力元 センサー切り替え
LocSys.Setting.bMoveSrcRePlot = rb_MoveREPlot.Checked;
LocSys.Setting.bMoveSrcGPS = rb_MoveGPS.Checked;
LocSys.Setting.bMoveSrcSVO = rb_MoveSVO.Checked;
LocSys.Setting.bMoveSrcReCompus = rb_MoveREandCompus.Checked;
LocSys.Setting.bMoveSrcPF = rb_MovePF.Checked;
LocSys.Setting.bMoveAMCL = rb_MoveAMCL.Checked;
}
//static int areaMapDrawCnt = 0;
public void AreaMap_Draw_WorldMap(Graphics g, ref CersioCtrl CersioCt, ref Brain BrainCtrl)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
// 全体マップ描画
float viewScale;
g.FillRectangle(Brushes.Black, 0, 0, worldMapBmp.Width, worldMapBmp.Height);
if (((float)LocSys.worldMap.WorldSize.w / (float)worldMapBmp.Width) < ((float)LocSys.worldMap.WorldSize.h / (float)worldMapBmp.Height))
{
viewScale = (float)(1.0 / ((float)LocSys.worldMap.WorldSize.h / (float)worldMapBmp.Height));
}
else
{
viewScale = (float)(1.0 / ((float)LocSys.worldMap.WorldSize.w / (float)worldMapBmp.Width));
}
//g.ResetTransform();
//g.TranslateTransform(-ctrX, -ctrY, MatrixOrder.Append);
//g.RotateTransform((float)layer.wAng, MatrixOrder.Append);
//g.ScaleTransform(viewScale, viewScale, MatrixOrder.Append);
if (null != worldMapBmp)
{
g.DrawImage(worldMapBmp, 0, 0);
}
//g.ResetTransform();
// 各マーカーの位置を描画
LocSys.DrawWorldMap(g, viewScale);
// ターゲット描画
if (null != CersioCt)
{
int tgtPosX, tgtPosY;
double dir = 0;
tgtPosX = tgtPosY = 0;
BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
BrainCtrl.RTS.getNowTargetDir(ref dir);
MarkPoint tgtMk = new MarkPoint(tgtPosX, tgtPosY, dir + 180);
DrawMaker(g, viewScale, tgtMk, Brushes.GreenYellow, 8);
// ターゲットまでのライン
DrawMakerLine(g, viewScale,
LocSys.R1,
tgtMk,
Pens.Olive, 1);
}
}
/// <summary>
/// LRF用のガイドライン描画
/// </summary>
/// <param name="g"></param>
/// <param name="LocSys"></param>
/// <param name="ctrX"></param>
/// <param name="ctrY"></param>
/// <param name="scale"></param>
public void LRF_Draw_GuideLine(Graphics g, ref LocPreSumpSystem LocSys, int ctrX, int ctrY, double scale)
{
// ガイド描画
// 30mを2m区切りで描画
for (int i = 1; i <= 30 / 2; i++)
{
int cirSize = (int)((((i * 2000) * 2) / LocSys.MapToRealScale) * scale);
g.DrawPie(Pens.LightGray,
(ctrX - cirSize / 2), (ctrY - cirSize / 2),
cirSize, cirSize,
-135 - 90, 270);
g.DrawString((i * 2).ToString("F1") + "m", fntMini, Brushes.LightGray, (ctrX - cirSize / 2), (ctrY - cirSize / 2));
}
}
public void AreaMap_Draw_Text(Graphics g, ref Brain BrainCtrl, long updateHwCnt)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
g.ResetTransform();
try
{
// Info
/*
* DrawString(g, 0, drawFont.Height * 0,
* "R1 X:" + ((int)(LocSys.R1.X + 0.5)).ToString("D4") +
* ",Y:" + ((int)(LocSys.R1.Y + 0.5)).ToString("D4") +
* ",角度:" + ((int)LocSys.R1.Theta).ToString("D3"),
* Brushes.Red, Brushes.Black);
*/
/*
* DrawString(g, 0, drawFont.Height * 1,
* "Compass:"******"D3") + "/ ReDir:" + ((int)(CersioCt.hwREDir)).ToString("D3") +
* ",ReX:" + ((int)(CersioCt.hwREX)).ToString("D4") + ",Y:" + ((int)(CersioCt.hwREY)).ToString("D4"),
* Brushes.Blue, Brushes.White);
*/
DrawString(g, 0, drawFont.Height * 1,
"RunCnt:" + updateHwCnt.ToString("D8") + "/ Goal:" + (BrainCtrl.goalFlg ? "TRUE" : "FALSE" + "/ Cp:" + BrainCtrl.RTS.GetNowCheckPointIdx().ToString()),
Brushes.Blue, Brushes.White);
/*
* DrawString(g, 0, drawFont.Height * 2,
* "LocProc:" + LocPreSumpSystem.swCNT_Update.ElapsedMilliseconds +
* "ms /Draw:" + LocSys.swCNT_Draw.ElapsedMilliseconds +
* "ms /MRF:" + LocPreSumpSystem.swCNT_MRF.ElapsedMilliseconds + "ms",
* Brushes.Blue, Brushes.White);
*/
LocPreSumpSystem.swCNT_Update.Reset();
LocPreSumpSystem.swCNT_MRF.Reset();
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
}
}
public void Reset(string mapFileName)
{
ModeCtrl = new ModeControl();
//
MapFile = MapData.LoadMapFile(mapFileName);
//
RTS = new Rooting(MapFile);
// マップ画像ファイル名、実サイズの横[mm], 実サイズ縦[mm] (北向き基準)
LocSys = new LocPreSumpSystem(MapFile.MapImageFileName, MapFile.RealWidth, MapFile.RealHeight);
UpdateCnt = 0;
goalFlg = false;
handleValue = 0.0;
accValue = 0.0;
Reset_StartPosition();
}
/// <summary>
/// LRF接続ボタン
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void cb_LRFConnect_CheckedChanged(object sender, EventArgs e)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
if (cb_LRFConnect.Checked)
{
// LRF接続
// 元のカーソルを保持
Cursor preCursor = Cursor.Current;
// カーソルを待機カーソルに変更
Cursor.Current = Cursors.WaitCursor;
{
int intLrfPot;
if (int.TryParse(tb_LRFPort.Text, out intLrfPot))
{
// 指定のIP,Portでオープン
LocSys.LRF.Open(tb_LRFIpAddr.Text, intLrfPot);
if (LocSys.LRF.IsConnect())
{
// 接続OK
tb_LRFIpAddr.BackColor = Color.Lime;
tb_LRFPort.BackColor = Color.Lime;
}
}
}
// カーソルを元に戻す
Cursor.Current = preCursor;
}
else
{
// LRF切断
LocSys.LRF.Close();
tb_LRFIpAddr.BackColor = SystemColors.Window;
tb_LRFPort.BackColor = SystemColors.Window;
}
}
/// <summary>
/// 緊急停止情報のLRFデータ
/// </summary>
/// <param name="g"></param>
/// <param name="LocSys"></param>
/// <param name="BrainCtrl"></param>
/// <param name="lrfdata"></param>
/// <param name="ctrX"></param>
/// <param name="ctrY"></param>
/// <param name="scale"></param>
/// <param name="picScale"></param>
public void LRF_Draw_PointEBS(Graphics g, ref LocPreSumpSystem LocSys, ref Brain BrainCtrl, double[] lrfdata, int ctrX, int ctrY, double scale, double picScale)
{
{
int stAng = EmergencyBrake.stAng + (int)BrainCtrl.EBS.HandleDiffAngle;
int edAng = EmergencyBrake.edAng + (int)BrainCtrl.EBS.HandleDiffAngle;
int cirSize;
if (BrainCtrl.EBS.CautionLv >= EmergencyBrake.StopLv)
{
// ブレーキレンジ内
Brush colBrs = Brushes.Red;
cirSize = (int)((EmergencyBrake.BrakeRange * 2.0 / LocSys.MapToRealScale) * scale);
g.FillPie(colBrs, (ctrX - cirSize / 2), (ctrY - cirSize / 2),
cirSize, cirSize,
stAng - 90, (edAng - stAng));
}
else if (BrainCtrl.EBS.CautionLv >= EmergencyBrake.SlowDownLv)
{
// スローダウンレンジ内
Brush colBrs = Brushes.Orange;
cirSize = (int)((EmergencyBrake.SlowRange * 2.0 / LocSys.MapToRealScale) * scale);
g.FillPie(colBrs, (ctrX - cirSize / 2), (ctrY - cirSize / 2),
cirSize, cirSize,
stAng - 90, (edAng - stAng));
}
{
Pen colPen = Pens.Yellow;
// スローダウン レンジ枠
cirSize = (int)((EmergencyBrake.SlowRange * 2.0 / LocSys.MapToRealScale) * scale);
g.DrawPie(colPen, (ctrX - cirSize / 2), (ctrY - cirSize / 2),
cirSize, cirSize,
stAng - 90, (edAng - stAng));
// ブレーキ レンジ枠
cirSize = (int)((EmergencyBrake.BrakeRange * 2.0 / LocSys.MapToRealScale) * scale);
g.DrawPie(colPen, (ctrX - cirSize / 2), (ctrY - cirSize / 2),
cirSize, cirSize,
stAng - 90, (edAng - stAng));
}
}
// EHS範囲描画
{
int stAng;
int edAng;
int cirSize = (int)((EmergencyHandring.MaxRange * 2.0 / LocSys.MapToRealScale) * scale);
Pen colPen = Pens.LightGreen;
// 左側
stAng = EmergencyHandring.stLAng;
edAng = EmergencyHandring.edLAng;
if (BrainCtrl.EHS.Result == EmergencyHandring.EHS_MODE.LeftWallHit ||
BrainCtrl.EHS.Result == EmergencyHandring.EHS_MODE.CenterPass)
{
g.FillPie(Brushes.Red, (ctrX - cirSize / 2), (ctrY - cirSize / 2),
cirSize, cirSize,
-stAng - 90, -(edAng - stAng));
}
g.DrawPie(colPen, (ctrX - cirSize / 2), (ctrY - cirSize / 2),
cirSize, cirSize,
-stAng - 90, -(edAng - stAng));
// 右側
stAng = EmergencyHandring.stRAng;
edAng = EmergencyHandring.edRAng;
if (BrainCtrl.EHS.Result == EmergencyHandring.EHS_MODE.RightWallHit ||
BrainCtrl.EHS.Result == EmergencyHandring.EHS_MODE.CenterPass)
{
g.FillPie(Brushes.Red, (ctrX - cirSize / 2), (ctrY - cirSize / 2),
cirSize, cirSize,
-stAng - 90, -(edAng - stAng));
}
g.DrawPie(colPen, (ctrX - cirSize / 2), (ctrY - cirSize / 2),
cirSize, cirSize,
-stAng - 90, -(edAng - stAng));
}
// ノイズリダクションLRF描画
if (lrfdata != null)
{
LRF_Draw_Point(g, Brushes.Cyan, lrfdata, ctrX, ctrY, picScale * (1.0 / LocSys.MapToRealScale));
}
}
/// <summary>
/// 地磁気から現在位置設定に代入
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void btn_GetCompustoR1_Click(object sender, EventArgs e)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
num_R1Dir.Value = (int)LocSys.C1.Theta;
}
// ---------------------------------------------------------------------------------------------------
#region "LRF,Indicatorエリア 描画"
/// <summary>
/// LRFウィンドウデータ描画
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void picbox_LRF_Paint(object sender, PaintEventArgs e)
{
Graphics g = e.Graphics;
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
// LRF取得データを描画
{
int ctrX = picbox_LRF.Width / 2;
int ctrY = picbox_LRF.Height * 6 / 8;
float scale = 1.0f;
// 背景色
switch (selPicboxLRFmode)
{
case (int)LRF_PICBOX_MODE.Normal:
ctrX = picbox_LRF.Width / 2;
ctrY = picbox_LRF.Height * 6 / 10;
picbox_LRF.BackColor = Color.Gray; //Color.White;
scale = 1.0f;
break;
case (int)LRF_PICBOX_MODE.EbsArea:
ctrX = picbox_LRF.Width / 2;
ctrY = picbox_LRF.Height * 6 / 8;
picbox_LRF.BackColor = Color.Black;
scale = 4.0f;
// EBSに反応があればズーム
//scale += ((float)CersioCt.BrainCtrl.EBS_CautionLv * 3.0f / (float)Brain.EBS_CautionLvMax);
// EHS
//if (CersioCt.BrainCtrl.EHS_Result != Brain.EHS_MODE.None) scale = 10.0f;
break;
case (int)LRF_PICBOX_MODE.Indicator:
picbox_LRF.BackColor = Color.Black;
break;
}
if (selPicboxLRFmode == (int)LRF_PICBOX_MODE.Normal ||
selPicboxLRFmode == (int)LRF_PICBOX_MODE.EbsArea)
{
// ガイドライン描画
formDraw.LRF_Draw_GuideLine(g, ref LocSys, ctrX, ctrY, scale);
}
switch (selPicboxLRFmode)
{
case (int)LRF_PICBOX_MODE.Normal:
// LRF描画
if (LocSys.LRF.getData() != null)
{
formDraw.LRF_Draw_Point(g, Brushes.Yellow, LocSys.LRF.getData(), ctrX, ctrY, (LRFViewScale / 1000.0f) * scale * (1.0 / LocSys.MapToRealScale));
}
{
int iH = 80;
//g.FillRectangle(Brushes.Black, 0, picbox_LRF.Height - iH, picbox_LRF.Width, iH);
formDraw.DrawIngicator(g, picbox_LRF.Height - iH, ref CersioCt, ref BrainCtrl);
}
break;
case (int)LRF_PICBOX_MODE.EbsArea:
// EBS範囲描画
formDraw.LRF_Draw_PointEBS(g, ref LocSys, ref BrainCtrl, LocSys.LRF.getData_UntiNoise(), ctrX, ctrY, scale, (LRFViewScale / 1000.0f) * scale);
break;
case (int)LRF_PICBOX_MODE.Indicator:
picbox_LRF.BackColor = Color.Black;
formDraw.DrawIngicator(g, picbox_LRF.Height / 2 - 50, ref CersioCt, ref BrainCtrl);
break;
}
}
}
/// <summary>
/// 定期更新(100ms周期)
/// </summary>
/// <param name="LocSys"></param>
/// <param name="useEBS">緊急ブレーキ</param>
/// <param name="useEHS">壁避け動作</param>
/// <param name="bStraightMode">直進モード</param>
/// <returns>true...バック中(緊急動作しない)</returns>
///
public bool Update(LocPreSumpSystem LocSys, bool useEBS, bool useEHS)
{
// LRFデータ取得
double[] lrfData = LocSys.LRF.getData();
// Rooting ------------------------------------------------------------------------------------------------
ModeCtrl.update();
if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.CheckPoint)
{
// ルート進行
// ルート算定
// 現在座標更新
RTS.setNowPostion((int)LocSys.GetResultLocationX(),
(int)LocSys.GetResultLocationY(),
(int)LocSys.GetResultAngle());
// ルート計算
RTS.calcRooting();
// チェックポイント通過をLEDで伝える
if (RTS.IsCheckPointPass())
{
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.WHITE_FLASH, true);
}
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.Avoid)
{
// 回避ルート進行
// ルート算定
// 現在座標更新
avoidRTS.setNowPostion((int)LocSys.GetResultLocationX(),
(int)LocSys.GetResultLocationY(),
(int)LocSys.GetResultAngle());
// ルート計算
avoidRTS.calcRooting();
// 回避中をLEDで伝える
//CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.WHITE_FLASH, false);
}
// DriveControl -------------------------------------------------------------------------------------------
// マップ情報取得
{
Grid nowGrid = LocSys.GetMapInfo(LocSys.R1);
// マップ情報反映
if (nowGrid == Grid.BlueArea)
{
// スローダウン
EBS.AccelSlowDownCnt = 5;
Brain.addLogMsg += "ColorMap:Blue\n";
}
// 壁の中にいる
if (nowGrid == Grid.RedArea)
{
// 強制補正エリア
Brain.addLogMsg += "LocRivision:ColorMap[Red](マップ情報の位置補正)\n";
bRevisionRequest = true;
}
// 補正実行エリア
{
if (nowGrid == Grid.GreenArea && !bGreenAreaFlg)
{
// 補正指示のエリアに入った
Brain.addLogMsg += "LocRivision:ColorMap[Green](マップ情報の位置補正)\n";
bGreenAreaFlg = true;
bRevisionRequest = true;
}
// 補正指示のエリアを抜けた判定
if (nowGrid != Grid.GreenArea)
{
bGreenAreaFlg = false;
}
}
}
// 動作モードごとの走行情報更新
// ハンドル、アクセルを調整
if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.CheckPoint)
{
// 通常時
// エマージェンシー ブレーキチェック
EBS.EmgBrk = false;
if (null != lrfData && lrfData.Length > 0)
{
//int BrakeLv = CheckEBS(lrfData);
// ノイズ除去モーとで非常停止発動
int BrakeLv = EBS.CheckEBS(LocSys.LRF.getData_UntiNoise(), UpdateCnt);
// 注意Lvに応じた対処(スローダウン指示)
// Lvで段階ごとに分けてるのは、揺れなどによるLRFのノイズ対策
// 瞬間的なノイズでいちいち止まらないように。
if (BrakeLv >= EmergencyBrake.SlowDownLv)
{
EBS.AccelSlowDownCnt = 20;
}
//if (BrakeLv >= EmergencyBrake.StopLv) EBS.EmgBrk = true; // 緊急停止
}
// ルートにそったハンドル、アクセル値を取得
double handleTgt = RTS.getHandleValue();
double accTgt = RTS.getAccelValue();
// 壁回避
if (useEHS)
{
EHS.HandleVal = EHS.CheckEHS(LocSys.LRF.getData_UntiNoise());
if (0.0 != EHS.HandleVal)
{
// 回避ハンドル操作
// ばたつき防止
handleTgt = CersioCtrl.nowSendHandleValue + ((EHS.HandleVal - CersioCtrl.nowSendHandleValue) * CersioCtrl.HandleControlPow);
EBS.AccelSlowDownCnt = 5; // 速度もさげる
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.BLUE);
}
}
// ハンドルで曲がるときは、速度を下げる
if (Math.Abs(handleTgt) > 0.25)
{
EBS.AccelSlowDownCnt = 5;
}
// スローダウン中 動作
if (EBS.AccelSlowDownCnt > 0)
{
// 遅くする
accValue = accTgt = RTS.getAccelValue() * EmergencyBrake.AccSlowdownRate;
EBS.AccelSlowDownCnt--;
// ＬＥＤパターンハザード
// スローダウンを知らせる
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.HAZERD);
}
else
{
// ＬＥＤパターン平常
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.Normal);
}
// 緊急ブレーキ動作
if (EBS.EmgBrk && useEBS)
{
ModeCtrl.SetActionMode(ModeControl.ActionMode.EmergencyStop);
}
handleValue = handleTgt;
accValue = accTgt;
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.EmergencyStop)
{
// 緊急ブレーキ 中
if (null != lrfData && lrfData.Length > 0)
{
int BrakeLv = EBS.CheckEBS(LocSys.LRF.getData_UntiNoise(), UpdateCnt);
// 停止状態の確認
if (BrakeLv >= EmergencyBrake.StopLv)
{
// 10秒経過したらバック動作へ
if (ModeCtrl.GetModePassSeconds(10))
{
ModeCtrl.SetActionMode(ModeControl.ActionMode.MoveBack);
}
}
else
{
// 緊急ブレーキ解除
ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint);
}
}
// 赤
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.RED, true);
accValue = 0.0;
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.MoveBack)
{
// 復帰（バック）モード
// 向かうべき方向とハンドルを逆に切り、バック
double handleTgt = -RTS.getHandleValue();
//double accTgt = RTS.getAccelValue();
// 障害物をみて、ハンドルを切る方向を求める
{
double ehsHandleVal = EHS.CheckEHS(LocSys.LRF.getData_UntiNoise());
if (useEHS && 0.0 != ehsHandleVal)
{
// 左右に壁を検知してたら、優先的によける
if (EHS.Result == EmergencyHandring.EHS_MODE.LeftWallHit ||
EHS.Result == EmergencyHandring.EHS_MODE.RightWallHit)
{
handleTgt = -ehsHandleVal;
}
}
}
handleValue = handleTgt;
accValue = -EmergencyBrake.AccSlowdownRate; // スローダウンの速度でバック
// ＬＥＤパターン
// バック中
CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.UnKnown1, true);
// バック完了、回避ルートを動的計算
if (ModeCtrl.GetModePassSeconds(10))
{
ActiveDetour actDetour = new ActiveDetour(LocSys.worldMap.AreaGridMap, LocSys.LRF.getData());
// 現在位置と到達したい位置で、回避ルートを求める
{
int nowPosX, nowPosY;
double nowAng;
int tgtPosX = 0;
int tgtPosY = 0;
RTS.getNowPostion(out nowPosX, out nowPosY, out nowAng);
RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
nowPosX = LocSys.worldMap.GetAreaX(nowPosX);
nowPosY = LocSys.worldMap.GetAreaX(nowPosY);
tgtPosX = LocSys.worldMap.GetAreaX(tgtPosX);
tgtPosY = LocSys.worldMap.GetAreaX(tgtPosY);
// 回避ルートを求める
List <Vector3> avoidCheckPoint = actDetour.Calc_DetourCheckPoint(nowPosX, nowPosY, tgtPosX, tgtPosY);
if (avoidCheckPoint.Count > 0)
{
// 回避ルートあり
// 新規ルートを作成
MapData avoidMap = new MapData();
avoidMap.checkPoint = avoidCheckPoint.ToArray();
avoidMap.MapName = "ActiveDetour AvoidMap";
avoidRTS = new Rooting(avoidMap);
// 回避情報を画面に表示
// 回避イメージ生成
AvoidRootImage = actDetour.getDetourRootImage();
// イメージ表示時間設定
AvoidRootDispTime = DateTime.Now.AddSeconds(15);
// 回避ルートモード
ModeCtrl.SetActionMode(ModeControl.ActionMode.Avoid);
}
else
{
// 回避ルートなし(回避不能)
// ※動作検討
// とりあえず、チェックポイントへ向かう
ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint);
}
}
}
}
UpdateCnt++;
// 回復モードか否か
if (ModeCtrl.GetActionMode() != ModeControl.ActionMode.MoveBack)
{
return(false);
}
return(true);
}
/// <summary>
/// VehicleRunnerログ出力
/// </summary>
public void Output_VRLog(ref Brain BrainCtrl, ref CersioCtrl CersioCt)
{
System.IO.StreamWriter sw = new System.IO.StreamWriter(saveLogFname, true, System.Text.Encoding.GetEncoding("shift_jis"));
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
// 固有識別子 + 時間
sw.Write("LocPresumpLog:" + DateTime.Now.ToString("yyyy/MM/dd HH:mm:ss.fff") + System.Environment.NewLine);
// ハードウェア情報
if (CersioCt.TCP_IsConnected())
{
if (null != CersioCt.hwSendStr)
{
sw.Write("hwSendStr:" + CersioCt.hwSendStr.Replace('\n', ' ') + System.Environment.NewLine);
}
if (null != CersioCt.hwResiveStr)
{
sw.Write("hwResiveStr:" + CersioCt.hwResiveStr + System.Environment.NewLine);
}
sw.Write("handle:" + CersioCtrl.nowSendHandleValue + " / acc:" + CersioCtrl.nowSendAccValue + System.Environment.NewLine);
}
else
{
sw.Write("Comment:No Connect BoxPC" + System.Environment.NewLine);
}
// 位置情報
{
sw.Write("R1:X " + LocSys.R1.X.ToString("f3") +
"/Y " + LocSys.R1.Y.ToString("f3") +
"/ Dir " + LocSys.R1.Theta.ToString("f2") +
System.Environment.NewLine);
sw.Write("V1:X " + LocSys.V1.X.ToString("f3") +
"/Y " + LocSys.V1.Y.ToString("f3") +
"/ Dir " + LocSys.V1.Theta.ToString("f2") +
System.Environment.NewLine);
sw.Write("E1:X " + LocSys.E1.X.ToString("f3") +
"/Y " + LocSys.E1.Y.ToString("f3") +
"/ Dir " + LocSys.E1.Theta.ToString("f2") +
System.Environment.NewLine);
sw.Write("C1:X " + LocSys.C1.X.ToString("f3") +
"/Y " + LocSys.C1.Y.ToString("f3") +
"/ Dir " + LocSys.C1.Theta.ToString("f2") +
System.Environment.NewLine);
sw.Write("G1:X " + LocSys.G1.X.ToString("f3") +
"/Y " + LocSys.G1.Y.ToString("f3") +
"/ Dir " + LocSys.G1.Theta.ToString("f2") +
System.Environment.NewLine);
}
//
{
// Rooting情報
{
sw.Write("RTS_TargetIndex:" + BrainCtrl.RTS.GetNowCheckPointIdx() + System.Environment.NewLine);
int tgtPosX = 0;
int tgtPosY = 0;
double tgtDir = 0;
BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
BrainCtrl.RTS.getNowTargetDir(ref tgtDir);
sw.Write("RTS_TargetPos:X " + tgtPosX.ToString("f3") +
"/Y " + tgtPosY.ToString("f3") +
"/Dir " + tgtDir.ToString("f2") +
System.Environment.NewLine);
sw.Write("RTS_Handle:" + BrainCtrl.RTS.getHandleValue().ToString("f2") + System.Environment.NewLine);
}
// Brain情報
{
sw.Write("EBS_CautionLv:" + BrainCtrl.EBS.CautionLv.ToString() + System.Environment.NewLine);
sw.Write("EHS_Handle:" + BrainCtrl.EHS.HandleVal.ToString("f2") + "/Result " + EmergencyHandring.ResultStr[(int)BrainCtrl.EHS.Result] + System.Environment.NewLine);
}
// CersioCtrl
{
sw.Write("GoalFlg:" + (BrainCtrl.goalFlg ? "TRUE" : "FALSE") + System.Environment.NewLine);
if (CersioCt.bhwCompass)
{
sw.Write("hwCompass:"******"hwREPlot:X " + CersioCt.hwREX.ToString("f3") +
"/Y " + CersioCt.hwREY.ToString("f3") +
"/Dir " + CersioCt.hwREDir.ToString("f2") +
System.Environment.NewLine);
}
if (CersioCt.bhwGPS)
{
sw.Write("hwGPS:X " + CersioCt.hwGPS_LandX.ToString("f5") +
"/Y " + CersioCt.hwGPS_LandY.ToString("f5") +
"/Dir " + CersioCt.hwGPS_MoveDir.ToString("f2") +
System.Environment.NewLine);
}
}
// 特記事項メッセージ出力
if (Brain.addLogMsg != null)
{
sw.Write("AddLog:" + Brain.addLogMsg + System.Environment.NewLine);
}
Brain.addLogMsg = null;
}
// 改行
sw.Write(System.Environment.NewLine);
//閉じる
sw.Close();
}
/// <summary>
/// 描画と親密な定期処理
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void tm_Update_Tick(object sender, EventArgs e)
{
if (null != BrainCtrl && null != BrainCtrl.LocSys)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
// tm_UpdateHw_Tick
{
#if !UnUseLRF
// LRF接続状況 表示
if (LocSys.LRF.IsConnect())
{
// LRFから取得
if (LocSys.LRF.isGetDatas)
{
lb_LRFResult.Text = "OK"; // 接続して、データも取得
}
else
{
lb_LRFResult.Text = "OK(noData)"; // 接続しているが、データ取得ならず
}
}
else
{
// 仮想マップから取得
lb_LRFResult.Text = "Disconnect";
}
#endif
// 実走行時、bServerと接続が切れたら再接続
if (updateHwCnt % 100 == 0)
{
// 状態を見て、自動接続
if (!CersioCt.TCP_IsConnected())
{
CersioCt.ConnectBoxPC_Async();
}
}
// ロータリーエンコーダ(Plot座標)情報
if (CersioCt.bhwREPlot)
{
// 自己位置に REPlot情報を渡す
LocSys.Input_RotaryEncoder(CersioCt.hwREX, CersioCt.hwREY, CersioCt.hwREDir);
// 受信情報を画面に表示
lbl_REPlotX.Text = CersioCt.hwREX.ToString("f1");
lbl_REPlotY.Text = CersioCt.hwREY.ToString("f1");
lbl_REPlotDir.Text = CersioCt.hwREDir.ToString("f1");
}
// ロータリーエンコーダ（タイヤ回転数）情報
if (CersioCt.bhwRE)
{
if (CersioCt.bhwCompass)
{
// 自己位置に情報を渡す
LocSys.Input_RotaryEncoder_Pulse(CersioCt.hwRErotL, CersioCt.hwRErotR, CersioCt.hwCompass);
}
lbl_RErotR.Text = CersioCt.hwRErotR.ToString("f1");
lbl_RErotL.Text = CersioCt.hwRErotL.ToString("f1");
}
// 地磁気情報
if (CersioCt.bhwCompass)
{
// 自己位置推定に渡す
LocSys.Input_Compass(CersioCt.hwCompass);
// 画面表示
lbl_Compass.Text = CersioCt.hwCompass.ToString();
}
// GPS情報
if (CersioCt.bhwGPS)
{
// 途中からでもGPSのデータを拾う
if (!LocPreSumpSystem.bEnableGPS)
{
// 応対座標算出のために、取得開始時点のマップ座標とGPS座標をリンク
LocPreSumpSystem.Set_GPSStart(CersioCt.hwGPS_LandX,
CersioCt.hwGPS_LandY,
(int)(LocSys.R1.X + 0.5),
(int)(LocSys.R1.Y + 0.5));
}
// 自己位置推定に渡す
if (CersioCt.bhwUsbGPS)
{
// 角度情報あり
LocSys.Input_GPSData(CersioCt.hwGPS_LandX, CersioCt.hwGPS_LandY, CersioCt.hwGPS_MoveDir);
}
else
{
// 角度情報なし
LocSys.Input_GPSData(CersioCt.hwGPS_LandX, CersioCt.hwGPS_LandY);
}
// 画面表示
lbl_GPS_Y.Text = CersioCt.hwGPS_LandY.ToString("f5");
lbl_GPS_X.Text = CersioCt.hwGPS_LandX.ToString("f5");
}
// ROS LRF取得
if (rb_LRF_ROSnode.Checked)
{
// LRFのデータを外部から入力
LocSys.LRF.SetExtData(CersioCt.GetROS_LRFdata());
}
// AMCL
LocSys.Input_AMCLData(CersioCt.hwAMCL_X, CersioCt.hwAMCL_Y, CersioCt.hwAMCL_Ang);
// LED状態 画面表示
if (CersioCt.ptnHeadLED == -1)
{
lbl_LED.Text = "ND";
}
else
{
string ledStr = CersioCt.ptnHeadLED.ToString();
if (CersioCt.ptnHeadLED >= 0 && CersioCt.ptnHeadLED < CersioCtrl.LEDMessage.Count())
{
ledStr += "," + CersioCtrl.LEDMessage[CersioCt.ptnHeadLED];
}
if (!ledStr.Equals(lbl_LED.Text))
{
lbl_LED.Text = ledStr;
}
}
// BoxPC接続状態確認
if (CersioCt.TCP_IsConnected())
{
// 接続ＯＫ
tb_SendData.BackColor = Color.Lime;
tb_ResiveData.BackColor = Color.Lime;
lb_BServerConnect.Text = "bServer [" + CersioCt.TCP_GetConnectedAddr() + "] 接続OK";
lb_BServerConnect.BackColor = Color.Lime;
}
else
{
// 接続ＮＧ
tb_SendData.BackColor = SystemColors.Window;
tb_ResiveData.BackColor = SystemColors.Window;
lb_BServerConnect.Text = "bServer 未接続";
lb_BServerConnect.BackColor = SystemColors.Window;
}
// 送受信文字 画面表示
if (null != CersioCt.hwResiveStr)
{
tb_ResiveData.Text = CersioCt.hwResiveStr.Replace('\n', ' ');
}
if (null != CersioCt.hwSendStr)
{
tb_SendData.Text = CersioCt.hwSendStr.Replace('\n', ' ');
}
// USB GPSからの取得情報画面表示
if (null != usbGPS)
{
tb_SirialResive.Text = usbGPS.resiveStr;
if (!string.IsNullOrEmpty(usbGPS.resiveStr))
{
CersioCt.usbGPSResive.Add(usbGPS.resiveStr);
}
usbGPS.resiveStr = "";
if (CersioCt.usbGPSResive.Count > 30)
{
CersioCt.usbGPSResive.RemoveRange(0, CersioCt.usbGPSResive.Count - 30);
}
}
// bServerエミュレーション表記
lbl_bServerEmu.Visible = CersioCt.bServerEmu;
updateHwCnt++;
}
// マップ上の現在位置更新
BrainCtrl.LocSys.update_NowLocation();
// パーティクルフィルター自己位置推定 演算
if (cb_VRRevision.Checked)
{
// 常時PF演算
if (cb_AlwaysPFCalc.Checked)
{
BrainCtrl.LocSys.ParticleFilter_Update();
}
// 自己位置補正執行判断
{
// 補正執行フラグ
bool bRevisionIssue = false;
// ボタン操作、またはマップ情報から 補正要請があるか？
if ((bLocRivisionTRG || BrainCtrl.bRevisionRequest))
{
bRevisionIssue = true;
bLocRivisionTRG = false;
}
// 一定時間で更新
if ((updateMainCnt % 30) == 0 && !cb_DontAlwaysRivision.Checked)
{
Brain.addLogMsg += "LocRivision:Timer(時間ごとの位置補正)\n";
bRevisionIssue = true;
}
if (bRevisionIssue)
{
string logMsg;
// 自己位置補正
logMsg = LocSys.LocalizeRevision(rb_UseGPS_Revision.Checked);
// REPlotを補正後の位置座標でリセット
{
CersioCt.SendCommand_RE_Reset();
CersioCt.setREPlot_Start(LocSys.E1.X * LocSys.MapToRealScale,
LocSys.E1.Y * LocSys.MapToRealScale,
LocSys.E1.Theta);
}
Brain.addLogMsg += logMsg;
}
}
}
// MAP座標更新処理
LocSys.MapArea_Update();
// 自律走行(緊急ブレーキ、壁よけ含む)処理 更新
BrainCtrl.AutonomousProc(cb_EmgBrake.Checked,
cb_EHS.Checked,
cb_InDoorMode.Checked,
bRunAutonomous);
}
// REからのスピード表示
tb_RESpeed.Text = CersioCtrl.SpeedMH.ToString("f1");
// ハンドル、アクセル値 表示
tb_AccelVal.Text = CersioCtrl.nowSendAccValue.ToString("f2");
tb_HandleVal.Text = CersioCtrl.nowSendHandleValue.ToString("f2");
// 自律走行情報
if (bRunAutonomous)
{
// 動作内容TextBox表示
// エマージェンシーブレーキ 動作カラー表示
if (null != BrainCtrl)
{
if (BrainCtrl.EBS.EmgBrk && cb_EmgBrake.Checked)
{
cb_EmgBrake.BackColor = Color.Red;
}
else
{
cb_EmgBrake.BackColor = SystemColors.Control;
}
if (BrainCtrl.EHS.Result != EmergencyHandring.EHS_MODE.None && cb_EHS.Checked)
{
cb_EHS.BackColor = Color.Orange;
}
else
{
cb_EHS.BackColor = SystemColors.Control;
}
// UntiEBS Cnt
lbl_BackCnt.Text = "EBS cnt:" + BrainCtrl.EmgBrakeContinueCnt.ToString();
//lbl_BackProcess.Visible = BrainCtrl.bNowBackProcess;
lbl_BackProcess.Text = BrainCtrl.ModeCtrl.GetActionModeString();
}
}
else
{
lbl_BackProcess.Text = "モニタリング";
}
#if LOGWRITE_MODE
// 自律走行、またはロギング中
if (bRunAutonomous || bRunMappingAndLogging)
{
// ログファイル出力
formLog.Output_VRLog(ref BrainCtrl, ref CersioCt);
#if LRFLOG_OUTPUT // ＬＲＦログ出力
// LRFログ出力
// データ量が多いので、周期の長い定期処理で実行
if (LocSys.LRF.IsConnect() && null != LocSys.LRF.getData())
{
formLog.Output_LRFLog(LocSys.LRF.getData());
}
#endif // LRFLOG_OUTPUT
#if GPSLOG_OUTPUT // GPSログ出力
if (null != usbGPS)
{
// ログ出力
formLog.Output_GPSLog(usbGPS.resiveStr);
}
#endif // GPSLOG_OUTPUT
}
#endif // LOGWRITE_MODE
// ログバッファクリア
formLog.LogBuffer_Clear(ref BrainCtrl, ref CersioCt);
// 画面描画
PictureUpdate();
}
