Change decition_brain_sf. add side drive.

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/decition/brain.cpp

@@ -278,6 +278,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);
+
 
 
 
@@ -326,6 +329,7 @@ void iv::decition::BrainDecition::run() {
 
 
 
+
     /*=========================    end   =============================*/
 
     std::string group_cotrol = xp.GetParam("group","false");
@@ -1163,6 +1167,22 @@ 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);
+}
+
 void iv::decition::BrainDecition::UpdateMap(const char *mapdata, const int mapdatasize)
 {
     //    std::cout<<"update map "<<std::endl;
@@ -1202,6 +1222,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/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;