add carstate

src/decition/common/common/car_status.h

@@ -197,6 +197,11 @@ namespace iv {
         bool useLidarPerPredict = false;
         bool avoidObs = false;
         bool inRoadAvoid = false;
+
+        int mvehState=0;
+        int mavoidX=0;
+        double mObsDistance=0;
+        double mObsSpeed=0;
         //hapo station 2021/02/05
         iv::StationCmd   stationCmd;
     };

src/decition/common/main.cpp

@@ -16,6 +16,7 @@ std::string gstrmemradar;
 std::string gstrmemdecition;
 std::string gstrmembrainstate;
 std::string gstrmemchassis;
+std::string gstrmembraincarstate;
 
 iv::Ivlog * givlog;
 
@@ -46,6 +47,7 @@ int main(int argc, char *argv[])
     gstrmemdecition = xp.GetParam("dection","deciton");
     gstrmembrainstate = xp.GetParam("brainstate","brainstate");
     gstrmemchassis = xp.GetParam("chassismsgname","chassis");
+    gstrmembraincarstate = xp.GetParam("carstate","carstate");
 
     iv::decition::BrainDecition brain;
     brain.inialize();

src/decition/decition_brain_sf/decition/brain.cpp

@@ -18,6 +18,7 @@ extern std::string gstrmemdecition;
 extern std::string gstrmembrainstate;
 extern iv::Ivlog * givlog;
 extern std::string gstrmemchassis;
+extern std::string gstrmembraincarstate;
 
 #define LOG_ENABLE
 
@@ -112,6 +113,7 @@ iv::decition::BrainDecition::BrainDecition()
     mpvbox = iv::modulecomm::RegisterRecv("vbox",iv::decition::ListenVbox);
 
     mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
+    mpaCarstate= iv::modulecomm::RegisterSend(gstrmembraincarstate.data(),10000,10);
 
     mpfusion = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
 
@@ -776,8 +778,7 @@ void iv::decition::BrainDecition::run() {
             mMutexMap.lock();
             decition_gps = decitionGps00->getDecideFromGPS(*gps_data_, navigation_data, templidar,*lidar_per,trafficLight);  // my dicition=============================
 //            free(templidar);
-            mMutexMap.unlock();
-
+            mMutexMap.unlock();                  
             if(mbUseExternMPC)
             {
                 fanya::GPS_INS xgpsins;
@@ -833,6 +834,15 @@ void iv::decition::BrainDecition::run() {
             //                decitionExecuter->executeDecition(decition_gps);
             ShareDecition(decition_gps);
 
+
+
+            iv::brain::carstate car_xbs;
+            car_xbs.set_mstate(ServiceCarStatus.mvehState);
+            car_xbs.set_mavoidx(ServiceCarStatus.mavoidX);
+            car_xbs.set_mobsdistance(ServiceCarStatus.mObsDistance);
+            car_xbs.set_mobsspeed(ServiceCarStatus.mObsSpeed);
+            ShareBraincarstate(car_xbs);
+
             givlog->debug("acc is %f brake is %f wheel is %f",
                           decition_gps->accelerator,decition_gps->brake,
                           decition_gps->wheel_angle);
@@ -1281,6 +1291,24 @@ void iv::decition::BrainDecition::ShareBrainstate(iv::brain::brainstate xbs)
     }
 }
 
+void iv::decition::BrainDecition::ShareBraincarstate(iv::brain::carstate 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(mpaCarstate,str,nsize);
+    }
+    else
+    {
+        std::cout<<"iv::decition::BrainDecition::ShareBrainstate serialize error."<<std::endl;
+    }
+}
+
+
+
 void iv::decition::BrainDecition::UpdateHMI(const char *pdata, const int ndatasize)
 {
     if(ndatasize < sizeof(iv::hmi::HMIBasic))return;

src/decition/decition_brain_sf/decition/brain.h

@@ -140,10 +140,12 @@ namespace iv {
             void * mpaPlanTrace;
             void * mpaObsTraceLeft;
             void * mpaObsTraceRight;
+            void * mpaCarstate;
 
 
             void ShareDecition(iv::decition::Decition decition);
             void ShareBrainstate(iv::brain::brainstate xbs);
+            void ShareBraincarstate(iv::brain::carstate xbs);
 
         private:
             void * mpaHMI;

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -281,10 +281,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         roadNowStart=true;
         isFirstRun = false;
         obstacle_avoid_flag=0;
+        avoidXNew=0;
 
         givlog->debug("decition_brain_bool","DoorTimeStart: %d,current: %d",
                         0,0);
     }
+    ServiceCarStatus.mvehState=vehState;
+    ServiceCarStatus.mavoidX=avoidXNew;
 
 
     if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
@@ -335,6 +338,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
     GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);
 
+    GaussProjCal(front_car_gps.gps_lng,front_car_gps.gps_lat, &front_car_gps.gps_x, &front_car_gps.gps_y);
+    front_car_dis=GetDistance(now_gps_ins, front_car_gps);
 
     //xiesi
     ///kkcai, 2020-01-03
@@ -1553,6 +1558,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
         //  obsDistance=-1; //1227
     }
+    ServiceCarStatus.mObsDistance=obsDistance;
+    ServiceCarStatus.mObsSpeed=obs_speed_for_avoid;
+
     //old_bz--------------------------------------------------------------------------------------------------
 
     if (vehState == avoiding)

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -119,8 +119,9 @@ public:
     static int PathPoint;
     float lastGroupOffsetX=0.0;
 
-    GPS_INS traffic_light_gps;
+    GPS_INS traffic_light_gps,front_car_gps;
     int traffic_light_pathpoint;
+    double front_car_dis;
 
     bool changingDangWei=false;