注释掉写日志文件，同时添加道路靠边行驶的功能

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_tools/compute_00.cpp

@@ -462,7 +462,7 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                     }
                 }
 
-                for(int i=0;i<MapTrace.size();i++){
+                /*for(int i=0;i<MapTrace.size();i++){
                     //将数据写入文件开始
                     ofstream outfile;
                     outfile.open("ctr0108003.txt", ostream::app);
@@ -471,7 +471,7 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                            <<MapTrace[i]->mfCurvature<<endl;
                     outfile.close();
                     //将数据写入文件结束
-                }
+                }*/
 
 }
 

src/decition/decition_brain_sf_Jeely_xingyueL/decition/brain.cpp

@@ -280,6 +280,9 @@ void iv::decition::BrainDecition::run() {
     ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
     ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());
 
+    ServiceCarStatus.msysparam.mbSideDrive = xp.GetParam("SideDrive",false);
+    ServiceCarStatus.msysparam.mfSideDis = xp.GetParam("SideDis",0.3);
+
 
 
 
@@ -1165,6 +1168,24 @@ void iv::decition::BrainDecition::stop() {
     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);
+    x.gps_x = rel_x1;
+    x.gps_y = rel_y1;
+}
+
 void iv::decition::BrainDecition::UpdateMap(const char *mapdata, const int mapdatasize)
 {
     //    std::cout<<"update map "<<std::endl;
@@ -1204,6 +1225,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);
 

src/decition/decition_brain_sf_Jeely_xingyueL/decition/brain.h

@@ -181,6 +181,7 @@ namespace iv {
             void UpdateChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
             void adjuseGpsLidarPosition();
             void UpdateGRPCGroupMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+            void SideMove(iv::GPS_INS &x);
 
             iv::group::groupinfo mgroupgrpcInfo;
             qint64 mnGroupgrpcUpdateTime = 0;