Browse Source

add FSM logic

chenxiaowei 3 years ago
parent
commit
0233e62d50

+ 1 - 1
autodeploy_ok.sh

@@ -32,7 +32,7 @@ detection_chassis
 controller_hapo
 controller_hapo
 
 
 ######decition######
 ######decition######
-decition_brain_sf
+#decition_brain_sf
 
 
 ######tools########
 ######tools########
 view_pointcloud
 view_pointcloud

+ 17 - 2
src/decition/common/common/car_status.h

@@ -232,8 +232,8 @@ namespace iv {
          //巡逻车增加停车点位.end
          //巡逻车增加停车点位.end
 
 
        bool mbReceiveTask=false; //js 的车辆增加状态机
        bool mbReceiveTask=false; //js 的车辆增加状态机
-       bool mbFulfilTask=false;
-       bool mbCloseToPath =false;
+       //bool mbFulfilTask=false;
+       //bool mbCloseToPath =false;
        bool mbAwayFromPath=false;
        bool mbAwayFromPath=false;
        int  mnReceiveTaskCnt=0;
        int  mnReceiveTaskCnt=0;
        bool mbNoPath=false;
        bool mbNoPath=false;
@@ -242,6 +242,21 @@ namespace iv {
        //超声波信息
        //超声波信息
        //std::vector<iv::ultrasonic::Area> mbUltraArea;
        //std::vector<iv::ultrasonic::Area> mbUltraArea;
        float mbUltraDis[4]={200,200,200,200};
        float mbUltraDis[4]={200,200,200,200};
+
+       //状态机信息,beangin
+       int mbFSM_State=0; //0:待命停车   1:任务中	 2:人工接管 3:停工停车 4:返程 5:返库
+       int mbLastFSM_State=0;
+       //接收共享内存其他模块传递出来的信息作状态机条件跳变
+       bool mbErrState = 0;//1出现故障,0 无故障,chassis
+       int mbStartStopWork = 2;//开工指令1 停工指令0
+       int mbTakeOverEn = 2; //进入接管1    退出接管0
+       bool mbOvertimeEn = 0;    //超时1  未超时0
+       int mbOvertimeCnt=0;//计数到一定数目还没收到任务,就记为超时
+       int mbTaskCmdType = 0;   //收到任务1    完成任务2   取消任务3
+       bool mbCloseToPath = 2; //靠近路线1   远离路线0
+       bool mbFullFillTask = 2;  //到达终点1  未达终点0
+       bool mbArriveInitPos=0;//到达初始位置1,未到达是0
+       bool mbArriveServicePos=0;//到达维护位置1,未到达是0
     };
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全,注意多线程加锁,单例模式
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全,注意多线程加锁,单例模式
 }
 }

+ 226 - 3
src/decition/decition_brain_sf_jsrunlegs/decition/brain.cpp

@@ -67,6 +67,11 @@ iv::decition::BrainDecition * gbrain;
             gbrain->UpdateUltra(strdata,nSize); //超声波数据
             gbrain->UpdateUltra(strdata,nSize); //超声波数据
         }
         }
 
 
+        void ListenFSMSkipCmd(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+        {
+            gbrain->UpdateFSMSkipCmd(strdata,nSize); //状态机跳转命令
+        }
+
     /*    void ListenMap_change_req(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
     /*    void ListenMap_change_req(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
         {
             iv::formation_map_index::map map;
             iv::formation_map_index::map map;
@@ -155,6 +160,7 @@ iv::decition::BrainDecition::BrainDecition()
 
 
     mpultraArea=iv::modulecomm::RegisterRecv("ultra_area",iv::decition::ListenUltraArea);//监听超声波数据
     mpultraArea=iv::modulecomm::RegisterRecv("ultra_area",iv::decition::ListenUltraArea);//监听超声波数据
 
 
+    mpFSMSkipCmd = iv::modulecomm::RegisterRecv("FSMSkipCommand",iv::decition::ListenFSMSkipCmd);
 
 
     mTime.start();
     mTime.start();
     mnOldTime = mTime.elapsed();
     mnOldTime = mTime.elapsed();
@@ -472,6 +478,208 @@ void iv::decition::BrainDecition::run() {
 
 
     while (eyes->isAllSensorRunning() &&(!boost::this_thread::interruption_requested()))
     while (eyes->isAllSensorRunning() &&(!boost::this_thread::interruption_requested()))
     {
     {
+        //有限状态机逻辑,begin
+              if(ServiceCarStatus.mbFSM_State==0)//待命
+               {
+                       ServiceCarStatus.mbRunPause=true;
+                    //待命停车
+                       if(ServiceCarStatus.mbOvertimeCnt<=3000)//待命计时
+                       {
+                           ServiceCarStatus.mbOvertimeCnt++;
+                       }
+                       else
+                       {
+                           ServiceCarStatus.mbOvertimeEn=1;
+                       }
+                       //
+                       if(ServiceCarStatus.mbErrState == 1&&ServiceCarStatus.speed==0.0)//出现故障
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 3;
+                           ServiceCarStatus.mbLastFSM_State=0;
+                       }
+                       //
+                       if(ServiceCarStatus.mbStartStopWork == 0)//停工指令
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 5;
+                           ServiceCarStatus.mbLastFSM_State=0;
+                       }
+                       //
+                       if(ServiceCarStatus.mbOvertimeEn == 1)//待命超时
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 4;
+                           ServiceCarStatus.mbLastFSM_State=0;
+                       }
+                       //
+                       if(ServiceCarStatus.mbTakeOverEn == 1&&ServiceCarStatus.speed==0.0)//接管指令
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 2;
+                           ServiceCarStatus.mbLastFSM_State = 0;
+                       }
+                       //
+                       if(ServiceCarStatus.mbTaskCmdType == 1)//收到任务(目的地)
+                       {
+                           ServiceCarStatus.mbRunPause=false; //true车辆不动,false车辆动
+                           ServiceCarStatus.mbFSM_State = 1;
+                           ServiceCarStatus.mbLastFSM_State=0;
+                       }
+              }
+              else if(ServiceCarStatus.mbFSM_State==1)
+               {
+                       //
+                       if((ServiceCarStatus.mbTaskCmdType == 2 || ServiceCarStatus.mbTaskCmdType == 3)&&ServiceCarStatus.speed==0.0)  //完成/取消任务
+                       {
+                           ServiceCarStatus.mbRunPause=true;
+                           ServiceCarStatus.mbOvertimeEn=0;
+                           ServiceCarStatus.mbOvertimeCnt=0;
+                           ServiceCarStatus.mbFSM_State = 0;
+                           ServiceCarStatus.mbLastFSM_State=1;
+                       }
+                       //
+                       if(ServiceCarStatus.mbStartStopWork == 0)//停工指令
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 5;
+                           ServiceCarStatus.mbLastFSM_State=1;
+                       }
+                       //
+                       if(ServiceCarStatus.mbTakeOverEn == 1&&ServiceCarStatus.speed==0.0)//接管指令
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 2;
+                           ServiceCarStatus.mbLastFSM_State=1;
+                       }
+                       //
+                       if(ServiceCarStatus.mbErrState ==1&&ServiceCarStatus.speed==0.0)//出现故障
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 3;
+                           ServiceCarStatus.mbLastFSM_State=1;
+                       }
+                }
+              else if(ServiceCarStatus.mbFSM_State==2)
+               {
+                       //ServiceCarStatus.mbRunPause=true;
+                   //
+                       if((ServiceCarStatus.mbTakeOverEn == 0)&& (ServiceCarStatus.mbLastFSM_State == 3))//退出接管 && 曾在停工状态
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 3;
+                           ServiceCarStatus.mbLastFSM_State=2;
+                       }
+                       //
+                       if((ServiceCarStatus.mbTakeOverEn == 0)&& (ServiceCarStatus.mbLastFSM_State == 5)
+                               &&(ServiceCarStatus.mbCloseToPath == 1))
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 5;
+                           ServiceCarStatus.mbLastFSM_State=2;
+                       }
+                       //
+                       if((ServiceCarStatus.mbTakeOverEn == 0)&& (ServiceCarStatus.mbLastFSM_State == 4)
+                               &&(ServiceCarStatus.mbCloseToPath == 1))
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 4;
+                           ServiceCarStatus.mbLastFSM_State=2;
+                       }
+                       if((ServiceCarStatus.mbTakeOverEn == 0) && (ServiceCarStatus.mbLastFSM_State == 0))//退出接管 && 远离路线
+                       {
+                           ServiceCarStatus.mbRunPause=true;
+                           ServiceCarStatus.mbFSM_State = 0;
+                           ServiceCarStatus.mbOvertimeEn=0;
+                           ServiceCarStatus.mbOvertimeCnt=0;
+                           ServiceCarStatus.mbLastFSM_State=2;
+                       }
+                       if((ServiceCarStatus.mbTakeOverEn == 0) && (ServiceCarStatus.mbLastFSM_State == 1)
+                               &&(ServiceCarStatus.mbCloseToPath == 1))//退出接管 && 远离路线
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 1;
+                           ServiceCarStatus.mbLastFSM_State=2;
+                       }
+                 }
+              else if(ServiceCarStatus.mbFSM_State==3)
+               {
+                       if(ServiceCarStatus.mbStartStopWork == 1)//开工指令
+                       {
+                           ServiceCarStatus.mbRunPause=true;
+                           ServiceCarStatus.mbOvertimeEn=0;
+                           ServiceCarStatus.mbOvertimeCnt=0;
+                           ServiceCarStatus.mbFSM_State = 0;
+                           ServiceCarStatus.mbLastFSM_State=3;
+                       }
+                       if(ServiceCarStatus.mbTakeOverEn == 1&&ServiceCarStatus.speed==0.0)//接管指令
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 2;
+                           ServiceCarStatus.mbLastFSM_State=3;
+                       }
+               }
+              else if(ServiceCarStatus.mbFSM_State==4)
+               {
+                   //
+                       if(ServiceCarStatus.mbLastFSM_State==0)
+                       {
+                           ServiceCarStatus.mbOvertimeEn=0;
+                           ServiceCarStatus.mbOvertimeCnt=0;
+                       }
+                        //
+                       if(ServiceCarStatus.mbErrState ==1&&ServiceCarStatus.speed==0.0)//出现故障
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 3;
+                           ServiceCarStatus.mbLastFSM_State=4;
+                       }
+                       //
+                       if(ServiceCarStatus.mbArriveInitPos == 1&&ServiceCarStatus.speed==0.0)
+                       {
+                           ServiceCarStatus.mbRunPause=true;
+                           ServiceCarStatus.mbFSM_State = 0;
+                           ServiceCarStatus.mbOvertimeEn=0;
+                           ServiceCarStatus.mbOvertimeCnt=0;
+                           ServiceCarStatus.mbLastFSM_State=4;
+                       }
+                       //
+                       if(ServiceCarStatus.mbTakeOverEn == 1&&ServiceCarStatus.speed==0.0)//接管指令
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 2;
+                           ServiceCarStatus.mbLastFSM_State=4;
+                       }
+                       //
+                       if(ServiceCarStatus.mbStartStopWork == 0)//
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 5;
+                           ServiceCarStatus.mbLastFSM_State=4;
+                       }
+                 }
+              else if(ServiceCarStatus.mbFSM_State==5)
+               {
+                   //
+                       if((ServiceCarStatus.mbArriveServicePos == 1)||(ServiceCarStatus.mbErrState == 1))//退出接管 && 曾在停工状态
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 3;
+                           ServiceCarStatus.mbLastFSM_State=5;
+                       }
+                       //
+                       if(ServiceCarStatus.mbTakeOverEn == 1&&ServiceCarStatus.speed==0.0)//接管指令
+                       {
+                           ServiceCarStatus.mbRunPause=false;
+                           ServiceCarStatus.mbFSM_State = 2;
+                           ServiceCarStatus.mbLastFSM_State=5;
+                       }
+               }
+              else
+              {
+               ;
+              }
+               //有限状态机逻辑,end
         if(navigation_data.size() <1)
         if(navigation_data.size() <1)
         {
         {
             iv::map::mapreq x;
             iv::map::mapreq x;
@@ -489,7 +697,7 @@ void iv::decition::BrainDecition::run() {
             delete str;
             delete str;
 
 
             ServiceCarStatus.mbReceiveTask=false;
             ServiceCarStatus.mbReceiveTask=false;
-            ServiceCarStatus.mbFulfilTask = false;
+            ServiceCarStatus.mbFullFillTask = false;
             ServiceCarStatus.mbCloseToPath = false;
             ServiceCarStatus.mbCloseToPath = false;
             ServiceCarStatus.mbAwayFromPath = false;
             ServiceCarStatus.mbAwayFromPath = false;
             ServiceCarStatus.mnReceiveTaskCnt=0;
             ServiceCarStatus.mnReceiveTaskCnt=0;
@@ -550,7 +758,7 @@ void iv::decition::BrainDecition::run() {
             xbs.set_decision_period(decision_period);
             xbs.set_decision_period(decision_period);
             xbs.set_mbawayfrompath(ServiceCarStatus.mbAwayFromPath);
             xbs.set_mbawayfrompath(ServiceCarStatus.mbAwayFromPath);
             xbs.set_mbclosetopath(ServiceCarStatus.mbCloseToPath);
             xbs.set_mbclosetopath(ServiceCarStatus.mbCloseToPath);
-            xbs.set_mbfulfiltask(ServiceCarStatus.mbFulfilTask);
+            xbs.set_mbfulfiltask(ServiceCarStatus.mbFullFillTask);
             xbs.set_mbreceivetask(ServiceCarStatus.mbReceiveTask);
             xbs.set_mbreceivetask(ServiceCarStatus.mbReceiveTask);
             xbs.set_mbnopath(ServiceCarStatus.mbNoPath);
             xbs.set_mbnopath(ServiceCarStatus.mbNoPath);
             ShareBrainstate(xbs);    //apollo_fu 20200413 qudiaozhushi
             ShareBrainstate(xbs);    //apollo_fu 20200413 qudiaozhushi
@@ -892,7 +1100,7 @@ void iv::decition::BrainDecition::run() {
             xbs.set_decision_period(decision_period);
             xbs.set_decision_period(decision_period);
             xbs.set_mbawayfrompath(ServiceCarStatus.mbAwayFromPath);
             xbs.set_mbawayfrompath(ServiceCarStatus.mbAwayFromPath);
             xbs.set_mbclosetopath(ServiceCarStatus.mbCloseToPath);
             xbs.set_mbclosetopath(ServiceCarStatus.mbCloseToPath);
-            xbs.set_mbfulfiltask(ServiceCarStatus.mbFulfilTask);
+            xbs.set_mbfulfiltask(ServiceCarStatus.mbFullFillTask);
             xbs.set_mbreceivetask(ServiceCarStatus.mbReceiveTask);
             xbs.set_mbreceivetask(ServiceCarStatus.mbReceiveTask);
             xbs.set_mbnopath(ServiceCarStatus.mbNoPath);
             xbs.set_mbnopath(ServiceCarStatus.mbNoPath);
             ShareBrainstate(xbs);
             ShareBrainstate(xbs);
@@ -1671,6 +1879,21 @@ void iv::decition::BrainDecition::UpdateUltra(const char *pdata, const int ndata
     }
     }
 
 
 
 
+}
+void iv::decition::BrainDecition::UpdateFSMSkipCmd(const char *pdata, const int ndatasize)
+{
+    if(ndatasize<1)return;
+    iv::brain::fsm_skip_cmd fsm_cmd;
+    if(false == fsm_cmd.ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<"Update FSM Skip CMD Parse fail."<<std::endl;
+        return;
+    }
+    ServiceCarStatus.mbTaskCmdType = fsm_cmd.missioncmd();
+    ServiceCarStatus.mbStartStopWork=fsm_cmd.workcmd();
+    ServiceCarStatus.mbTakeOverEn = fsm_cmd.remotectrlcmd();
+    ServiceCarStatus.mbArriveInitPos=fsm_cmd.waitingstationarrived();
+    ServiceCarStatus.mbArriveServicePos=fsm_cmd.maintainstationarrived();
 }
 }
 void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
 void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
 
 

+ 4 - 0
src/decition/decition_brain_sf_jsrunlegs/decition/brain.h

@@ -49,6 +49,8 @@
 #include "groupmsg.pb.h"
 #include "groupmsg.pb.h"
 #include "ultraarea.pb.h"
 #include "ultraarea.pb.h"
 
 
+#include "FSMSkipCMD.pb.h"
+
 #include "fanyaapi.h"
 #include "fanyaapi.h"
 
 
 namespace iv {
 namespace iv {
@@ -143,6 +145,7 @@ namespace iv {
             void * mpaObsTraceLeft;
             void * mpaObsTraceLeft;
             void * mpaObsTraceRight;
             void * mpaObsTraceRight;
             void * mpaCarstate;
             void * mpaCarstate;
+            void * mpFSMSkipCmd;
 
 
 
 
             void ShareDecition(iv::decition::Decition decition);
             void ShareDecition(iv::decition::Decition decition);
@@ -172,6 +175,7 @@ namespace iv {
             void GetFusion(const char* pdata, const int ndatasize);
             void GetFusion(const char* pdata, const int ndatasize);
             void UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
             void UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
             void UpdateUltra(const char* pdata, const int ndatasize);
             void UpdateUltra(const char* pdata, const int ndatasize);
+            void UpdateFSMSkipCmd(const char * pdata,const int ndatasize);
 
 
         private:
         private:
             std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;
             std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;

+ 37 - 5
src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.cpp

@@ -177,7 +177,7 @@ bool map_start_point = true;
 bool first_start_en=true;  //自主巡迹数据存储用
 bool first_start_en=true;  //自主巡迹数据存储用
 int  start_tracepoint;
 int  start_tracepoint;
 int  end_tracepoint;
 int  end_tracepoint;
-//
+
 
 
 std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta,速度规划用变量
 std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta,速度规划用变量
 
 
@@ -211,6 +211,7 @@ static bool final_brake_lock=false,brake_mode=false;
 bool gfinal_brake=false;
 bool gfinal_brake=false;
 double gdistance_to_end=1000;
 double gdistance_to_end=1000;
 
 
+//double lastdSpeed=0;
 //日常展示
 //日常展示
 
 
 #include <QDateTime>
 #include <QDateTime>
@@ -310,8 +311,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //    now_gps_ins.gps_y=gps.gps_y;
     //    now_gps_ins.gps_y=gps.gps_y;
 
 
     //    GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y,&now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
     //    GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y,&now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
-  static int file_num; //数据存储,20210903,cxw
 
 
+static int file_num;
     if (isFirstRun)
     if (isFirstRun)
     {
     {
         file_num=0;
         file_num=0;
@@ -363,7 +364,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         ServiceCarStatus.mbReceiveTask =true;  //JS状态机
         ServiceCarStatus.mbReceiveTask =true;  //JS状态机
         ServiceCarStatus.mnReceiveTaskCnt=0;
         ServiceCarStatus.mnReceiveTaskCnt=0;
 
 
-        ServiceCarStatus.mbFulfilTask = false;
+        ServiceCarStatus.mbFullFillTask = false;
         ServiceCarStatus.mbCloseToPath = false;
         ServiceCarStatus.mbCloseToPath = false;
         ServiceCarStatus.mbAwayFromPath = false;
         ServiceCarStatus.mbAwayFromPath = false;
         givlog->debug("decition_brain_bool","DoorTimeStart: %d,current: %d",
         givlog->debug("decition_brain_bool","DoorTimeStart: %d,current: %d",
@@ -477,6 +478,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 
 
     realspeed = now_gps_ins.speed;
     realspeed = now_gps_ins.speed;
+    //realspeed =lastdSpeed;
 
 
     secSpeed = realspeed / 3.6;
     secSpeed = realspeed / 3.6;
 
 
@@ -1122,7 +1124,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         }
                         }
                         if(final_brake==true){
                         if(final_brake==true){
                                 if((realspeed>3)&&(final_brake_lock==false)){
                                 if((realspeed>3)&&(final_brake_lock==false)){
+
                                             //minDecelerate=-0.7;
                                             //minDecelerate=-0.7;
+
                                 }else{
                                 }else{
                                            dSpeed=min(dSpeed, 3.0);
                                            dSpeed=min(dSpeed, 3.0);
                                             final_brake_lock=true;
                                             final_brake_lock=true;
@@ -1130,7 +1134,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                                             //if(distance_to_end<0.8){
                                             //if(distance_to_end<0.8){
                                              if(distance_to_end<0.4){
                                              if(distance_to_end<0.4){
                                                 minDecelerate=-0.7;
                                                 minDecelerate=-0.7;
-                                                ServiceCarStatus.mbFulfilTask = true;
+                                                ServiceCarStatus.mbFullFillTask = true;
                                             }
                                             }
 
 
                                 }
                                 }
@@ -1138,6 +1142,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         gfinal_brake=final_brake;
                         gfinal_brake=final_brake;
                         gdistance_to_end = distance_to_end;
                         gdistance_to_end = distance_to_end;
                 }
                 }
+
     }
     }
 
 
     if(!ServiceCarStatus.inRoadAvoid){
     if(!ServiceCarStatus.inRoadAvoid){
@@ -1806,7 +1811,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             int* avoidLeft_p=&avoidLeft_value;
             int* avoidLeft_p=&avoidLeft_value;
             int* avoidRight_p=&avoidRight_value;
             int* avoidRight_p=&avoidRight_value;
             computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
             computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
-            avoidXPre=computeBackDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value,avoidXNew);
+            avoidXPre=computeBackDistance
+                    (lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value,avoidXNew);
             givlog->debug("decition_brain","vehState: %d,roadOri: %d,roadSum: %d,vehWidth: %f,avoidLeft: %d,avoidRight_value: %d,avoidXNewPre: %d",
             givlog->debug("decition_brain","vehState: %d,roadOri: %d,roadSum: %d,vehWidth: %f,avoidLeft: %d,avoidRight_value: %d,avoidXNewPre: %d",
                             vehState,roadOri,roadSum,ServiceCarStatus.msysparam.vehWidth,avoidLeft_value,avoidRight_value,avoidXPre);
                             vehState,roadOri,roadSum,ServiceCarStatus.msysparam.vehWidth,avoidLeft_value,avoidRight_value,avoidXPre);
         }
         }
@@ -2435,6 +2441,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
        dSpeed=min(dSpeed, 3.0);
        dSpeed=min(dSpeed, 3.0);
     }
     }
 
 
+
     if(ServiceCarStatus.group_control){
     if(ServiceCarStatus.group_control){
         dSpeed = ServiceCarStatus.group_comm_speed;
         dSpeed = ServiceCarStatus.group_comm_speed;
     }
     }
@@ -2445,6 +2452,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             minDecelerate=min(-0.6f,minDecelerate); //1.0f    zj-0.6
             minDecelerate=min(-0.6f,minDecelerate); //1.0f    zj-0.6
     }
     }
 
 
+
     gps_decition->wheel_angle = 0.0 - controlAng;
     gps_decition->wheel_angle = 0.0 - controlAng;
     if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
     if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
     {
     {
@@ -2494,6 +2502,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
            {
            {
                dis2=2;
                dis2=2;
            }
            }
+
+           //double dis2 =  GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
            if(circleMode)//闭环地图
            if(circleMode)//闭环地图
            {
            {
                if(dis1<3&&dis2<3&&circleMode) //距离不能太小,太小了可能会错过,所以适当将距离增大一些
                if(dis1<3&&dis2<3&&circleMode) //距离不能太小,太小了可能会错过,所以适当将距离增大一些
@@ -2651,6 +2661,28 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 //                           <<"OBS_Speed"<< ","<<obsSpeed<< ","
 //                           <<"OBS_Speed"<< ","<<obsSpeed<< ","
 //                           <<"OBS_Distance"<< ","<<obsDistance<< ","
 //                           <<"OBS_Distance"<< ","<<obsDistance<< ","
 //                           <<"TTC"<< ","<<ttc<< ","
 //                           <<"TTC"<< ","<<ttc<< ","
+
+                           <<"Decide_torque"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
+                           <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
+                           <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
+                           <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+                           <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
+                           <<"Vehicle_state"<< ","<<vehState<< ","
+                           <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
+                           <<"avoidXNew"<<","<<avoidXNew<<","
+                           <<"avoidXNewPre"<<","<<avoidXNewPre<<","
+                           <<"avoidXPre"<<","<<avoidXPre<<","
+                           <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
+                           <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
+                           <<"Min_Decelation"","<<minDecelerate<< ","
+                           <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                          // <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+                          // <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+                          // <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                          // <<"Trace_Point"<< ","<<PathPoint<< ","
+                          // <<"OBS_Speed"<< ","<<obsSpeed<< ","
+                          // <<"OBS_Distance"<< ","<<obsDistance<< ","
+                          // <<"TTC"<< ","<<ttc<< ","
                            <<endl;
                            <<endl;
                    outfile.close();
                    outfile.close();
                }
                }

+ 2 - 0
src/decition/decition_brain_sf_jsrunlegs/decition_brain_sf_jsrunlegs.pro

@@ -35,6 +35,7 @@ SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/carstate.pb.cc \
     ../../include/msgtype/carstate.pb.cc \
     ../../include/msgtype/ultrasonic.pb.cc \
     ../../include/msgtype/ultrasonic.pb.cc \
     ../../include/msgtype/ultraarea.pb.cc \
     ../../include/msgtype/ultraarea.pb.cc \
+    ../../include/msgtype/FSMSkipCMD.pb.cc \
     ../../include/msgtype/groupmsg.pb.cc
     ../../include/msgtype/groupmsg.pb.cc
 
 
 
 
@@ -101,6 +102,7 @@ HEADERS += \
     ../../include/msgtype/ultrasonic.pb.h \
     ../../include/msgtype/ultrasonic.pb.h \
     ../../include/msgtype/ultraarea.pb.h \
     ../../include/msgtype/ultraarea.pb.h \
     ../../include/msgtype/carstate.pb.h \
     ../../include/msgtype/carstate.pb.h \
+    ../../include/msgtype/FSMSkipCMD.pb.h \
     ../../include/msgtype/groupmsg.pb.h
     ../../include/msgtype/groupmsg.pb.h