Example #1
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);
        }
        /// <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())
                    {
                        // 接続OK
                        tb_SendData.BackColor       = Color.Lime;
                        tb_ResiveData.BackColor     = Color.Lime;
                        lb_BServerConnect.Text      = "bServer [" + CersioCt.TCP_GetConnectedAddr() + "] 接続OK";
                        lb_BServerConnect.BackColor = Color.Lime;
                    }
                    else
                    {
                        // 接続NG
                        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();
        }