add side driving

src/decition/common/common/sysparam_type.h

@@ -33,6 +33,9 @@ namespace iv {
 
         bool keni =false;
 
+        bool mbSideDrive=false; //vehicle side driving
+        double mfSideDis=0.3;//vehicle right to lane right distance
+
 
     };
 }

src/decition/decition_brain_sf_jsguide/decition/brain.cpp

@@ -1304,7 +1304,20 @@ void iv::decition::BrainDecition::stop() {
     thread_run->interrupt();
     thread_run->join();
 }
-
+void iv::decition::BrainDecition::SideMove(iv::GPS_INS &x)
+{
+    if(ServiceCarStatus.msysparam.mbSideDrive == false)return;
+    double fhdg = (90 - x.ins_heading_angle)*M_PI/180.0;
+    double rel_x,rel_y;
+    GaussProjCal(x.gps_lng,x.gps_lat,&rel_x,&rel_y);
+    double fmove = 0;
+    fmove = ServiceCarStatus.msysparam.mfSideDis + ServiceCarStatus.msysparam.vehWidth/2.0 - x.mfLaneWidth +x.mfDisToLaneLeft;
+    if(fmove == 0.0)return;
+    double rel_x1,rel_y1;
+    rel_x1 = rel_x + fmove * cos(fhdg + M_PI/2.0);
+    rel_y1 = rel_y + fmove * sin(fhdg + M_PI/2.0);
+    GaussProjInvCal(rel_x1,rel_y1,&x.gps_lng,&x.gps_lat);
+}
 void iv::decition::BrainDecition::UpdateMap(const char *mapdata, const int mapdatasize)
 {
     //    std::cout<<"update map "<<std::endl;
@@ -1344,6 +1357,7 @@ void iv::decition::BrainDecition::UpdateMap(const char *mapdata, const int mapda
             iv::GPS_INS x;
             memcpy(&x,mapdata + i*gpsunitsize,gpsunitsize);
             iv::GPSData data(new iv::GPS_INS);
+            if(ServiceCarStatus.msysparam.mbSideDrive)SideMove(x);
             *data = x;
             navigation_data.push_back(data);