add function that aavoid in road for decition_brain and decition_brain_sf

sh/HAPO/agx1/ADS_decision.xml

@@ -31,6 +31,9 @@
 		<param name="frontGpsXiuzheng" value="3.7" />
 		<param name="gpsOffset_X" value="0" />
 		<param name="gpsOffset_Y" value="2.2" />
+		<param name="vehWidth" value="2.1" />
+		<param name="avoidObs" value="false" />
+		<param name="avoidInRoad" value="true" />
 	</node>
 </xml>
 

src/decition/common/common/car_status.h

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

src/decition/decition_brain/decition/brain.cpp

@@ -411,6 +411,14 @@ void iv::decition::BrainDecition::run() {
         ServiceCarStatus.inRoadAvoid = false;
     }
 
+    std::string avoidObs = xp.GetParam("avoidObs","false");  //If Use MPC set true
+    if(avoidObs == "true")
+    {
+        ServiceCarStatus.avoidObs = true;
+    }else{
+        ServiceCarStatus.avoidObs = false;
+    }
+
     //map
 
     mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -980,7 +980,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         roadSum = gpsMapLine[PathPoint]->roadSum*3;
     }
 
-
+    if(ServiceCarStatus.avoidObs){
+         gpsMapLine[PathPoint]->runMode=1;
+    }else{
+         gpsMapLine[PathPoint]->runMode=0;
+    }
 
 
  //   avoidX = (roadOri-roadNow)*roadWidth;
@@ -3882,13 +3886,7 @@ float iv::decition::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData g
         default:
             break;
         }
-
-
-
     }
 
-
     return x;
-
-
 }

src/decition/decition_brain_sf/decition/brain.cpp

@@ -369,6 +369,22 @@ void iv::decition::BrainDecition::run() {
     }
     //map
 
+    std::string inRoadAvoid = xp.GetParam("inRoadAvoid","true");  //If Use MPC set true
+    if(inRoadAvoid == "true")
+    {
+        ServiceCarStatus.inRoadAvoid = true;
+    }else{
+        ServiceCarStatus.inRoadAvoid = false;
+    }
+
+    std::string avoidObs = xp.GetParam("avoidObs","false");  //If Use MPC set true
+    if(avoidObs == "true")
+    {
+        ServiceCarStatus.avoidObs = true;
+    }else{
+        ServiceCarStatus.avoidObs = false;
+    }
+
     mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
 
 

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -926,7 +926,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 
-    avoidX = (roadOri-roadNow)*roadWidth;
+
 
     gpsTraceOri.clear();
     gpsTraceNow.clear();
@@ -970,9 +970,23 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 
-    roadOri = gpsMapLine[PathPoint]->roadOri;
+    if(!ServiceCarStatus.inRoadAvoid){
+        roadOri = gpsMapLine[PathPoint]->roadOri;
+        roadSum = gpsMapLine[PathPoint]->roadSum;
+    }else{
+        roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
+        roadSum = gpsMapLine[PathPoint]->roadSum*3;
+    }
+
+    if(ServiceCarStatus.avoidObs){
+         gpsMapLine[PathPoint]->runMode=1;
+    }else{
+         gpsMapLine[PathPoint]->runMode=0;
+    }
+
 
-    roadSum = gpsMapLine[PathPoint]->roadSum;
+    // avoidX = (roadOri-roadNow)*roadWidth;
+     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
 
 
     if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
@@ -1038,7 +1052,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if(useFrenet){
         //如果车辆在变道中，启用frenet规划局部路径
         if(vehState == avoiding || vehState == backOri){
-            avoidX = (roadOri - roadNow)*roadWidth;
+           // avoidX = (roadOri - roadNow)*roadWidth;
+             avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
             gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
             //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
             gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
@@ -1470,14 +1485,16 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else if(useOldAvoid){
             roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
-            avoidX = (roadOri - roadNow)*roadWidth; //1012
+          //  avoidX = (roadOri - roadNow)*roadWidth; //1012
+             avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         }
     }
     if (roadNow != roadOri && roadPre!=-1)
     {
 
         roadNow = roadPre;
-        avoidX = (roadOri - roadNow)*roadWidth;
+   //     avoidX = (roadOri - roadNow)*roadWidth;
+         avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         gpsTraceNow.clear();
         if(useOldAvoid){
             gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
@@ -1573,7 +1590,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             //roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine);
             if(useOldAvoid){
                 roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine,lidar_per);
-                avoidX = (roadOri - roadNow)*roadWidth;  //1012
+              //  avoidX = (roadOri - roadNow)*roadWidth;  //1012
+                 avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
             }
             else if(useFrenet){
                 double offsetL = -(roadSum - 1)*roadWidth;
@@ -1590,7 +1608,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             {
                 if(useOldAvoid){
                     roadNow = roadPre;
-                    avoidX = (roadOri - roadNow)*roadWidth;
+                  //  avoidX = (roadOri - roadNow)*roadWidth;
+                     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
                     gpsTraceNow.clear();
                     gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
                 }
@@ -1600,7 +1619,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         startAvoidGpsInsVector[roadNow] = now_gps_ins;
                     }
                     roadNow = roadPre;
-                    avoidX = (roadOri - roadNow)*roadWidth;
+                //    avoidX = (roadOri - roadNow)*roadWidth;
+                     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
                     gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
                 }
 
@@ -2749,7 +2769,8 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
     {
         gpsTraceAvoid.clear();
         //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-        avoidX = (roadWidth)*(roadOri - i);
+  //      avoidX = (roadWidth)*(roadOri - i);
+         avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
         //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
         computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
@@ -2775,7 +2796,8 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
     for (int i = 1; i < roadSum; i++)
     {
         if (roadNow + i < roadSum) {
-            avoidX = (roadOri-roadNow-i)*roadWidth;
+         //   avoidX = (roadOri-roadNow-i)*roadWidth;
+             avoidX=computeAvoidX(roadNow+i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
             if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow+i))
             {
                 /*if (roadNow==roadOri)
@@ -2792,7 +2814,8 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
 
         if (roadNow - i >= 0)
         {
-            avoidX = (roadOri - roadNow+i)*roadWidth;
+        //    avoidX = (roadOri - roadNow+i)*roadWidth;
+             avoidX=computeAvoidX(roadNow-i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
             if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow - i))
             {
                 /*if (roadNow == roadOri)
@@ -2847,7 +2870,8 @@ int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS
     {
         gpsTraceBack.clear();
         //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-        avoidX = (roadWidth)*(roadOri - i);
+      //  avoidX = (roadWidth)*(roadOri - i);
+         avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
         gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
         computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
     }
@@ -3438,3 +3462,41 @@ void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gps
         dSpeed=min(dSpeed,5.0);
     }
 }
+
+float iv::decition::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth){
+    if(roadAim==roadOri){
+        return 0;
+    }
+    float x=0;
+    float veh_to_roadSide=(gps->mfRoadWidth- ServiceCarStatus.msysparam.vehWidth)*0.5;
+    float roadSide_to_roadSide=ServiceCarStatus.msysparam.vehWidth;
+    if(!ServiceCarStatus.inRoadAvoid){
+        x= (roadOri-roadAim)*gps->mfRoadWidth;
+    }else{
+        int num=roadOri-roadAim;
+        switch (num%3) {
+        case 0:
+            x=(num/3)*gps->mfRoadWidth;
+            break;
+        case 1:
+            if(num>0){
+                x=(num/3)*gps->mfRoadWidth +veh_to_roadSide;
+            }else{
+                x=(num/3)*gps->mfRoadWidth -veh_to_roadSide;
+            }
+            break;
+        case 2:
+            if(num>0){
+                x=(num/3)*gps->mfRoadWidth +veh_to_roadSide+roadSide_to_roadSide;
+            }else{
+                x=(num/3)*gps->mfRoadWidth -veh_to_roadSide-roadSide_to_roadSide;
+            }
+
+            break;
+        default:
+            break;
+        }
+
+    }
+    return x;
+}

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -205,6 +205,7 @@ public:
 
 
     void transferGpsMode2( const std::vector<GPSData> gpsMapLine);
+    float computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth);
 
     GPS_INS aim_gps_ins;
     GPS_INS chaoche_start_gps;