change old_bz funciotn

autodeploy.sh

@@ -8,34 +8,37 @@ if [ ! $check_result ];then
 fi
 
 app_name=(
-driver_lidar_rs16
-#driver_lidar_vlp16
+#driver_lidar_rs16
+driver_lidar_vlp16
 driver_gps_hcp2
-driver_radio_p900
-#driver_can_nvidia_agx
+driver_camera_miivii
+driver_cloud_grpc_client
+#driver_radio_p900
+driver_can_nvidia_agx
 #driver_can_kvaser
 #driver_can_vci
 driver_map_trace
-driver_radio_p900
+#driver_radio_p900
 detection_radar_delphi_esr
 detection_lidar_grid
-#view_pointcloud
+view_pointcloud
 view_gps
 view_rawcan
 view_radar
 IVSysMan
 ivmapmake
 view_ivlog
-tool_querymsg
+#tool_querymsg
 detection_chassis
 ui_ads_hmi
 decition_brain
 #decition_brain_ge3
-controller_midcar
+#controller_midcar
 driver_map_xodrload
 tool_xodrobj
 ivlog_record
 adciv_record
+picview
 )
 
 for x in ${app_name[@]}

autogen.sh

@@ -1,7 +1,7 @@
 
-qtmake="/opt/Qt5.10.1/5.10.1/gcc_64/bin/qmake"
+#qtmake="/opt/Qt5.10.1/5.10.1/gcc_64/bin/qmake"
 #qtmake=/usr/bin/qmake
-#qtmake="/usr/lib/aarch64-linux-gnu/qt5/bin/qmake"
+qtmake="/usr/lib/aarch64-linux-gnu/qt5/bin/qmake"
 if [ ! $qtmake ];then
 	echo -e "\e[33m qtmake not set, auto find it\e[0m"
 	qtmake=`find /opt -name "qmake" 2>/dev/null | grep 'gcc_64'`
@@ -145,9 +145,9 @@ rm .qmake.stash
 cd ../../../
 
 controller_app_name=(
-	controller_ge3
+#	controller_ge3
 	controller_vv7
-    controller_midcar
+#    controller_midcar
 )
 
 for x in ${controller_app_name[@]}
@@ -186,17 +186,19 @@ do
 done
 
 driver_app_name=(
-	driver_lidar_rs16
-#	driver_lidar_vlp16
+#	driver_lidar_rs16
+	driver_lidar_vlp16
 	driver_gps_hcp2
 	#driver_gps_ins550d
 	driver_can_nvidia_agx
-	driver_can_kvaser
-	driver_radio_p900
+#	driver_can_kvaser
+#	driver_radio_p900
 #	driver_can_vci
 	driver_map_trace
 	driver_map_xodrload
-    driver_radio_p900
+#   driver_radio_p900
+	driver_camera_miivii
+	driver_cloud_grpc_client
 )
 
 for x in ${driver_app_name[@]}
@@ -242,10 +244,11 @@ tool_app_name=(
 	IVSysMan
 	ivmapmake
 	view_ivlog
-	tool_querymsg
+#	tool_querymsg
 	tool_xodrobj
 	ivlog_record
-	adciv_record
+	adciv_record	
+	picview
 )
 
 for x in ${tool_app_name[@]}

deploy.sh

@@ -1,7 +1,7 @@
 #! /bin/bash
 
-Qtgccdir=''
-#Qtgccdir='/usr/lib/aarch64-linux-gnu/qt5'
+#Qtgccdir=''
+Qtgccdir='/usr/lib/aarch64-linux-gnu/qt5'
 #QtLibDir=/usr/lib/aarch64-linux-gnu/
 
 

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -2978,74 +2978,85 @@ void iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::Per
 int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
     roadPre = -1;
 
-    if (roadNow<roadOri)
-    {
-        for (int i = 0; i < roadNow; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-
-
-        for (int i = roadOri+1; i < roadSum; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-        }
-    }
-    else if (roadNow>roadOri)
-    {
-        for (int i = 0; i < roadOri; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-        for (int i = roadNow + 1; i < roadSum; i++)
-        {
-            gpsTraceAvoid.clear();
-            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-
+//    if (roadNow<roadOri)
+//    {
+//        for (int i = 0; i < roadNow; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+//        }
+
+
+//        for (int i = roadOri+1; i < roadSum; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+//        }
+//    }
+//    else if (roadNow>roadOri)
+//    {
+//        for (int i = 0; i < roadOri; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//           computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+//        }
+//        for (int i = roadNow + 1; i < roadSum; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//           computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+//        }
+
+//    }
+
+//    else
+//    {
+//        for (int i = 0; i < roadOri; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//        }
+//        for (int i = roadOri + 1; i < roadSum; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//        }
+
+//    }
+
+    for (int i =  0; i < roadSum; i++)
+    {
+        gpsTraceAvoid.clear();
+        //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+        avoidX = (roadWidth)*(roadOri - i);
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+        //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+       computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
     }
 
-    else
-    {
-        for (int i = 0; i < roadOri; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-        for (int i = roadOri + 1; i < roadSum; i++)
-        {
-            gpsTraceAvoid.clear();
-            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-
-    }
 
     if (lidarGridPtr!=NULL)
     {
@@ -3106,32 +3117,43 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
 int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
     roadPre = -1;
 
-    computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+ //   computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
 
 
 
-    if (roadNow>roadOri+1)
-    {
-        for (int i = roadOri+1; i < roadNow; i++)
-        {
-            gpsTraceBack.clear();
-            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-    }
-    else if (roadNow < roadOri - 1) {
+//    if (roadNow>=roadOri+1)
+//    {
+//        for (int i = roadOri+1; i < roadNow; i++)
+//        {
+//            gpsTraceBack.clear();
+//            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//        }
+//    }
+//    else if (roadNow <= roadOri - 1) {
 
-        for (int i = roadOri - 1; i > roadNow; i--)
-        {
-            gpsTraceBack.clear();
-            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
+//        for (int i = roadOri - 1; i > roadNow; i--)
+//        {
+//            gpsTraceBack.clear();
+//            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//        }
 
+//    }
+
+
+
+    for (int i =  0; i <roadSum; i++)
+    {
+        gpsTraceBack.clear();
+        //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
+        avoidX = (roadWidth)*(roadOri - i);
+        gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+        computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
     }
 
 

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -2628,73 +2628,83 @@ void iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::Per
 int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
     roadPre = -1;
 
-    if (roadNow<roadOri)
-    {
-        for (int i = 0; i < roadNow; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-
-
-        for (int i = roadOri+1; i < roadSum; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-        }
-    }
-    else if (roadNow>roadOri)
-    {
-        for (int i = 0; i < roadOri; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-        for (int i = roadNow + 1; i < roadSum; i++)
-        {
-            gpsTraceAvoid.clear();
-            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-
-    }
-
-    else
-    {
-        for (int i = 0; i < roadOri; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-        for (int i = roadOri + 1; i < roadSum; i++)
-        {
-            gpsTraceAvoid.clear();
-            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-
+//    if (roadNow<roadOri)
+//    {
+//        for (int i = 0; i < roadNow; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//        }
+
+
+//        for (int i = roadOri+1; i < roadSum; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+//        }
+//    }
+//    else if (roadNow>roadOri)
+//    {
+//        for (int i = 0; i < roadOri; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//        }
+//        for (int i = roadNow + 1; i < roadSum; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//        }
+
+//    }
+
+//    else
+//    {
+//        for (int i = 0; i < roadOri; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//        }
+//        for (int i = roadOri + 1; i < roadSum; i++)
+//        {
+//            gpsTraceAvoid.clear();
+//            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//        }
+
+//    }
+
+    for (int i =  0; i < roadSum; i++)
+    {
+        gpsTraceAvoid.clear();
+        //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+        avoidX = (roadWidth)*(roadOri - i);
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+        //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+       computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
     }
 
     if (lidarGridPtr!=NULL)
@@ -2756,34 +2766,43 @@ int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GP
 int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
     roadPre = -1;
 
-    computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//    computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
 
 
 
-    if (roadNow>roadOri+1)
-    {
-        for (int i = roadOri+1; i < roadNow; i++)
-        {
-            gpsTraceBack.clear();
-            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-    }
-    else if (roadNow < roadOri - 1) {
+//    if (roadNow>roadOri+1)
+//    {
+//        for (int i = roadOri+1; i < roadNow; i++)
+//        {
+//            gpsTraceBack.clear();
+//            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//        }
+//    }
+//    else if (roadNow < roadOri - 1) {
 
-        for (int i = roadOri - 1; i > roadNow; i--)
-        {
-            gpsTraceBack.clear();
-            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
+//        for (int i = roadOri - 1; i > roadNow; i--)
+//        {
+//            gpsTraceBack.clear();
+//            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
+//            avoidX = (roadWidth)*(roadOri - i);
+//            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+//        }
+
+//    }
 
-    }
 
+    for (int i =  0; i <roadSum; i++)
+    {
+        gpsTraceBack.clear();
+        //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
+        avoidX = (roadWidth)*(roadOri - i);
+        gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+        computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
+    }
 
 
     if (lidarGridPtr != NULL)