add per obs predictfunciton

src/decition/common/common/car_status.h

@@ -185,6 +185,7 @@ namespace iv {
 
         uint32_t ultraDistance[13];
         bool useObsPredict = false;
+        bool useLidarPerPredict = false;
         //hapo station 2021/02/05
         iv::StationCmd   stationCmd;
     };

src/decition/decition_brain_sf/decition/brain.cpp

@@ -334,6 +334,7 @@ void iv::decition::BrainDecition::run() {
     std::string gpsOffset_x = xp.GetParam("gpsOffset_X","0");
     std::string gpsOffset_y = xp.GetParam("gpsOffset_Y","0");
     std::string strexternmpc = xp.GetParam("ExternMPC","false");  //If Use MPC set true
+    std::string lidar_per_predict = xp.GetParam("lidarPP","false");  //If Use MPC set true
 
 
      adjuseGpsLidarPosition();
@@ -343,6 +344,10 @@ void iv::decition::BrainDecition::run() {
         mbUseExternMPC = true;
     }
 
+    if(lidar_per_predict == "true"){
+        ServiceCarStatus.useLidarPerPredict = true;
+    }
+
     ServiceCarStatus.msysparam.lidarGpsXiuzheng = atof(lidarGpsXiuzheng.data());
     ServiceCarStatus.msysparam.radarGpsXiuzheng = atof(radarGpsXiuzheng.data());
     ServiceCarStatus.msysparam.rearRadarGpsXiuzheng = atof(rearRadarGpsXiuzheng.data());
@@ -466,34 +471,59 @@ void iv::decition::BrainDecition::run() {
 
         eyes->getSensorObstacle(obs_radar_, obs_camera_,gps_data_, obs_lidar_grid_);	//从传感器线程临界区交换数据
 
-/*
-        if(obs_lidar_grid_!= NULL)
-        {
-            std::cout<<"receive fusion date"<<std::endl;
-            int i,j;
-            for(i=0;i<iv::grx;i++)
-            {
-                for(j=0;j<iv::gry;j++)
-                {
-                 if(obs_lidar_grid_[i*iv::gry+j].ob == 2){
-                     std::cout<<"ok"<<std::endl;
-                     if(obs_lidar_grid_[i*iv::gry+j].id > 10)
-                     {
-                         int xx = obs_lidar_grid_[i*iv::gry+j].id;
-                         xx++;
-                     }
-                      std::cout<<obs_lidar_grid_[i*iv::gry+j].id<<std::endl;
-                 }
+
+//        if(obs_lidar_grid_!= NULL)
+//        {
+//            std::cout<<"receive fusion date"<<std::endl;
+//            int i,j;
+//            for(i=0;i<iv::grx;i++)
+//            {
+//                for(j=0;j<iv::gry;j++)
+//                {
+//                 if(obs_lidar_grid_[i*iv::gry+j].ob == 2){
+//                     std::cout<<"ok"<<std::endl;
+//                     if(obs_lidar_grid_[i*iv::gry+j].id > 10)
+//                     {
+//                         int xx = obs_lidar_grid_[i*iv::gry+j].id;
+//                         xx++;
+//                     }
+//                      std::cout<<obs_lidar_grid_[i*iv::gry+j].id<<std::endl;
+//                 }
 
 
-                }
-            }
-              std::cout<<"print fusion date end"<<std::endl;
-        }
-*/
+//                }
+//            }
+//              std::cout<<"print fusion date end"<<std::endl;
+//        }
+
 
         ServiceLidar.copylidarperto(lidar_per);
 
+//                if(lidar_per->size() >0)
+//                {
+//                    int i;
+//                    for(i=0;i<lidar_per->size();i++)
+//                    {
+//                        if(lidar_per->at(i).label>0)
+//                        {
+//                            std::cout<<"label is"<<lidar_per->at(i).label<<" vy = "<<lidar_per->at(i).velocity.y<<std::endl;
+//                        }
+//                    }
+//                    //if size > 0, have perception result;
+//                }
+
+
+                if(lidar_per->empty()){
+                   miss_lidar_per_count++;
+                   miss_lidar_per_count=min(miss_lidar_per_limit,miss_lidar_per_count);
+                   if(miss_lidar_per_count<miss_lidar_per_limit && old_lidar_per!=NULL){
+                    lidar_per=old_lidar_per;
+                   }
+                }else{
+                    old_lidar_per=lidar_per;
+                    miss_lidar_per_count=0;
+                }
+
 //                if(lidar_per->size() >0)
 //                {
 //                    int i;
@@ -566,6 +596,12 @@ void iv::decition::BrainDecition::run() {
         }
         //useMobileEye chuku end;
 
+
+
+
+
+
+
         if (gps_data_  &&!handPark && !ServiceCarStatus.m_bOutGarage){// && obs_lidar_grid_) {	//处理GPS数据
             //todo gps_data_为当前读到的gps位置信息
             //decition_gps = iv::decition::Decition(new iv::decition::DecitionBasic);
@@ -619,6 +655,36 @@ void iv::decition::BrainDecition::run() {
             if(templidar == NULL)templidar = old_obs_lidar_grid_;
         //    GaussProjCal(gps_data_->gps_lng,gps_data_->gps_lat,&gps_data_->gps_x,&gps_data_->gps_y);
 
+
+
+//            if(templidar!= NULL)
+//            {
+//                std::cout<<"receive fusion date"<<std::endl;
+//                int i,j;
+//                for(i=0;i<iv::grx;i++)
+//                {
+//                    for(j=0;j<iv::gry;j++)
+//                    {
+//                     if(templidar[i*iv::gry+j].ob == 2){
+//                         std::cout<<"ok"<<std::endl;
+//                         if(templidar[i*iv::gry+j].id > 10)
+//                         {
+//                             int xx = templidar[i*iv::gry+j].id;
+//                             xx++;
+//                         }
+//                          std::cout<<templidar[i*iv::gry+j].id<<std::endl;
+//                     }
+
+
+//                    }
+//                }
+//                  std::cout<<"print fusion date end"<<std::endl;
+//            }
+
+
+
+
+
             mMutexMap.lock();
             decition_gps = decitionGps00->getDecideFromGPS(*gps_data_, navigation_data, templidar,*lidar_per,trafficLight);  // my dicition=============================
             mMutexMap.unlock();

src/decition/decition_brain_sf/decition/brain.h

@@ -112,6 +112,11 @@ namespace iv {
             iv::TrafficLight trafficLight;
             iv::Obs_grid* obs_fusion_grid_;     //fusion网格化
 
+            std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>>  old_lidar_per;
+            std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>>  tmp_lidar_per;
+            int miss_lidar_per_count=0;
+            int miss_lidar_per_limit=20;
+
 
 
             int lastMode;

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -2514,6 +2514,16 @@ void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
 
     obsSd= obsPoint.obs_speed_y;
 
+    if(ServiceCarStatus.useLidarPerPredict){
+       double preDis= predictObsOnRoad(lidar_per, gpsTrace, realspeed);
+       if (preDis<obs){
+           obs = preDis;
+           if(abs(obs-preDis>0.5)){
+            obsSd = 0-realspeed;
+           }
+       }
+    }
+
 
     if(roadNum==roadNow){
         obsDistance=obs;
@@ -2644,8 +2654,8 @@ void iv::decition::DecideGps00::computeRearObsOnRoad(iv::LidarGridPtr lidarGridP
 
 
 
-void iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
-    double preObsDis;
+double iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
+    double preObsDis=500;
 
     if(!lidar_per.empty()){
         preObsDis=PredictObsDistance(  realSpeed,  gpsTrace, lidar_per);
@@ -2654,10 +2664,13 @@ void iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::Per
     }else{
         preObsDis=lastPreObsDistance;
     }
-    if(preObsDis<obsDistance){
-        obsDistance=preObsDis;
-        lastDistance=obsDistance;
-    }
+
+  //  ServiceCarStatus.mfttc = preObsDis;
+    return preObsDis;
+//    if(preObsDis<obsDistance){
+//        obsDistance=preObsDis;
+//        lastDistance=obsDistance;
+//    }
 }
 
 int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -171,7 +171,7 @@ public:
     void computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine);
     void computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr,const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
 
-    void predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed);
+    double predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed);
 
     int chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins,std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);
     int chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);

src/decition/decition_brain_sf/decition/obs_predict.cpp

@@ -16,17 +16,17 @@ double iv::decition::PredictObsDistance( double realSpeed,std::vector<Point2D>
 
     double crashDistance=200;
     for(int i=0;i<lidar_per.size();i++){
-        if(lidar_per[i].life>300 && (lidar_per[i].velocity.x!=0 || lidar_per[i].velocity.y!=0)){
+        if(lidar_per[i].life>0.3 && (lidar_per[i].velocity.x!=0 || lidar_per[i].velocity.y!=0)){
             if(lidar_per[i].location.y>20 && lidar_per[i].velocity.y>0){
-
+                continue;
             }
             else if(lidar_per[i].location.y<0 && lidar_per[i].velocity.y<0){
-
+                 continue;
             }
             else if(lidar_per[i].location.x>20 && lidar_per[i].velocity.x>0){
-
+                 continue;
             }else if(lidar_per[i].location.x<-20 && lidar_per[i].velocity.x<0){
-
+                 continue;
             }else{
 
                 double    crashDis = getCrashDis(realSpeed,gpsTrace,lidar_per[i]);