xiansu he lidar

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -1277,17 +1277,17 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
                 getGpsTraceNowLeftRight(gpsTraceNow);
             }
-            else //if((vehState==preAvoid)||(vehState==normalRun)||(vehState))
-            {
-                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
-                gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
-                gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
-            }
-//            else
+//            else //if((vehState==preAvoid)||(vehState==normalRun)||(vehState))
 //            {
-//                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidXNew);
-//                getGpsTraceNowLeftRight(gpsTraceNow);
+//                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+//                gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+//                gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
 //            }
+            else
+            {
+                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidXNew);
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
         }
     }
 #else
@@ -1820,7 +1820,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         {
             vehState=normalRun;
             obstacle_avoid_flag=0;
-            avoidXNewPreVector.clear();
+           // avoidXNewPreVector.clear();//20230526,huifu
         }
     }
 
@@ -1902,13 +1902,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
     else
     {
-        if(PathPoint+400<gpsMapLine.size()){
+        if(PathPoint+200<gpsMapLine.size()){   //400gaiwei 200
             int road_permit_sum=0;
-            for(int k=PathPoint;k<PathPoint+400;k++){
+            for(int k=PathPoint;k<PathPoint+200;k++){
                 if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
                     road_permit_sum++;
             }
-            if(road_permit_sum>=400)
+            if(road_permit_sum>=200)
                 road_permit_avoid=true;
         }
     }
@@ -2015,118 +2015,156 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         givlog->debug("decition_brain","======avoidXNewPre==========:%f",avoidXNewPre);
 
-        const int nAvoidPreCount = 100;
+        //const int nAvoidPreCount = 100;
+        if(avoidXNewPreVector.size()<5){
+                   avoidXNewPreVector.push_back(avoidXNewPre);
+               }else{
+                   if(avoidXNewPreVector[0]!=avoidXNewLast){
+                       for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+                           if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+                               avoidXNewPreFilter=avoidXNewLast;
+                               break;
+                           }
+                           if(i==avoidXNewPreVector.size()-1)
+                               avoidXNewPreFilter=avoidXNewPreVector[0];
+                       }
+                   }
+                   avoidXNewPreVector.clear();
+               }
+               if(avoidXNewPreFilter!=avoidXNewLast){
+                   avoidXNew=avoidXNewPre;
+                   if(avoidXNew<0)
+                       turnLampFlag=-1;
+                   else if(avoidXNew>0)
+                       turnLampFlag=1;
+                   else
+                       turnLampFlag=0;
+
+                   gpsTraceNow.clear();
+                   gpsTraceAvoidXY.clear();
+                   givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+                                 avoidXNew,avoidXNewLast);
+                   //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+                   gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+                   vehState = avoiding;
+                   startAvoidGpsIns = now_gps_ins;
+                   obstacle_avoid_flag=1;
+                   hasCheckedAvoidLidar = false;
+                   avoidXNewLast=avoidXNew;
+                   avoidXNewPreFilter=avoidXNew;
+               }
+           }
 
-        if(avoidXNewPreVector.size()<nAvoidPreCount){
-            avoidXNewPreVector.push_back(avoidXNewPre);
-        }
-        else
-        {
-            avoidXNewPreVector.erase(avoidXNewPreVector.begin());
-            avoidXNewPreVector.push_back(avoidXNewPre);
-        }
+//        if(avoidXNewPreVector.size()<nAvoidPreCount){
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }
+//        else
+//        {
+//            avoidXNewPreVector.erase(avoidXNewPreVector.begin());
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }
 
-        if(vehState == preAvoid)
-        {
-            avoidXNew = avoidXNewPre;
-        }
-        else
-        {
-            if(fabs(avoidXNewLast)>0.1)
-            {
+//        if(vehState == preAvoid)
+//        {
+//            avoidXNew = avoidXNewPre;
+//        }
+//        else
+//        {
+//            if(fabs(avoidXNewLast)>0.1)
+//            {
 
-                bool bOriginSafe = true;
+//                bool bOriginSafe = true;
 
-                if(avoidXNewPreVector.size()<nAvoidPreCount)
-                {
-                    bOriginSafe = false;
-                }
-                else
-                {
-                    unsigned int j;
-                    unsigned int nSize = avoidXNewPreVector.size();
-                    for(j=0;j<nSize;j++)
-                    {
-                        if(fabs(avoidXNewPreVector[j])>0.1)
-                        {
-                            bOriginSafe = false;
-                            break;
-                        }
-                    }
-                }
-                if(bOriginSafe)
-                {
-                    avoidXNew = 0;
- //                   avoidXNewPreVector.clear();
-                }
-                else
-                {
-                    unsigned int j;
-                    unsigned int nSize = avoidXNewPreVector.size();
-                    if(avoidXNewPreVector.size()==nAvoidPreCount)
-                    {
-                        double filter = 0;
-                        for(j=0;j<nSize;j++)
-                        {
-                            if(fabs(avoidXNewPreVector[j])>0.1)
-                            {
-                                if(fabs(filter)<0.1)filter = avoidXNewPreVector[j];
-                                else
-                                {
-                                    if(fabs(filter - avoidXNewPreVector[j])>0.1)
-                                    {
-                                        filter = 0;
-                                        break;
-                                    }
-                                }
-                            }
-                        }
-                        if(fabs(filter)<0.1)
-                        {
-                            avoidXNew = avoidXNewLast;
-                        }
-                        else
-                        {
-                            if(fabs(filter - avoidXNewLast)>=2.0)
-                            {
-                                avoidXNew = filter;
-                            }
-                            else
-                            {
-                                avoidXNew = avoidXNewLast;
-                            }
-                        }
-                    }
-                    else
-                    {
-                        avoidXNew = avoidXNewLast;
-                    }
-                }
-            }
-        }
+//                if(avoidXNewPreVector.size()<nAvoidPreCount)
+//                {
+//                    bOriginSafe = false;
+//                }
+//                else
+//                {
+//                    unsigned int j;
+//                    unsigned int nSize = avoidXNewPreVector.size();
+//                    for(j=0;j<nSize;j++)
+//                    {
+//                        if(fabs(avoidXNewPreVector[j])>0.1)
+//                        {
+//                            bOriginSafe = false;
+//                            break;
+//                        }
+//                    }
+//                }
+//                if(bOriginSafe)
+//                {
+//                    avoidXNew = 0;
+// //                   avoidXNewPreVector.clear();
+//                }
+//                else
+//                {
+//                    unsigned int j;
+//                    unsigned int nSize = avoidXNewPreVector.size();
+//                    if(avoidXNewPreVector.size()==nAvoidPreCount)
+//                    {
+//                        double filter = 0;
+//                        for(j=0;j<nSize;j++)
+//                        {
+//                            if(fabs(avoidXNewPreVector[j])>0.1)
+//                            {
+//                                if(fabs(filter)<0.1)filter = avoidXNewPreVector[j];
+//                                else
+//                                {
+//                                    if(fabs(filter - avoidXNewPreVector[j])>0.1)
+//                                    {
+//                                        filter = 0;
+//                                        break;
+//                                    }
+//                                }
+//                            }
+//                        }
+//                        if(fabs(filter)<0.1)
+//                        {
+//                            avoidXNew = avoidXNewLast;
+//                        }
+//                        else
+//                        {
+//                            if(fabs(filter - avoidXNewLast)>=2.0)
+//                            {
+//                                avoidXNew = filter;
+//                            }
+//                            else
+//                            {
+//                                avoidXNew = avoidXNewLast;
+//                            }
+//                        }
+//                    }
+//                    else
+//                    {
+//                        avoidXNew = avoidXNewLast;
+//                    }
+//                }
+//            }
+//        }
 
 
-        if(avoidXNew<0)
-            turnLampFlag=-1;
-        else if(avoidXNew>0)
-            turnLampFlag=1;
-        else
-            turnLampFlag=0;
+//        if(avoidXNew<0)
+//            turnLampFlag=-1;
+//        else if(avoidXNew>0)
+//            turnLampFlag=1;
+//        else
+//            turnLampFlag=0;
 
-        if(fabs(avoidXNew - avoidXNewLast)>0.1)
-        {
-            gpsTraceNow.clear();
-            gpsTraceAvoidXY.clear();
-            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
-                          avoidXNew,avoidXNewLast);
-            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
-            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
-            vehState = avoiding;
-            startAvoidGpsIns = now_gps_ins;
-            obstacle_avoid_flag=1;
-            hasCheckedAvoidLidar = false;
-            avoidXNewLast=avoidXNew;
-        }
+//        if(fabs(avoidXNew - avoidXNewLast)>0.1)
+//        {
+//            gpsTraceNow.clear();
+//            gpsTraceAvoidXY.clear();
+//            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+//                          avoidXNew,avoidXNewLast);
+//            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//            vehState = avoiding;
+//            startAvoidGpsIns = now_gps_ins;
+//            obstacle_avoid_flag=1;
+//            hasCheckedAvoidLidar = false;
+//            avoidXNewLast=avoidXNew;
+//        }
 
 //        if(avoidXNewPreVector.size()<5){
 //            avoidXNewPreVector.push_back(avoidXNewPre);
@@ -2165,7 +2203,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 //            avoidXNewLast=avoidXNew;
 //            avoidXNewPreFilter=avoidXNew;
 //        }
-    }
+//    }
 //20230407,
 //    if((vehState == preAvoid)||((avoidXNew!=0)))
 //  //    if((vehState == preAvoid)||(avoidXNew!=0))
@@ -2673,9 +2711,9 @@ else
 
     if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
     {
-        if(dSpeed>10.0)
+        if(dSpeed>20.0)
         {
-            dSpeed=10.0;   //shenlan bisai xiansu 10
+            dSpeed=20.0;   //shenlan bisai xiansu 10
         }
     }
 
@@ -3007,7 +3045,7 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
     int tracesize=800;
     if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
     {
-        tracesize=400;
+        tracesize=400;//400;
     }
     else
     {

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.h

@@ -300,7 +300,7 @@ extern double avoidX;
 //extern int avoidXNew;
 extern double avoidXNew; //20230213
 extern bool parkBesideRoad;
-extern double steerSpeed;·
+extern double steerSpeed;
 extern bool transferPieriod;
 extern bool transferPieriod2;
 extern double traceDevi;

src/detection/detection_lidar_grid/perception_lidar_vv7.cpp

@@ -122,8 +122,8 @@ static void writepctosm(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,void
 
 
 
-static float SENSOR_HEIGHT = 2.1;
-static float VEHICLE_HEIGHT = 2.6;
+static float SENSOR_HEIGHT = 1.9;
+static float VEHICLE_HEIGHT = 2.2;
 //static float local_max_slope_ = 10;
 //static float general_max_slope_ = 3;
 //static float concentric_divider_distance_ = 1.5;
@@ -282,7 +282,7 @@ static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::s
         Cpos = (x-HOR_MIN)/GRID_SIZE;
         if((Rpos>=0)&&(Rpos<nVecSize)&&(Cpos>=0)&&(Cpos<nHorSize))
         {
-            if((z-floor_h[Rpos*nHorSize+Cpos])>0.3)
+            if((z-floor_h[Rpos*nHorSize+Cpos])>0.15)
             {
                point_cloud_nofloor->push_back(po);
                ++point_cloud_nofloor->width;

src/v2x/v2xpc5/mainwindow.cpp

@@ -678,34 +678,38 @@ void MainWindow::outLight(onelightMessage light1)
         }
     }
     protobuf.set_radiolightremain(light1.timeRemaining);
+//    if(visionflag)
+//    {
+//        float yaw=m_structMGpsImu.yaw+180;
+//        if (yaw>=360)
+//        {
+//            yaw=yaw-360;
+//        }
+//        if (yaw<=light1Stophead+30 and yaw>=light1Stophead-30)//判断与正向红绿灯的朝向
+//        {
+//            lightStopLat=light1StopLat;
+//            lightStopLon=light1StopLon;
+
+//        }
+//        else if (yaw<=light2Stophead+30 and yaw>=light2Stophead-30)//判断与侧向红绿灯的朝向
+//        {
+//            lightStopLat=light2StopLat;
+//            lightStopLon=light2StopLon;
+//        }
+//        else{
+//            lightStopLat=0xff;
+//            lightStopLon=0xff;
+//        }
+//        protobuf.set_trafficlightstoplat(lightStopLat);
+//        protobuf.set_trafficlightstoplon(lightStopLon);
+//        //sendProto(protobuf);
+
+//    }
     if(visionflag)
     {
-        float yaw=m_structMGpsImu.yaw+180;
-        if (yaw>=360)
-        {
-            yaw=yaw-360;
-        }
-        if (yaw<=light1Stophead+30 and yaw>=light1Stophead-30)//判断与正向红绿灯的朝向
-        {
-            lightStopLat=light1StopLat;
-            lightStopLon=light1StopLon;
-
-        }
-        else if (yaw<=light2Stophead+30 and yaw>=light2Stophead-30)//判断与侧向红绿灯的朝向
-        {
-            lightStopLat=light2StopLat;
-            lightStopLon=light2StopLon;
-        }
-        else{
-            lightStopLat=0xff;
-            lightStopLon=0xff;
-        }
-        protobuf.set_trafficlightstoplat(lightStopLat);
-        protobuf.set_trafficlightstoplon(lightStopLon);
-        //sendProto(protobuf);
-
+        lightStopLat=light1StopLat;
+        lightStopLon=light1StopLon;
     }
-
     protobuf.set_trafficlightstoplat(lightStopLat);
     protobuf.set_trafficlightstoplon(lightStopLon);
     //sendProto(protobuf);