/// <summary>
/// 自律走行処理 定期更新(100ms)
/// </summary>
/// <param name="bRunAutonomous"></param>
/// <param name="bMoveBaseCtrl"></param>
/// <param name="SpeedKmh"></param>
/// <returns></returns>
public bool AutonomousProc(bool bRunAutonomous, bool bMoveBaseCtrl, double SpeedKmh)
{
// 自立走行に切り替わった瞬間か？
bool trgRunAutonomous = (bRunAutonomousOld != bRunAutonomous && bRunAutonomous) ? true : false;
UpdateCnt++;
// すべてのルートを回りゴールした。
if (goalFlg)
{
CarCtrl.nowAccValue = 0.0;
CarCtrl.nowHandleValue = 0.0f;
// 停止
CarCtrl.SetCommandAC(0.0, 0.0);
// スマイル
CarCtrl.SetHeadMarkLED(LEDControl.LED_PATTERN.Smile);
return(goalFlg);
}
// 現在座標更新
LocSys.RTS.SetNowPostion(LocSys.GetResultLocationX(),
LocSys.GetResultLocationY(),
LocSys.GetResultAngle());
// チェックポイント送信
{
// チェックポイントをROSへ指示(bServer接続時も配信)
if (LocSys.RTS.TrgCheckPoint() || trg_bServerConnect || trgRunAutonomous)
{
Vector3 checkPnt = LocSys.RTS.GetCheckPointToWayPoint();// LocSys.RTS.getNowCheckPoint();
CarCtrl.SetCommandAG(checkPnt.x, checkPnt.y, checkPnt.z);
}
// 再スタート時
if (trgRunAutonomous)
{
// 0.0を一度送信
ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint);
CarCtrl.SetCommandAC(0.0, 0.0);
CarCtrl.nowAccValue = 0.0;
CarCtrl.nowHandleValue = 0.0;
noFowrdCnt = 0;
noPowerTrainCnt = 0;
}
}
// LED指示
LedUpdate(bRunAutonomous);
// モード更新
ModeCtrl.Update(LocSys.GetResultDistance_mm());
// ルート計算
LocSys.RTS.CalcRooting(bRunAutonomous);
if (bRunAutonomous)
{
// 走行指示出力
/*
* // ROSの回転角度から、Benzハンドル角度の上限に合わせる
* double angLimit = (CarCtrl.hwMVBS_Ang * 180.0 / Math.PI);
* if (angLimit > 30.0) angLimit = 30.0;
* if (angLimit < -30.0) angLimit = -30.0;
*
*
* double moveAng = -(angLimit / 30.0);//-CarCtrl.hwMVBS_Ang;// * 0.3;
*/
double moveAng = CarCtrl.hwMVBS_Ang * 2.0;
if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.CheckPoint)
{
// モード：チェックポイント目指す
if (bMoveBaseCtrl)
{
// move_baseコントロール
if (CarCtrl.hwMVBS_X > 0.0)
{
// move_baseから前進指示の場合
double speedRate = CarCtrl.hwMVBS_X;
// ハンドルで曲がるときは、速度を下げる
if (Math.Abs(moveAng) > 0.25)
{
speedRate *= 0.75;
}
CarCtrl.CalcHandleAccelControl(moveAng, CarCtrl.CalcAccelFromSpeed(SpeedKmh * speedRate, true));
//CarCtrl.SendCalcHandleAccelControl(moveAng, getAccelValue()*0.5);
ModeCtrl.TmpCnt = 0;
}
else
{
// move_baseから前進指示なし
ModeCtrl.TmpCnt++;
// 0.5秒
if (ModeCtrl.TmpCnt > 5)
{
ModeCtrl.SetActionMode(ModeControl.ActionMode.EmergencyStop);
}
}
// ACコマンド送信
CarCtrl.SetCommandAC(CarCtrl.nowHandleValue, CarCtrl.nowAccValue);
}
else
{
// VR コントロール
// ルートにそったハンドル、アクセル値を取得
double handleTgt = LocSys.RTS.GetHandleValue();
//double accTgt = LocSys.RTS.GetAccelValue();
double speedRate = 1.0;
// ハンドルで曲がるときは、速度を下げる
if (Math.Abs(handleTgt) > 0.25)
{
speedRate = 0.75;
}
CarCtrl.CalcHandleAccelControl(handleTgt, CarCtrl.CalcAccelFromSpeed(SpeedKmh * speedRate, true));
// ACコマンド送信
CarCtrl.SetCommandAC(CarCtrl.nowHandleValue, CarCtrl.nowAccValue);
}
// 共通処理
// 動輪の状態確認
if (CarCtrl.sendAccelValue >= 0.1)
{
// 動力出力指示にかかわらず、(速度50mm/s以下)
if (CarCtrl.nowSpeedMmSec <= 50.0)
{
// スタック(前進不能)カウンタ
noFowrdCnt++;
if (noFowrdCnt > 1000)
{
// バックで脱出を試みる
ModeCtrl.SetActionMode(ModeControl.ActionMode.MoveBack);
CarCtrl.nowAccValue = 0.0;
noFowrdCnt = 0;
}
// 動力出力指示にかかわらずほぼ静止(速度10mm/s以下)
if (CarCtrl.nowSpeedMmSec <= 10.0)
{
// 動力カット状態カウンタ
noPowerTrainCnt++;
// 10秒以上経過
if (noPowerTrainCnt > 100)
{
// 出力0%から再スタート
CarCtrl.nowAccValue = 0.0;
noPowerTrainCnt = 0;
}
}
}
else
{
// 指示どおり走行している状態
noPowerTrainCnt = 0;
noFowrdCnt = 0;
}
}
else
{
// 指示通り静止している状態
}
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.EmergencyStop)
{
if (ModeCtrl.GetActionCount() < 5)
{
// 徐行
CarCtrl.nowAccValue = 0.1;
CarCtrl.CalcHandleAccelControl(moveAng, CarCtrl.nowAccValue);
}
else
{
// move_baseから停止状態
//double speedRate = CarCtrl.hwMVBS_X;
//speedRate *= 0.75;
if (CarCtrl.hwMVBS_X > 0.0)
{
// 徐行解除
ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint);
}
else
{
ModeCtrl.TmpCnt++;
// その場、回転
if (Math.Abs(CarCtrl.hwMVBS_Ang) >= 0.01)
{
// 減速 [0.5km/h]
//CarCtrl.CalcHandleAccelControl(0.0, 0.0);
CarCtrl.CalcHandleAccelControl(moveAng, CarCtrl.CalcAccelFromSpeed(0.25, true));
if (ModeCtrl.TmpCnt > 20)
{
// Back モード変更
ModeCtrl.SetActionMode(ModeControl.ActionMode.MoveBack);
CarCtrl.nowAccValue = 0.0;
}
}
else
{
// 停止
if (ModeCtrl.TmpCnt > 20)
{
ModeCtrl.SetActionMode(ModeControl.ActionMode.EmergencyStop);
}
}
}
}
// ACコマンド送信
CarCtrl.SetCommandAC(CarCtrl.nowHandleValue, CarCtrl.nowAccValue);
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.StackStop)
{
// 完全停止
if (ModeCtrl.GetModePassSeconds() > 8)
{
ModeCtrl.SetActionMode(ModeControl.ActionMode.EmergencyStop);
}
}
else if (ModeCtrl.GetActionMode() == ModeControl.ActionMode.MoveBack)
{
// モード：バック動作
if (ModeCtrl.GetActionCount() == 0)
{
// 逆ハンドル設定
if (Math.Abs(CarCtrl.nowTargetHandle) > 0.1)
{
backHandle = -CarCtrl.nowTargetHandle;
}
else
{
backHandle = -LocSys.RTS.GetHandleValue();
}
}
if (ModeCtrl.GetActionCount() < 5)
{
// スピコンブレーキ解除
CarCtrl.SetCommandAC(0.0, 0.0);
}
else
{
// 400mm バック
//if (ModeCtrl.GetModePassSeconds() > 3)
if (ModeCtrl.PassDistanceMm() > 400.0)
{
// 復旧
ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint);
CarCtrl.nowAccValue = 0.0;
// チェックポイント再送
{
Vector3 checkPnt = LocSys.RTS.GetCheckPointToWayPoint();
CarCtrl.SetCommandAG(checkPnt.x, checkPnt.y, checkPnt.z);
}
}
else
{
// ハンドル維持して、バック
CarCtrl.CalcHandleAccelControl(backHandle, CarCtrl.CalcAccelFromSpeed(SpeedKmh * 0.5, false));
}
// ACコマンド送信
CarCtrl.SetCommandAC(CarCtrl.nowHandleValue, CarCtrl.nowAccValue);
}
}
else
{
// モード：未設定
ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint);
}
}
else
{
// 走行指示出力しない
CarCtrl.nowAccValue = 0.0;
CarCtrl.nowHandleValue = 0.0f;
ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint);
}
// 接続トリガ取得
trg_bServerConnect = false;
if (!bServerConnectFlg && CarCtrl.TCP_IsConnected())
{
trg_bServerConnect = true;
}
bServerConnectFlg = CarCtrl.TCP_IsConnected();
bRunAutonomousOld = bRunAutonomous;
// ゴール状態取得
goalFlg = LocSys.RTS.GetGoalStatus();
return(goalFlg);
}
/// <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)
{
LocationSystem LocSys = BrainCtrl.LocSys;
// tm_UpdateHw_Tick
{
// 実走行時、bServerと接続が切れたら再接続
if (updateHwCnt % 100 == 0)
{
// 切断状態なら、自動接続
if (!CersioCt.TCP_IsConnected())
{
//
//CersioCt.Connect_bServer_Async(bServerEmuAddr);
CersioCt.Connect_bServer_Async(bServerAddr);
}
}
// ロータリーエンコーダ（タイヤ回転数）情報
if (CersioCt.bhwRE)
{
//lbl_RErotR.Text = CersioCt.hwRErotR.ToString("f1");
//lbl_RErotL.Text = CersioCt.hwRErotL.ToString("f1");
dataGridViewReceiveData.Rows[(int)GeridRow.RE].Cells[1].Value = "L:" + CersioCt.hwRErotL.ToString("f1");
dataGridViewReceiveData.Rows[(int)GeridRow.RE].Cells[2].Value = "R:" + CersioCt.hwRErotR.ToString("f1");
}
// AMCL
LocSys.Input_ROSPosition(CersioCt.hwAMCL_X, CersioCt.hwAMCL_Y, CersioCt.hwAMCL_Ang);
if (CersioCt.bhwTrgAMCL)
{
// 受信再開時の初期化
LocSys.Reset_ROSPosition(CersioCt.hwAMCL_X, CersioCt.hwAMCL_Y, CersioCt.hwAMCL_Ang);
CersioCt.bhwTrgAMCL = false;
}
// VehicleRunner Plot
{
PointD wlPos = new PointD();
REncoderToMap.CalcWheelPlotXY(ref wlR, ref wlL, ref reAng,
CersioCt.hwRErotR,
CersioCt.hwRErotL,
reOldR,
reOldL);
wlPos.X = (wlR.X + wlL.X) * 0.5;
wlPos.Y = (wlR.Y + wlL.Y) * 0.5;
LocSys.Input_REPosition(wlPos.X, wlPos.Y, -reAng);
reOldR = CersioCt.hwRErotR;
reOldL = CersioCt.hwRErotL;
}
// LED状態 画面表示
if (CersioCt.LEDCtrl.ptnHeadLED == -1)
{
//lbl_LED.Text = "ND";
dataGridViewReceiveData.Rows[(int)GeridRow.LED].Cells[1].Value = "None";
}
else
{
string ledStr = CersioCt.LEDCtrl.ptnHeadLED.ToString();
if (CersioCt.LEDCtrl.ptnHeadLED >= 0 && CersioCt.LEDCtrl.ptnHeadLED < LEDControl.LEDMessage.Count())
{
ledStr += "," + LEDControl.LEDMessage[CersioCt.LEDCtrl.ptnHeadLED];
}
dataGridViewReceiveData.Rows[(int)GeridRow.LED].Cells[1].Value = ledStr;
/*
* if (!ledStr.Equals(lbl_LED.Text))
* {
* lbl_LED.Text = ledStr;
* }*/
}
// 現在座標 表示
//lbl_REPlotX.Text = LocSys.GetResultLocationX().ToString("F2");
//lbl_REPlotY.Text = LocSys.GetResultLocationY().ToString("F2");
//lbl_REPlotDir.Text = LocSys.GetResultAngle().ToString("F2");
dataGridViewReceiveData.Rows[(int)GeridRow.Plot].Cells[1].Value = LocSys.GetResultLocationX().ToString("F2");
dataGridViewReceiveData.Rows[(int)GeridRow.Plot].Cells[2].Value = LocSys.GetResultLocationY().ToString("F2");
dataGridViewReceiveData.Rows[(int)GeridRow.Plot].Cells[3].Value = LocSys.GetResultAngle().ToString("F2");
// CheckPointIndex
if (BrainCtrl.LocSys.RTS.TrgCheckPoint())
{
// チェックポイント通過時に表示更新
numericUD_CheckPoint.Value = BrainCtrl.LocSys.RTS.GetCheckPointIdx();
}
// 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 (!string.IsNullOrEmpty(CersioCt.hwResiveStr))
{
tb_ResiveData.Text = CersioCt.hwResiveStr.Replace('\n', ' ');
CersioCt.hwResiveStr = "";
}
if (!string.IsNullOrEmpty(CersioCt.hwSendStr))
{
tb_SendData.Text = CersioCt.hwSendStr.Replace('\n', ' ');
CersioCt.hwSendStr = "";
}
updateHwCnt++;
}
// マップ上の現在位置更新
// 現在位置を、AMCL,REPlotどちらを使うか
LocationSystem.LOCATION_SENSOR selSensor = (rb_SelAMCL.Checked ? LocationSystem.LOCATION_SENSOR.AMCL : LocationSystem.LOCATION_SENSOR.REPlot);
BrainCtrl.LocSys.update_NowLocation(selSensor);
// 自律走行(緊急ブレーキ、壁よけ含む)処理 更新
double speedKmh = (double)numericUpDownCtrlSpeed.Value;
BrainCtrl.AutonomousProc(bRunAutonomous, cb_MoveBaseControl.Checked, speedKmh);
// 距離計
//tb_Trip.Text = (LocSys.GetResultDistance_mm() * (1.0 / 1000.0)).ToString("f2");
tb_Trip.Text = (CersioCt.nowLengthMm * (1.0 / 1000.0)).ToString("f2");
}
// スピード表示
tb_RESpeed.Text = ((CersioCt.nowSpeedMmSec * 3600.0) / (1000.0 * 1000.0)).ToString("f2"); // Km/Hour
//tb_RESpeed.Text = (CersioCt.SpeedMmSec).ToString("f2"); // mm/Sec
// ACコマンド送信した ハンドル、アクセル値 表示
tb_AccelVal.Text = CersioCt.sendAccelValue.ToString("f2");
tb_HandleVal.Text = CersioCt.sendHandleValue.ToString("f2");
//labelMoveBaseX.Text = CersioCt.hwMVBS_X.ToString("f2");
//labelMoveBaseAng.Text = CersioCt.hwMVBS_Ang.ToString("f2");
// グリッド表示
dataGridViewReceiveData.Rows[(int)GeridRow.MoveBase].Cells[1].Value = CersioCt.hwMVBS_X.ToString("f2");
dataGridViewReceiveData.Rows[(int)GeridRow.MoveBase].Cells[2].Value = CersioCt.hwMVBS_Y.ToString("f2");
dataGridViewReceiveData.Rows[(int)GeridRow.MoveBase].Cells[3].Value = CersioCt.hwMVBS_Ang.ToString("f2");
dataGridViewReceiveData.Rows[(int)GeridRow.CmdVel].Cells[1].Value = CersioCt.nowAccValue.ToString("f2");
dataGridViewReceiveData.Rows[(int)GeridRow.CmdVel].Cells[2].Value = "0.00";
dataGridViewReceiveData.Rows[(int)GeridRow.CmdVel].Cells[3].Value = CersioCt.nowHandleValue.ToString("f2");
dataGridViewReceiveData.Rows[(int)GeridRow.ActionMode].Cells[1].Value = BrainCtrl.ModeCtrl.GetActionModeString();
dataGridViewReceiveData.Rows[(int)GeridRow.StackCnt].Cells[1].Value = BrainCtrl.noPowerTrainCnt.ToString();
dataGridViewReceiveData.Rows[(int)GeridRow.StackCnt].Cells[2].Value = BrainCtrl.noFowrdCnt.ToString();
// 自律走行情報
if (bRunAutonomous)
{
// 動作内容TextBox表示
lbl_ActionMode.Text = "自律走行[" + BrainCtrl.ModeCtrl.GetActionModeString() + "]";
}
else
{
lbl_ActionMode.Text = "モニタリング モード";
}
// 画面描画
PictureUpdate();
}
/// <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();
}