add brain proto

src/controller/controller_rubshCar/main.cpp

@@ -30,6 +30,7 @@ std::string gstrmemchassis;
 bool gbSendRun = true;
 
 bool gbChassisEPS = false;
+bool controlInvalid=false;
 
 iv::brain::decition gdecition_def;
 iv::brain::decition gdecition;
@@ -50,9 +51,11 @@ bool bHasBrakeCmd = false;
 bool bHasParkCmd = false;
 
 static QMutex gMutex;
+qint64 brain_update_time;
 
 void executeDecition(const iv::brain::decition decition)
 {
+    float last_torque=0;
     if(decition.has_gear())
     {
         gcontroller->control_gear_en(true);
@@ -71,8 +74,19 @@ void executeDecition(const iv::brain::decition decition)
     {
         gcontroller->control_acc_en(true);
         gcontroller->control_torque(decition.torque());
+        brain_update_time=QDateTime::currentMSecsSinceEpoch();
+        last_torque=decition.torque();
         bHasDriveCmd = true;
     }
+
+    if((QDateTime::currentMSecsSinceEpoch()-brain_update_time>1000)&&(last_torque<0.5)){
+        controlInvalid=true;
+    }else{
+        controlInvalid=false;
+    }
+
+
+
     if(decition.has_brake())
     {
         gcontroller->control_brake_en(true);
@@ -267,7 +281,7 @@ void sendthread()
             gnDecitionNum--;
         }
         executeDecition(xdecition);
-        if(gbChassisEPS == false) ExecSend();
+        if((gbChassisEPS == false)&&(controlInvalid==false)) ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
 }

src/decition/common/common/car_status.h

@@ -98,6 +98,11 @@ namespace iv {
               bool txt_log_on=false;
               bool table_look_up_on=false;
 
+              int brain_control_enable=0;
+              int rubbish_car_stop=0;
+              int rubbish_empty=0;
+              int brain_control_invalid=0;
+
 
         std::vector <iv::platform::station> car_stations;
 

src/decition/decition_brain_sf/decition/adc_adapter/sightseeing_adapter.cpp

@@ -182,6 +182,15 @@ iv::decition::Decition iv::decition::SightseeingAdapter::getAdapterDeciton(GPS_I
         controlBrake=0;
     }
 
+    if(ServiceCarStatus.rubbish_car_stop==1){
+        dSpeed=0;
+        DecideGps00::minDecelerate=-0.5;
+        if(realSpeed<0.01){
+            ServiceCarStatus.brain_control_invalid=1;
+            ServiceCarStatus.rubbish_car_stop=0;
+        }
+    }
+
     //rubbish end  2022.3.23
 
     if(DecideGps00::minDecelerate<0){

src/decition/decition_brain_sf/decition/brain.cpp

@@ -12,6 +12,7 @@
 #include <fstream>
 #include <QTime>
 
+
 #include "ivfault.h"
 extern iv::Ivfault * givfault;
 
@@ -62,6 +63,11 @@ iv::decition::BrainDecition * gbrain;
            gbrain->GetFusion(strdata,nSize);
         }
 
+        void ListenRubbishCar(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+        {
+            gbrain->UpdateRubbishCar(strdata,nSize);
+        }
+
     /*    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;
@@ -118,6 +124,8 @@ iv::decition::BrainDecition::BrainDecition()
 
     mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
     mpaCarstate= iv::modulecomm::RegisterSend(gstrmembraincarstate.data(),10000,10);
+    mpaRubbishControl= iv::modulecomm::RegisterSend("bridgeSend",10000,10);
+
 
     mpfusion = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
 
@@ -147,7 +155,7 @@ iv::decition::BrainDecition::BrainDecition()
     mpaPlanTrace = iv::modulecomm::RegisterSend("plantrace",100000,1);
     mpaObsTraceLeft = iv::modulecomm::RegisterSend("obstraceleft",100000,1);
     mpaObsTraceRight = iv::modulecomm::RegisterSend("obstraceright",100000,1);
-
+    mpaRubbishCar = iv::modulecomm::RegisterRecv("bridgeRec",iv::decition::ListenRubbishCar);
 
     mTime.start();
     mnOldTime = mTime.elapsed();
@@ -171,7 +179,9 @@ iv::decition::BrainDecition::~BrainDecition()
     iv::modulecomm::Unregister(mpaCarstate);
     iv::modulecomm::Unregister(mpaVechicleState);
     iv::modulecomm::Unregister(mpvbox);
+    iv::modulecomm::Unregister(mpaRubbishCar);
     iv::modulecomm::Unregister(mpa);
+    iv::modulecomm::Unregister(mpaRubbishControl);
 
     delete eyes;
     std::cout<<"Brain Unialize."<<std::endl;
@@ -875,19 +885,16 @@ void iv::decition::BrainDecition::run() {
                           decition_gps->accelerator,decition_gps->brake,
                           decition_gps->wheel_angle);
             givlog->debug("period id %f",decision_period);
-
-            //	ODS("\n周期时长:%f\n", start - last);
-            //	ODS("\n决策时长:%f\n", end - start);
-            //	ODS("\n接收到的实时GPS车速:%f\n", gps_data_->speed);
-            //	ODS("\n接收到的实时GPS车y轴加速:%f\n", gps_data_->accel_y);
-            //	ODS("\n接收到的实时GPS车x轴加速:%f\n", gps_data_->accel_x);
             std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策加速度:%f\n" << decition_gps->accelerator << std::endl;
-            //		ODS("\n决策刹车:%f\n", decition_gps->brake);
             std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策方向:%f\n" << decition_gps->wheel_angle << std::endl;
             std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策速度:%f\n" << decition_gps->speed << std::endl;
-//            double decition_period=GetTickCount()-start;
-//            givlog->debug("decition_brain","period: %f",
-//                            decition_period);
+
+            iv::bridge::sender brainSendRubbish;
+            if(ServiceCarStatus.brain_control_invalid==1){
+                brainSendRubbish.set_carctrl(ServiceCarStatus.brain_control_invalid);
+                ShareRubbishControl(brainSendRubbish);
+            }
+
             last = start;
             //decitionMaker->decideFromGPS(decition_gps, gps_data_, navigation_data);		//gps_data_为当前读到的gps位置信息  navigation_data为导航数据  decition_gps为根据前两者得出的决策速度与决策角度
             //            }
@@ -1620,3 +1627,42 @@ void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
     ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
     ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
 }
+
+void iv::decition::BrainDecition::UpdateRubbishCar(const char *pdata, const int ndatasize){
+
+    iv::bridge::reciver rubbish_message;
+    if(false == rubbish_message.ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<"Listen rubbish car control fail."<<std::endl;
+        return;
+    }
+
+    if(rubbish_message.has_pause()){    //装好垃圾，交回控制权
+        ServiceCarStatus.brain_control_enable=1;
+    }
+    if(rubbish_message.has_stop()){     //视觉系统发向控制系统，垃圾车需要停止
+        ServiceCarStatus.rubbish_car_stop=1;
+    }
+    if(rubbish_message.has_st_rbshbox()){  //完成垃圾倾倒动作，垃圾车回起点
+        ServiceCarStatus.rubbish_empty=1;
+    }
+//    qDebug("updateGroupInfo %ld",QDateTime::currentMSecsSinceEpoch());
+//    qDebug("serverSt: %d, speed: %f, state: %d", ServiceCarStatus.group_server_status, ServiceCarStatus.group_comm_speed, ServiceCarStatus.group_state);
+}
+
+void iv::decition::BrainDecition::ShareRubbishControl(iv::bridge::sender xbs)
+{
+    int nsize = xbs.ByteSize();
+    char * str = new char[nsize];
+    std::shared_ptr<char> pstr;
+    pstr.reset(str);
+    if(xbs.SerializeToArray(str,nsize))
+    {
+        iv::modulecomm::ModuleSendMsg(mpaRubbishControl,str,nsize);
+    }
+    else
+    {
+        std::cout<<"iv::decition::BrainDecition::ShareBrainstate serialize error."<<std::endl;
+    }
+}
+

src/decition/decition_brain_sf/decition/brain.h

@@ -48,6 +48,9 @@
 #include "carstate.pb.h"
 #include "groupmsg.pb.h"
 
+#include "canmsg.pb.h"
+#include "bridge.pb.h"
+
 #include "fanyaapi.h"
 
 namespace iv {
@@ -141,18 +144,20 @@ namespace iv {
             void * mpaObsTraceLeft;
             void * mpaObsTraceRight;
             void * mpaCarstate;
+            void * mpaRubbishControl;
 
 
             void ShareDecition(iv::decition::Decition decition);
             void ShareBrainstate(iv::brain::brainstate xbs);
             void ShareBraincarstate(iv::brain::carstate xbs);
-
+            void ShareRubbishControl(iv::bridge::sender xbs);
         private:
             void * mpaHMI;
             void * mpaPlatform;
             void *mpaGroup;
             void * mpmapChangeSig;
             void * mpaChassis;
+            void *mpaRubbishCar;
             std::string mstrmemmap_index;
 
             int mnOldTime;
@@ -169,6 +174,7 @@ namespace iv {
             void UpdateVbox(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 UpdateRubbishCar(const char * pdata,const int ndatasize);
 
         private:
             std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;

src/decition/decition_brain_sf/decition_brain_sf.pro

@@ -33,7 +33,10 @@ SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/fusionobjectarray.pb.cc \
     ../../include/msgtype/fusionobject.pb.cc \
     ../../include/msgtype/carstate.pb.cc \
-    ../../include/msgtype/groupmsg.pb.cc
+    ../../include/msgtype/groupmsg.pb.cc \
+    ../../include/msgtype/bridge.pb.cc \
+    ../../include/msgtype/canmsg.pb.cc \
+    ../../include/msgtype/canraw.pb.cc
 
 
 include($$PWD/../common/common/common.pri)
@@ -97,6 +100,9 @@ HEADERS += \
     ../common/perception_sf/sensor_radar.h \
     ../common/common/sysparam_type.h \
     ../../include/msgtype/carstate.pb.h \
-    ../../include/msgtype/groupmsg.pb.h
+    ../../include/msgtype/groupmsg.pb.h \
+    ../../include/msgtype/bridge.pb.h \
+    ../../include/msgtype/canmsg.pb.h \
+    ../../include/msgtype/canraw.pb.h