modify avoid road

src/decition/decition_brain_sf/decition/adc_adapter/hapo_adapter.cpp

@@ -45,7 +45,10 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
         if(obsDistance>0 && obsDistance<10){
             controlBrake= u*1.0;     //1.5    zj-1.2
         }
-         if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
+        if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around){
+            controlBrake=0;
+            controlSpeed=max(0.0,ServiceCarStatus.torque_st-10.0);
+        }else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
                  && dSpeed>0 && lastBrake==0){
             controlBrake=0;
             controlSpeed=0;
@@ -192,8 +195,8 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
 //    if((*decition)->brake>0)
 //    {
 
-//    givlog->debug("brain_decition","brake: %f,obsSpeed: %f,reverse_speed: %f",
-//                            (*decition)->brake,obsSpeed,reverse_speed);
+    givlog->debug("brain_decition","brake: %f,obsDistance: %f,obsSpeed: %f,reverse_speed: %f",
+                            (*decition)->brake,obsDistance,obsSpeed,reverse_speed);
 
 //    }
 

src/decition/decition_brain_sf/decition/adc_tools/compute_00.cpp

@@ -256,11 +256,11 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                 {
                     if((doubledata[i][3]>-0.2)&&(doubledata[i][3]<0.2)){
                         MapTrace[i]->roadMode=0;
-                    }else if(((doubledata[i][3]>0.2)&&(doubledata[i][3]<1.0))||((doubledata[i][3]>-1.0)&&(doubledata[i][3]<-0.2))){
+                    }else if(((doubledata[i][3]>0.2)&&(doubledata[i][3]<0.4))||((doubledata[i][3]>-0.4)&&(doubledata[i][3]<-0.2))){
                         MapTrace[i]->roadMode=5;
-                    }else if(((doubledata[i][3]>1.0)&&(doubledata[i][3]<2.0))||((doubledata[i][3]>-2.0)&&(doubledata[i][3]<-1.0))){
+                    }else if(((doubledata[i][3]>0.4)&&(doubledata[i][3]<1.0))||((doubledata[i][3]>-1.0)&&(doubledata[i][3]<-0.4))){
                         MapTrace[i]->roadMode=18;
-                    }else if(((doubledata[i][3]>2.0))||((doubledata[i][3]<-2.0))){
+                    }else if(((doubledata[i][3]>1.0))||((doubledata[i][3]<-1.0))){
                         MapTrace[i]->roadMode=14;
                     }
                 }
@@ -683,8 +683,9 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
                         obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
 
                         isRemove = true;
-                        givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
-                                      obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
+//                        givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
+//                                      obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
+
                         //		DecideGps00().lidarDistance = obsPoint.y;
                         return obsPoint;
                     }
@@ -724,8 +725,9 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
                         obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
 
                         isRemove = true;
-                        givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
-                                      obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
+//                        givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
+//                                      obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
+
                         //		DecideGps00().lidarDistance = obsPoint.y;
                         return obsPoint;
                     }

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -987,6 +987,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         roadSum = gpsMapLine[PathPoint]->roadSum*3;
     }
 
+    roadOri =0;
+    roadSum =2;
 
     if(roadNowStart){
         roadNow=roadOri;
@@ -1130,29 +1132,20 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         if(vehState==avoiding){
             ServiceCarStatus.msysparam.vehWidth=2.1;
-            controlAng=max(-150.0,controlAng);
-            controlAng=min(150.0,controlAng);
+            controlAng=max(-35.0,controlAng);//150  zj-80
+            controlAng=min(35.0,controlAng);//150   zj-80
         }
         if(vehState==backOri){
-            controlAng=max(-20.0,controlAng);//120   zj-80
-            controlAng=min(20.0,controlAng);//120     zj-80
+            controlAng=max(-35.0,controlAng);//120   zj-80
+            controlAng=min(35.0,controlAng);//120     zj-80
         }
     }
 
-    givlog->debug("brain_decition","vehState: %d",
-                  vehState);
+    givlog->debug("brain_decition","vehState: %d,controlAng: %f",
+            vehState,controlAng);
 
-    if(vehState==backOri){
-        controlAng=max(-20.0,controlAng);//120   zj-80
-        controlAng=min(20.0,controlAng);//120     zj-80
-
-        givlog->debug("brain_decition","controlAng: %f",
-                      controlAng);
-
-    }
 
     //准备驻车
-
     if (readyZhucheMode)
     {
 
@@ -1355,16 +1348,16 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     dSpeed = min(doubledata[PathPoint][4],dSpeed);
 
-    givlog->debug("brain_decition_speed","dspeed: %f",
-                  dSpeed);
-
     if (gpsMapLine[PathPoint]->speed_mode == 2)
     {
         dSpeed = min(25.0,dSpeed);
 
     }
 
-
+    if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri))
+    {
+        dSpeed = min(8.0,dSpeed);
+    }
 
     if((gpsMapLine[PathPoint]->speed)>0.001)
     {
@@ -1378,21 +1371,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     }
 
-
-
     dSecSpeed = dSpeed / 3.6;
 
-
-
-
-
-
-    std::cout<<"juecesudu2="<<dSpeed<<std::endl;
-
-
-
-
-
+    givlog->debug("brain_decition_speed","dspeed: %f",
+                  dSpeed);
 
 
     if(vehState==changingRoad){
@@ -1416,8 +1398,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if(!ServiceCarStatus.daocheMode){
         computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
 
-        //givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
-                      //obsDistance,obsSpeed);
+
         if(obs_filter_flag==0)
         {
             if(obsDistance>0&&obsDistance<60)
@@ -1462,8 +1443,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
 
 
-
-
         obs_speed_for_avoid=secSpeed+obsSpeed;
 
 
@@ -1482,11 +1461,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if (vehState == avoiding)
     {
-        cout<<"\n==================avoiding=================\n"<<endl;
-        //  vehState=normalRun; //1025
-        if (dSpeed > 10) {
-            dSpeed = 10;
-        }
         //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
         if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
             vehState = normalRun;
@@ -1518,12 +1492,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if (vehState==backOri)
     {
-        cout<<"\n================backOri===========\n"<<endl;
-        //  vehState=normalRun; //1025
-        if (dSpeed > 10) {
-            dSpeed = 10;
-        }
-
         //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
         if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
             vehState = normalRun;
@@ -1551,9 +1519,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if (roadNow!=roadOri)
     {
-        //        useFrenet = true;
-        //        useOldAvoid = false;
-
         if(useFrenet){
             if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
             {
@@ -1591,17 +1556,17 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     static bool avoid_speed_flag=false;
-    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis+10)&&(abs(realspeed)>0.5)&&
+    /*if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis+10)&&(abs(realspeed)>0.5)&&
             (vehState==normalRun) &&(ObsTimeStart==-1)//&&(obs_speed_for_avoid<0.6)
             && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000))
     {
         minDecelerate=-0.4;
         avoid_speed_flag=true;
-    }
+    }*/
 
 
     //shiyanbizhang 0929
-    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&& (avoid_speed_flag==true)        //&&(obs_speed_for_avoid<0.6)
+    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)//&& (avoid_speed_flag==true)        //&&(obs_speed_for_avoid<0.6)
             &&(vehState==normalRun||vehState==backOri || vehState==avoiding) &&(ObsTimeStart==-1)
             && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000)){
         ObsTimeStart=GetTickCount();
@@ -1734,24 +1699,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //--------------------------------------------------------------------------------old_bz_end
 
 
-
-
-
-    //TOUCHEPINGBI
-
-
-
-
-
-
-
-
-
-
-
-
-
-
     if (parkMode)
     {
         minDecelerate=-1.0;

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -191,61 +191,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
                     nomal_centroid_->CopyFrom(nomal_centroid);
                 }
             }
-        }
-        for(int k = 0; k<10; k++)
-        {
-            //            std::cout<<"fusion    begin"<<std::endl;
-            iv::fusion::PointXYZ point_forcaste;
-            iv::fusion::PointXYZ *point_forcaste_;
-            float forcast_x = lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() +
-                    lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
-            float forcast_y = lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() +
-                    lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
-            point_forcaste.set_x(forcast_x);
-            point_forcaste.set_y(forcast_y);
-            point_forcaste.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
-            point_forcaste_ = fusion_object.add_point_forecast();
-            point_forcaste_->CopyFrom(point_forcaste);
-
-            iv::fusion::NomalForecast forcast_normal;
-            iv::fusion::NomalForecast *forcast_normal_;
-            forcast_normal.set_forecast_index(i);
-            //            std::cout<<"fusion     end"<<std::endl;
-            if((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()>0)&&
-                    (lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()>0))
-            {
-                int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
-                if(xp == 0)xp=1;
-                int yp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()/0.2)/2.0);
-                if(yp == 0)yp=1;
-                int ix,iy;
-                for(ix = 0; ix<(xp*2); ix++)
-                {
-                    for(iy = 0; iy<(yp*2); iy++)
-                    {
-                        iv::fusion::NomalXYZ nomal_forcast;
-                        iv::fusion::NomalXYZ *nomal_forcast_;
-                        float nomal_x = ix*0.2 - xp*0.2;
-                        float nomal_y = iy*0.2 - yp*0.2;
-                        float nomal_z = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
-                        float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) -
-                                nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
-                        float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) +
-                                nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
-                        nomal_forcast.set_nomal_x(forcast_x + s);
-                        nomal_forcast.set_nomal_y(forcast_y + t);
-                        nomal_forcast_ = forcast_normal.add_forecast_nomal();
-                        nomal_forcast_->CopyFrom(nomal_forcast);
-                    }
-                }
-            }
-            forcast_normal_=fusion_object.add_forecast_nomals();
-            forcast_normal_->CopyFrom(forcast_normal);
-        }
 
-        iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
-        pobj->CopyFrom(fusion_object);
-    }
     for(int j = 0; j< radar_idx.size() ; j++){
         iv::fusion::fusionobject fusion_obj;
         fusion_obj.set_yaw(radar_object_array.obj(radar_idx[j]).angle());
@@ -297,56 +243,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             }
         }
 
-        for(int k = 0; k<10; k++)
-        {
-            //                std::cout<<"fusion    begin"<<std::endl;
-            iv::fusion::PointXYZ point_forcaste;
-            iv::fusion::PointXYZ *point_forcaste_;
-            float forcast_x = radar_object_array.obj(radar_idx[j]).x()
-                    + radar_object_array.obj(radar_idx[j]).vx()*time_step*(k+1);
-            float forcast_y = radar_object_array.obj(radar_idx[j]).y()
-                    + radar_object_array.obj(radar_idx[j]).vy()*time_step*(k+1);
-            point_forcaste.set_x(forcast_x);
-            point_forcaste.set_y(forcast_y);
-            point_forcaste.set_z(1.0);
-            point_forcaste_ = fusion_obj.add_point_forecast();
-            point_forcaste_->CopyFrom(point_forcaste);
-
-            iv::fusion::NomalForecast forcast_normal;
-            iv::fusion::NomalForecast *forcast_normal_;
-            forcast_normal.set_forecast_index(k);
 
-            int xp = (int)((0.5/0.2)/2.0);
-            if(xp == 0)xp=1;
-            int yp = (int)((0.5/0.2)/2.0);
-            if(yp == 0)yp=1;
-            int ix,iy;
-            for(ix = 0; ix<(xp*2); ix++)
-            {
-                for(iy = 0; iy<(yp*2); iy++)
-                {
-                    iv::fusion::NomalXYZ nomal_forcast;
-                    iv::fusion::NomalXYZ *nomal_forcast_;
-                    float nomal_x = ix*0.2 - xp*0.2;
-                    float nomal_y = iy*0.2 - yp*0.2;
-                    float nomal_z = 1.0;
-                    float s = nomal_x*cos(radar_object_array.obj(radar_idx[j]).angle())
-                            - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
-                    float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
-                            + nomal_y*cos(radar_object_array.obj(radar_idx[j]).angle());
-                    nomal_forcast.set_nomal_x(forcast_x + s);
-                    nomal_forcast.set_nomal_y(forcast_y + t);
-                    nomal_forcast_ = forcast_normal.add_forecast_nomal();
-                    nomal_forcast_->CopyFrom(nomal_forcast);
-                }
-            }
-            forcast_normal_=fusion_obj.add_forecast_nomals();
-            forcast_normal_->CopyFrom(forcast_normal);
-        }
-
-        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
-        obj->CopyFrom(fusion_obj);
-    }
 //    std::cout<<"   fusion   end    "<<std::endl;
 }
 

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -73,6 +73,7 @@ void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const un
     }
 //    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
     gMutex.lock();
+//    lidar_obj.CopyFrom(lidarobj);
     datafusion(lidarobj,radarobjvec,li_ra_fusion);
     gMutex.unlock();
 }
@@ -98,6 +99,7 @@ void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned
 
     gMutex.lock();
     mobileye_info.CopyFrom(mobileye);
+//    datafusion(lidar_obj,radarobjvec,li_ra_fusion);
     gMutex.unlock();
 }
 
@@ -106,17 +108,17 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 {
 //    gMutex.lock();
 //    Transformation::RadarPross(radarobjvec);
-//    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
+    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
 
     AddMobileye(li_ra_fusion,mobileye_info);
-    for(int i=0;i<li_ra_fusion.obj_size();i++)
-    {
-        std::cout<<"x    y      vx    vy   w "<<li_ra_fusion.obj(i).centroid().x()<<"          "
-                <<li_ra_fusion.obj(i).centroid().y()<<"         "
-                <<li_ra_fusion.obj(i).vel_relative().x()<<"     "
-                <<li_ra_fusion.obj(i).vel_relative().y()<<"     "
-                <<li_ra_fusion.obj(i).dimensions().x()<<std::endl;
-    }
+//    for(int i=0;i<li_ra_fusion.obj_size();i++)
+//    {
+//        std::cout<<"x    y      vx    vy   w "<<li_ra_fusion.obj(i).centroid().x()<<"          "
+//                <<li_ra_fusion.obj(i).centroid().y()<<"         "
+//                <<li_ra_fusion.obj(i).vel_relative().x()<<"     "
+//                <<li_ra_fusion.obj(i).vel_relative().y()<<"     "
+//                <<li_ra_fusion.obj(i).dimensions().x()<<std::endl;
+//    }
 
 
 //    int nsize =0;