/// <summary>
/// ログ用のバッファクリア
/// </summary>
/// <param name="BrainCtrl"></param>
/// <param name="CersioCt"></param>
/// <param name="LocSys"></param>
public void LogBuffer_Clear(ref Brain BrainCtrl, ref CersioCtrl CersioCt)
{
if (null != CersioCt)
{
CersioCt.hwSendStr = "";
CersioCt.hwResiveStr = "";
}
}
/// <summary>
/// マップイメージログ出力
/// </summary>
/// <param name="BrainCtrl"></param>
/// <returns></returns>
public bool Output_ImageLog(ref Brain BrainCtrl )
{
try
{
// 軌跡ログ出力
if (!string.IsNullOrEmpty(saveLogFname))
{
MarkPoint tgtMaker = null;
// 次の目的地取得
{
int tgtPosX = 0;
int tgtPosY = 0;
double dir = 0;
BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
BrainCtrl.RTS.getNowTargetDir(ref dir);
tgtMaker = new MarkPoint(tgtPosX, tgtPosY, dir);
}
{
Bitmap bmp = BrainCtrl.LocSys.MakeMakerLogBmp(false, tgtMaker);
if (null != bmp)
{
// 画像ファイル保存
string saveImageLogFname = Path.ChangeExtension(saveLogFname, "png");
bmp.Save(saveImageLogFname, System.Drawing.Imaging.ImageFormat.Png);
}
}
}
}
catch (Exception ex)
{
MessageBox.Show(ex.Message + System.Environment.NewLine + ex.StackTrace);
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="g"></param>
/// <param name="baseY"></param>
public void DrawIngicator(Graphics g, int baseY, ref CersioCtrl CersioCt, ref Brain BrainCtrl)
{
{
// Handle
int stX = 20;
int stY = baseY + 8;
int Wd = 200;
int Ht = 15;
float handleVal = (float)((Wd / 2) * (-CersioCtrl.nowSendHandleValue));
if (handleVal > 0)
{
g.FillRectangle(Brushes.Red, stX + Wd / 2, stY, handleVal, Ht);
}
else
{
g.FillRectangle(Brushes.Red, stX + Wd / 2 + handleVal, stY, -handleVal, Ht);
}
g.DrawRectangle(Pens.White, stX, stY, Wd, Ht);
g.DrawLine(Pens.White, stX + Wd / 2, stY, stX + Wd / 2, stY + Ht);
g.DrawString("Handle", fntMini, Brushes.White, stX + Wd / 2 - 38 / 2, stY + Ht - fntMini.GetHeight());
// ACC
stX = 40;
stY = baseY + 30;
Wd = 10;
Ht = 40;
float accVal = (float)(Ht * CersioCtrl.nowSendAccValue);
g.FillRectangle(Brushes.Red, stX, stY + Ht - accVal, Wd, accVal);
g.DrawRectangle(Pens.White, stX, stY, Wd, Ht);
g.DrawString("Acc", fntMini, Brushes.White, stX - 35, stY + Ht - fntMini.GetHeight());
}
// Compus
if (CersioCt.bhwCompass)
{
int ang = CersioCt.hwCompass;
DrawMaker(g, Brushes.Red, 90, baseY + 50, ang, 12);
g.DrawString(ang.ToString(), fntMini, Brushes.White, 90 - 25, baseY + 50);
g.DrawString("Compus", fntMini, Brushes.White, 90 - 25, baseY + 50 + 14);
}
// RE Dir
if (CersioCt.bhwREPlot)
{
double ang = CersioCt.hwREDir;
DrawMaker(g, Brushes.Purple, 150, baseY + 50, ang, 12);
g.DrawString(ang.ToString("F1"), fntMini, Brushes.White, 150 - 25, baseY + 50);
g.DrawString("R.E.Dir", fntMini, Brushes.White, 150 - 25, baseY + 50 + 14);
}
// GPS Dir
if (CersioCt.bhwGPS)
{
if (CersioCt.bhwUsbGPS)
{
// USBGPSは角度あり
DrawMaker(g, Brushes.LimeGreen, 210, baseY + 50, CersioCt.hwGPS_MoveDir, 12);
}
else
{
// bServerGPS 角度なし
DrawMaker(g, Brushes.LimeGreen, 210, baseY + 50, 12);
}
g.DrawString("GPSDir", fntMini, Brushes.White, 210 - 25, baseY + 50 + 14);
}
// 向けたいハンドル角度
{
double ang = BrainCtrl.RTS.getNowTargetStearingDir();
DrawMaker(g, Brushes.Cyan, 40, baseY + 120, ang, 12);
g.DrawString(ang.ToString("F1"), fntMini, Brushes.White, 40 - 25, baseY + 120);
g.DrawString("Handle", fntMini, Brushes.White, 40 - 25, baseY + 120 + 14);
}
// 現在の向き
{
double ang = BrainCtrl.RTS.getNowDir();
DrawMaker(g, Brushes.Red, 100, baseY + 120, ang, 12);
g.DrawString(ang.ToString("F1"), fntMini, Brushes.White, 100 - 25, baseY + 120);
g.DrawString("RTSNow", fntMini, Brushes.White, 100 - 25, baseY + 120 + 14);
}
// 相手の向き
{
double ang = BrainCtrl.RTS.getNowTargetDir();
DrawMaker(g, Brushes.Purple, 160, baseY + 120, ang, 12);
g.DrawString(ang.ToString("F1"), fntMini, Brushes.White, 160 - 25, baseY + 120);
g.DrawString("RTSTgt", fntMini, Brushes.White, 160 - 25, baseY + 120 + 14);
}
}
/// <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));
}
}
//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);
}
}
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);
}
}
/// <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>
/// コンストラクタ
/// </summary>
public VehicleRunnerForm()
{
InitializeComponent();
formLog.init();
// セルシオコントローラ初期化
CersioCt = new CersioCtrl();
// BoxPc(bServer) Emu接続
// 起動時にレスポンスの早いエミュレータをつなぎにいく
// エミュレータがなければ、リトライ時 実機のbServerにつなぎにいく
//bServerConnectAsync();
CersioCt.ConnectBoxPC_Emulator();
// ブレイン起動
BrainCtrl = new Brain(CersioCt, defaultMapFile);
// 初期ボタン設定セット
rb_Move_CheckedChanged(null, null);
rb_Dir_CheckedChanged(null, null);
// 自己位置補正設定 切り替え
cb_VRRevision_CheckedChanged(null, null);
// マップウィンドウサイズのbmp作成
formDraw.MakePictureBoxWorldMap(BrainCtrl.LocSys.worldMap.mapBmp, picbox_AreaMap);
// LRF 入力スケール調整反映
tb_LRFScale.Text = trackBar_LRFViewScale.Value.ToString();
//btm_LRFScale_Click(null, null);
tb_LRFScale_TextChanged(null, null);
// センサー値取得 スレッド起動
Thread trdSensorLRF = new Thread(new ThreadStart(ThreadSensorUpdate_LRF));
trdSensorLRF.IsBackground = true; // アプリの終了とともに終了する
trdSensorLRF.Priority = ThreadPriority.AboveNormal;
trdSensorLRF.Start();
Thread trdSensor = new Thread(new ThreadStart(ThreadSensorUpdate_bServer));
trdSensor.IsBackground = true;
trdSensor.Priority = ThreadPriority.AboveNormal;
trdSensor.Start();
// 位置座標更新 スレッド起動
/*
Thread trdLocalize = new Thread(new ThreadStart(ThreadLocalizationUpdate));
trdLocalize.IsBackground = true;
//trdSensor.Priority = ThreadPriority.AboveNormal;
trdLocalize.Start();
*/
#if EMULATOR_MODE
// LRF エミュレーション
tb_LRFIpAddr.Text = "127.0.0.10";
#endif
}