add ultra logic

src/decition/common/common/car_status.h

@@ -27,6 +27,7 @@
 #include "common/tracepointstation.h"
 
 #include "groupmsg.pb.h"
+#include "ultraarea.pb.h"
 #include <QMutex>
 
 #define RADAR_OBJ_NUM 64
@@ -237,6 +238,10 @@ namespace iv {
        int  mnReceiveTaskCnt=0;
        bool mbNoPath=false;
        int mbNoPathCnt=0;
+
+       //超声波信息
+       //std::vector<iv::ultrasonic::Area> mbUltraArea;
+       float mbUltraDis[4]={200,200,200,200};
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_jsrunlegs/decition/brain.cpp

@@ -62,6 +62,11 @@ iv::decition::BrainDecition * gbrain;
            gbrain->GetFusion(strdata,nSize);
         }
 
+        void ListenUltraArea(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+        {
+            gbrain->UpdateUltra(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;
@@ -148,6 +153,8 @@ iv::decition::BrainDecition::BrainDecition()
     mpaObsTraceLeft = iv::modulecomm::RegisterSend("obstraceleft",100000,1);
     mpaObsTraceRight = iv::modulecomm::RegisterSend("obstraceright",100000,1);
 
+    mpultraArea=iv::modulecomm::RegisterRecv("ultra-area",iv::decition::ListenUltraArea);//监听超声波数据
+
 
     mTime.start();
     mnOldTime = mTime.elapsed();
@@ -1643,6 +1650,28 @@ void iv::decition::BrainDecition::UpdateSate(){
      decitionGps00->isFirstRun=true;
 }
 
+void iv::decition::BrainDecition::UpdateUltra(const char *pdata, const int ndatasize)
+{
+    if(ndatasize<1)return;
+    iv::ultrasonic::ultraarea ultra_msg;
+    if(false == ultra_msg.ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<"Update UltraMsg Parse fail."<<std::endl;
+        return;
+    }
+
+    iv::ultrasonic::Area ultra_area;
+    for(int i=0;i<ultra_msg.area_size();i++)
+    {
+        ultra_area =ultra_msg.area(i);
+        if(ultra_area.valid()==true&&(ultra_area.id()>=1))
+        {
+            ServiceCarStatus.mbUltraDis[ultra_area.id()-1] = ultra_area.dist();
+        }
+    }
+
+
+}
 void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
 
     ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;

src/decition/decition_brain_sf_jsrunlegs/decition/brain.h

@@ -47,6 +47,7 @@
 #include "vbox.pb.h"
 #include "carstate.pb.h"
 #include "groupmsg.pb.h"
+#include "ultraarea.pb.h"
 
 #include "fanyaapi.h"
 
@@ -133,6 +134,7 @@ namespace iv {
             void * mpvbox;
             void * mpfusion;
             QMutex mMutexMap;
+            void * mpultraArea;//超声波数据
 
             void * mpaDecition;
             void * mpaVechicleState;
@@ -169,6 +171,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 UpdateUltra(const char* pdata, const int ndatasize);
 
         private:
             std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;

src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.cpp

@@ -2417,6 +2417,41 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
+    //超声波逻辑,begin
+   if(ServiceCarStatus.mbUltraDis[0]>=3.0&&ServiceCarStatus.mbUltraDis[0]<=5.0)
+   {
+      dSpeed=min(dSpeed,3.0);
+   }
+   else if(ServiceCarStatus.mbUltraDis[0]>=0.0&&ServiceCarStatus.mbUltraDis[0]<3.0)
+   {
+       minDecelerate=min(-1.0f,minDecelerate);
+   }
+
+   if((ServiceCarStatus.mbUltraDis[1]>=1.0&&ServiceCarStatus.mbUltraDis[1]<5.0)||
+      (ServiceCarStatus.mbUltraDis[3]>=1.0&&ServiceCarStatus.mbUltraDis[3]<5.0))
+   {
+      dSpeed=min(dSpeed,3.0);
+   }
+   else if((ServiceCarStatus.mbUltraDis[1]>=0.0&&ServiceCarStatus.mbUltraDis[1]<1.0)||
+           (ServiceCarStatus.mbUltraDis[3]>=0.0&&ServiceCarStatus.mbUltraDis[3]<1.0))
+   {
+       minDecelerate=min(-0.5f,minDecelerate);
+   }
+
+   if(ServiceCarStatus.daocheMode)
+   {
+       if(ServiceCarStatus.mbUltraDis[2]>=3.0&&ServiceCarStatus.mbUltraDis[2]<=5.0)
+       {
+          dSpeed=min(dSpeed,3.0);
+       }
+       else if(ServiceCarStatus.mbUltraDis[2]>=0.0&&ServiceCarStatus.mbUltraDis[2]<3.0)
+       {
+           minDecelerate=min(-1.0f,minDecelerate);
+       }
+   }
+//超声波逻辑,end
+
+
     std::cout<<"final desire speed="<<dSpeed<<std::endl;
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 

src/decition/decition_brain_sf_jsrunlegs/decition_brain_sf_jsrunlegs.pro

@@ -33,6 +33,8 @@ SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/fusionobjectarray.pb.cc \
     ../../include/msgtype/fusionobject.pb.cc \
     ../../include/msgtype/carstate.pb.cc \
+    ../../include/msgtype/ultrasonic.pb.cc \
+    ../../include/msgtype/ultraarea.pb.cc \
     ../../include/msgtype/groupmsg.pb.cc
 
 
@@ -96,6 +98,8 @@ HEADERS += \
     ../common/perception_sf/sensor_lidar.h \
     ../common/perception_sf/sensor_radar.h \
     ../common/common/sysparam_type.h \
+    ../../include/msgtype/ultrasonic.pb.h \
+    ../../include/msgtype/ultraarea.pb.h \
     ../../include/msgtype/carstate.pb.h \
     ../../include/msgtype/groupmsg.pb.h