控制根据地图曲率判断是否避障

src/decition/decition_brain_sf_jsguide/decition/decide_gps_00.cpp

@@ -1863,7 +1863,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         int road_permit_sum=0;
         for(int k=PathPoint;k<PathPoint+400;k++){
                 //if((gpsMapLine[k]->mbnoavoid==false)&&(gpsMapLine[k]->roadSum>1))
-            if(gpsMapLine[k]->mbnoavoid==false)   //apollo_fu 20220622
+            if((gpsMapLine[k]->mbnoavoid==false) && (abs(gpsMapLine[k]->mfCurvature) < 0.03) )   //apollo_fu 20220622
                 road_permit_sum++;
         }
         if(road_permit_sum>=400)
@@ -4364,9 +4364,13 @@ double iv::decition::DecideGps00::computeDistToEnd(const std::vector<GPSData> gp
 
 void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,int* avoidXLeft, int* avoidXRight ){
 
-    double RightBoundary=(((double)roadOriginal+0.5)*roadWidthEvery-vehicleWidth/2);
+    //double RightBoundary=(((double)roadOriginal+0.5)*roadWidthEvery-vehicleWidth/2);
+    double RightBoundary=(((double)roadOriginal+0.5)*roadWidthEvery-vehicleWidth);
+
     *avoidXRight=((int)RightBoundary);
-    double LeftBoundary=(((double)(roadSumMap-roadOriginal-1)+0.5)*roadWidthEvery-vehicleWidth/2);
+    //double LeftBoundary=(((double)(roadSumMap-roadOriginal-1)+0.5)*roadWidthEvery-vehicleWidth/2);
+    double LeftBoundary=(((double)(roadSumMap-roadOriginal-1)+0.5)*roadWidthEvery-vehicleWidth);
+
     *avoidXLeft=(-(int)LeftBoundary);
 }
 

src/detection/detection_lidar_grid/perception_lidar_vv7.cpp

@@ -121,7 +121,7 @@ static void writepctosm(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,void
 
 
 
-static float SENSOR_HEIGHT = 0.63;
+static float SENSOR_HEIGHT = 1.35;
 static float VEHICLE_HEIGHT = 1.50;
 //static float local_max_slope_ = 10;
 //static float general_max_slope_ = 3;
@@ -700,7 +700,7 @@ int main(int argc, char *argv[])
         return 0;
     }
 
-    snprintf(gstr_sensorheight,255,"0.63");
+    snprintf(gstr_sensorheight,255,"1.35");
     snprintf(gstr_vehicleheight,255,"1.5");
     snprintf(gstr_inputmemname,255,"lidar_pc");
     snprintf(gstr_outputmemname,255,"li_ra_fusion");

src/driver/driver_lidar_rs16/lidar_driver_rs16.cpp

@@ -277,7 +277,7 @@ void process_rs16obs(char * strdata,int nLen)
     Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序（绕旋转后的轴接着旋转）
     // 四元数-->>旋转矩阵
     rotation_matrix = q_zyx.toRotationMatrix();
-    trans_matrix<<0, 0.85, 0.72;
+    trans_matrix<<0, 0, 0;
 //    int dx;
 //    int dy;
 //    float lidar_32_xdistance = 0.3;			//32线激光雷达X轴补偿