Exemplo n.º 1
0
        /// <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);
        }
Exemplo n.º 2
0
        /// <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);
        }