hapo obstalce avoid modify

src/decition/decition_brain/decition/adc_tools/compute_00.cpp

@@ -107,11 +107,11 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
                                 {
                                 if(MarkingCount<150)
                                 {
-                                     if((BackAveFive-FrontAveFive)<=4.0)
+                                     if((BackAveFive-FrontAveFive)<=3.5)
                                      {
                                            (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
                                      }
-                                     else if((BackAveFive-FrontAveFive)>4.0)
+                                     else if((BackAveFive-FrontAveFive)>3.5)
                                      {
                                            (*gpsMap[MarkingIndex]).roadMode=15;
                                      }

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

@@ -5,6 +5,8 @@
 #include <iostream>
 #include <fstream>
 
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
 
 using namespace std;
 
@@ -41,7 +43,7 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
         controlSpeed=0;
         controlBrake=u; //102
         if(obsDistance>0 && obsDistance<10){
-            controlBrake= u*1.5;
+            controlBrake= u*1.0;     //1.5    zj-1.2
         }
          if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
                  && dSpeed>0 && lastBrake==0){
@@ -164,33 +166,42 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
     }
 
     if(controlSpeed>0){
-        controlSpeed=max(controlSpeed,2.4f);
+        controlSpeed=max(controlSpeed,4.6f);
     }
 
+
+
+
+
+
+
     //0227 10m nei xianzhi shache
-    if(obsDistance<10 &&obsDistance>0){
+    if(obsDistance<18 &&obsDistance>0){
         controlSpeed=0;
-        controlBrake=max(controlBrake,0.8f);
-    }
+        controlBrake=max(controlBrake,0.6f);//0.8   zj-0.6
+        controlBrake=min(controlBrake,0.8f);
 
+    }
 
     if(DecideGps00::minDecelerate<0){
         controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
         controlSpeed=0;
     }
 
+
     controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
 
     controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
 
    // (*decition)->brake = controlBrake*10;
-      (*decition)->brake = controlBrake*9;
+      (*decition)->brake = controlBrake*8;//9     zj-6
 
     (*decition)->torque=controlSpeed;
       lastBrake= (*decition)->brake;
 
 
-
+      givlog->debug("brain_decition","brake: %f, torque: %f,obsDistance: %f",
+                    (*decition)->brake,(*decition)->torque,obsDistance);
 
 
     lastTorque=(*decition)->torque;
@@ -209,7 +220,7 @@ iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_
     (*decition)->angleEnable=true;
     (*decition)->speedEnable=true;
     (*decition)-> brakeEnable=true;
-    (*decition)->air_enable = true;
+    (*decition)->air_enable = false;
     (*decition)->driveMode=1;
     (*decition)->brakeType=0;
     (*decition)->angSpeed=60;

src/decition/decition_brain_sf/decition/brain.cpp

@@ -494,31 +494,31 @@ 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);
 

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -978,11 +978,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         roadSum = gpsMapLine[PathPoint]->roadSum*3;
     }
 
+
     if(roadNowStart){
         roadNow=roadOri;
         roadNowStart=false;
     }
 
+
     if(ServiceCarStatus.avoidObs){
          gpsMapLine[PathPoint]->runMode=1;
     }else{
@@ -1517,7 +1519,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     //shiyanbizhang 0929
-    if(obsDistance>0&& (obsDistance<ServiceCarStatus.socfDis+20.0) &&(abs(realspeed)<1.0) &&
+    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis) &&(abs(realspeed)<5.0) &&
             (vehState==normalRun||vehState==backOri || vehState==avoiding) &&(ObsTimeStart==-1)
             && (gpsMapLine[PathPoint]->runMode==1)){
         ObsTimeStart=GetTickCount();
@@ -1537,7 +1539,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         cout<<"\n====================preAvoid time count finish==================\n"<<endl;
     }
 
-    if(obsDistance<0 ||obsDistance>ServiceCarStatus.socfDis){
+    if(obsDistance<0 ||obsDistance>ServiceCarStatus.aocfDis){
         ObsTimeStart=-1;
         ObsTimeEnd=-1;
         ObsTimeSpan=-1;
@@ -1581,6 +1583,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
 
+
+      givlog->debug("brain_decition","mfRoadwidth: %f",
+                   gpsMapLine[PathPoint]->mfLaneWidth );
+
+
     if (vehState == preAvoid)
     {
         cout<<"\n====================preAvoid==================\n"<<endl;
@@ -1899,7 +1906,12 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
    // if(obsDistance>6.5){
      //   obsDistance=500;
     //}
-    if(obsDistance>0 && obsDistance<10){
+
+    if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
+        if(obsDistance>0 && obsDistance<8){
+                dSpeed=0;
+            }
+    }else if(obsDistance>0 && obsDistance<15){
         dSpeed=0;
     }
     //  obsDistance=-1; //1227
@@ -2555,10 +2567,12 @@ void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
         obsSpeed=obsSd;
     }
 
-
-
+    if(obsDistance<500&&obsDistance>0){
+     std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
+    }
     std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
 
+
     ServiceCarStatus.mObs = obsDistance;
     if(ServiceCarStatus.mObs>100){
         ServiceCarStatus.mObs =-1;
@@ -2903,7 +2917,7 @@ int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS
 
     //if ((GetDistance(now_gps_ins, startAvoid_gps_ins)>avoidRunDistance)&&
     //(checkReturnEnable(avoidX, lidarGridPtr,roadOri)))
-    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)) &&
+    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],20.0)) &&
             (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
     {
         roadPre = roadOri;
@@ -3344,7 +3358,7 @@ void iv::decition::DecideGps00::getEsrObsDistanceByFrenet(const std::vector<Poin
 void iv::decition::DecideGps00::getV2XTrafficPositionVector(const std::vector<GPSData> gpsMapLine){
     v2xTrafficVector.clear();
     for (int var = 0; var < gpsMapLine.size(); var++) {
-        if(gpsMapLine[var]->roadMode==6){
+        if(gpsMapLine[var]->roadMode==6|| gpsMapLine[var]->mode2==1000001){
             v2xTrafficVector.push_back(var);
         }
     }
@@ -3473,28 +3487,28 @@ float iv::decition::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData g
         return 0;
     }
     float x=0;
-    float veh_to_roadSide=(gps->mfRoadWidth- ServiceCarStatus.msysparam.vehWidth)*0.5;
+    float veh_to_roadSide=(gps->mfLaneWidth- ServiceCarStatus.msysparam.vehWidth)*0.5;
     float roadSide_to_roadSide=ServiceCarStatus.msysparam.vehWidth;
     if(!ServiceCarStatus.inRoadAvoid){
-        x= (roadOri-roadAim)*gps->mfRoadWidth;
+        x= (roadOri-roadAim)*gps->mfLaneWidth;
     }else{
         int num=roadOri-roadAim;
         switch (abs(num)%3) {
         case 0:
-            x=(num/3)*gps->mfRoadWidth;
+            x=(num/3)*gps->mfLaneWidth;
             break;
         case 1:
             if(num>0){
-                x=(num/3)*gps->mfRoadWidth +veh_to_roadSide;
+                x=(num/3)*gps->mfLaneWidth +veh_to_roadSide;
             }else{
-                x=(num/3)*gps->mfRoadWidth -veh_to_roadSide;
+                x=(num/3)*gps->mfLaneWidth -veh_to_roadSide;
             }
             break;
         case 2:
             if(num>0){
-                x=(num/3)*gps->mfRoadWidth +veh_to_roadSide+roadSide_to_roadSide;
+                x=(num/3)*gps->mfLaneWidth +veh_to_roadSide+roadSide_to_roadSide;
             }else{
-                x=(num/3)*gps->mfRoadWidth -veh_to_roadSide-roadSide_to_roadSide;
+                x=(num/3)*gps->mfLaneWidth -veh_to_roadSide-roadSide_to_roadSide;
             }
 
             break;

src/detection/detection_lidar_ukf_pda/imm_ukf_pda.cpp

@@ -690,6 +690,8 @@ void ImmUkfPda::makeOutput(const iv::lidar::objectarray & input,
     double tyaw = targets_[i].x_merge_(3);
     double tyaw_rate = targets_[i].x_merge_(4);
 
+    std::cout<<"          v     tyaw :      "<<tv<<"  "   <<tyaw<<std::endl;
+
     while (tyaw > M_PI)
       tyaw -= 2. * M_PI;
     while (tyaw < -M_PI)

src/fusion/fusion_pointcloud_bus/lidarmerge.cpp

@@ -46,6 +46,8 @@ pcl::PointCloud<pcl::PointXYZI>::Ptr mergefunc(std::vector<iv::lidar_data> xvect
                 if(point.x>xvectorlidar[i].fmax_x)continue;
                 if(point.y>xvectorlidar[i].fmax_y)continue;
                 if(point.z>xvectorlidar[i].fmax_z)continue;
+                if(point.z > xvectorlidar[i].fignore_zmax) continue;
+//                std::cout<<"  fignore  zmax  "<<xvectorlidar[i].fignore_zmax<<std::endl;
 
                 if(point.x<xvectorlidar[i].fmin_x)continue;
                 if(point.y<xvectorlidar[i].fmin_y)continue;

src/fusion/fusion_pointcloud_bus/lidarmerge.h

@@ -24,6 +24,7 @@ struct lidar_data
     double fignore_ymax = 0;
     double fignore_xmin = 0;
     double fignore_ymin = 0;
+    double fignore_zmax = 0;
     pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud = NULL;
     int64_t mupdatetime = 0;
     pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud_old = NULL;

src/fusion/fusion_pointcloud_bus/main.cpp

@@ -68,6 +68,7 @@ void dec_yaml(const char * stryamlpath)
     double fignore_ymax = 0;
     double fignore_xmin = 0;
     double fignore_ymin = 0;
+    double fignore_zmax = 0;
 
     if(config["ignore"]["xmin"])
     {
@@ -92,6 +93,11 @@ void dec_yaml(const char * stryamlpath)
         strtem = config["ignore"]["ymax"].as<std::string>();
         fignore_ymax = atof(strtem.data());
     }
+    if(config["ignore"]["zmax"])
+    {
+        strtem = config["ignore"]["zmax"].as<std::string>();
+        fignore_zmax = atof(strtem.data());
+    }
 
 
     for(i=0;i<nlidarsize;i++)
@@ -144,6 +150,8 @@ void dec_yaml(const char * stryamlpath)
                 xlidardata.fignore_ymax = fignore_ymax;
                 xlidardata.fignore_xmin = fignore_xmin;
                 xlidardata.fignore_ymin = fignore_ymin;
+                xlidardata.fignore_zmax = fignore_zmax;
+
 
                 strncpy(xlidardata.strmemname,strmemname.data(),255);
                 gvectorlidar.push_back(xlidardata);