/// <summary> /// 初期化 /// </summary> public void Init(MapData mapData) { ModeCtrl = new ModeControl(); // チェックポイントへ向かう ModeCtrl.SetActionMode(ModeControl.ActionMode.CheckPoint); // Locpresump // マップ画像ファイル名、実サイズの横[mm], 実サイズ縦[mm] (北向き基準) LocSys = new LocationSystem(mapData); UpdateCnt = 0; // 現在座標リセット Reset_Rooting(true); }
/// <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--; // LEDパターンハザード // スローダウンを知らせる CarCtrl.SetHeadMarkLED((int)CersioCtrl.LED_PATTERN.HAZERD); } else { // LEDパターン平常 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; // スローダウンの速度でバック // LEDパターン // バック中 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> /// 自律走行処理 定期更新(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); }