change decition_brain_sf_Jeely_xingyueL.

src/decition/decition_brain_sf_Jeely_xingyueL/decition/decide_gps_00.cpp

@@ -228,15 +228,20 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //    vector<iv::Point2D> fpTraceTmp;
 
 
-    bool bgroupctrl = false;
+
+
     qint64 ngroupvalid = 3000;
-    iv::group::groupctrl xgroupctrl;
+
     if((QDateTime::currentMSecsSinceEpoch() - ServiceCarStatus.mgroupctrlupdatetime ) < ngroupvalid)
     {
         ServiceCarStatus.mMutexgroupctrl.lock();
-        xgroupctrl.CopyFrom(ServiceCarStatus.mgroupgctrl);
+        mgroupctrl.CopyFrom(ServiceCarStatus.mgroupgctrl);
         ServiceCarStatus.mMutexgroupctrl.unlock();
-        bgroupctrl = true;
+        mbgroupctrl = true;
+    }
+    else
+    {
+        mbgroupctrl = false;
     }
 
     bool bgroupgrpc = false;
@@ -1227,7 +1232,34 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         roadNow = roadOri;
     }
 
-    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
+    if(mbgroupctrl == false)
+    {
+        gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
+    }
+    else
+    {
+        bool boff = false;
+        double foff = 0;
+        if((ServiceCarStatus.curID == 1) &&(fabs(mgroupctrl.ffrontchange())>0.01))
+        {
+            foff = mgroupctrl.ffrontchange();
+            boff = true;
+        }
+        if((ServiceCarStatus.curID == 2 &&(fabs(mgroupctrl.frearchange()))>0.01))
+        {
+            foff = mgroupctrl.frearchange();
+            boff = true;
+        }
+        if(boff)
+        {
+            gpsTraceOri= getGpsTraceInGroup(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode,foff);
+        }
+        else
+        {
+            gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
+        }
+    }
+
     gpsTraceMapOri=gpsTraceOri;
 
     if((vehState == avoiding)&&(obstacle_avoid_flag == 1)){
@@ -1600,9 +1632,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             }
     }
 
-    if(bgroupctrl)
+    if(mbgroupctrl)
     {
-        dSpeed = min(xgroupctrl.fgroupspeed(),dSpeed);
+        dSpeed = min(mgroupctrl.fgroupspeed(),dSpeed);
     }
 
     if (gpsMapLine[PathPoint]->speed_mode == 2)
@@ -2845,7 +2877,138 @@ void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decit
     }
 }
 
+std::vector<Point2D> iv::decition::DecideGps00::getGpsTraceInGroup(GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode,double foff)
+{
+    vector<iv::Point2D> trace;
+    traceOriLeft.clear();
+    traceOriRight.clear();
+
+
+
+
+    if (gpsMapLine.size() > 800 && PathPoint >= 0) {
+        int aimIndex;
+        if(circleMode){
+            aimIndex=PathPoint+800;
+        }else{
+            aimIndex=min((PathPoint+800),(int)gpsMapLine.size());
+        }
+
+        double fMove = 0;
+        bool boff = true;
+
+        for (int i = PathPoint; i < aimIndex; i++)
+        {
+             int index = i % gpsMapLine.size();
+
+             if(foff>0)
+             {
+                 if(((*gpsMapLine[index]).mfDisToRoadLeft - foff -1.0)<0)
+                 {
+                     boff = false;
+                     break;
+                 }
+             }
+             else
+             {
+                 if(((*gpsMapLine[index]).mfDisToRoadLeft - foff +1.0)> ((*gpsMapLine[index]).mfRoadWidth))
+                 {
+                     boff = false;
+                     break;
+                 }
+             }
+        }
+
+        if(boff)fMove = foff;
+
+
+        for (int i = PathPoint; i < aimIndex; i++)
+        {
+            int index = i % gpsMapLine.size();
+
+            double xraw = (*gpsMapLine[index]).gps_x;
+            double yraw = (*gpsMapLine[index]).gps_y;
+            double fhead = (90 - (*gpsMapLine[index]).ins_heading_angle) *M_PI/180.0;
+            xraw = xraw  + fMove *cos(fhead + M_PI/2.0);
+            yraw = yraw + fMove * sin(fhead + M_PI/2.0);
+            Point2D pt = Coordinate_Transfer(xraw, yraw, now_gps_ins);
+            pt.x += offset_real * 0.032;
+            pt.v1 = (*gpsMapLine[index]).speed_mode;
+            pt.v2 = (*gpsMapLine[index]).mode2;
+            pt.roadMode=(*gpsMapLine[index]).roadMode;
+            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+            {
+                readyZhucheMode = true;
+                DecideGps00::zhuchePointNum = index;
+            }
+
+            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+            {
+                readyZhucheMode = true;
+                DecideGps00::zhuchePointNum = index;
+            }
+
+            //csvv7
+            if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
+            {
+                readyParkMode = true;
+                DecideGps00::finishPointNum = index;
+            }
+            switch (pt.v1)
+            {
+            case 0:
+                pt.speed = 80;
+                break;
+            case 1:
+                pt.speed = 25;
+                break;
+            case 2:
+                pt.speed =25;
+                break;
+            case 3:
+                pt.speed = 20;
+                break;
+            case 4:
+                pt.speed =18;
+                break;
+            case 5:
+                pt.speed = 18;
+                break;
+            case 7:
+                pt.speed = 10;
+                break;
+            case 22:
+                pt.speed = 5;
+                break;
+            case 23:
+                pt.speed = 5;
+                break;
+            default:
+                break;
+            }
+            trace.push_back(pt);
+
+
+            double hdg=90 - (*gpsMapLine[index]).ins_heading_angle;
+            if(hdg < 0)  hdg = hdg + 360;
+            if(hdg >= 360) hdg = hdg - 360;
+
+            double xyhdg  = hdg/180.0*PI;
+            double ptLeft_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg + PI / 2);
+            double ptLeft_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg + PI / 2);
+            Point2D ptLeft = Coordinate_Transfer(ptLeft_x, ptLeft_y, now_gps_ins);
+
+            double ptRight_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg - PI / 2);
+            double ptRight_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg - PI / 2);
+            Point2D ptRight = Coordinate_Transfer(ptRight_x, ptRight_y, now_gps_ins);
+
+            traceOriLeft.push_back(ptLeft);
+            traceOriRight.push_back(ptRight);
 
+        }
+    }
+    return trace;
+}
 
 std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
     vector<iv::Point2D> trace;
@@ -2853,6 +3016,10 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
     traceOriLeft.clear();
     traceOriRight.clear();
 
+
+    double fMove = 0;
+
+
     if (gpsMapLine.size() > 800 && PathPoint >= 0) {
         int aimIndex;
         if(circleMode){

src/decition/decition_brain_sf_Jeely_xingyueL/decition/decide_gps_00.h

@@ -25,6 +25,8 @@
 
 #include "decition/groupspeedctrl.h"
 
+#include "groupctrl.pb.h"
+
 namespace iv {
 namespace decition {
 //根据传感器传输来的信息作出决策
@@ -186,6 +188,7 @@ public:
 
     void initMethods();
     static std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, bool circleMode);
+    static std::vector<Point2D> getGpsTraceInGroup(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, bool circleMode,double foff);
     static std::vector<Point2D> getGpsTraceAvoid(GPS_INS rp, const std::vector<Point2D> gpsTrace, int lastIndex, bool circleMode);
 
 
@@ -284,6 +287,9 @@ public:
     GroupSpeedCtrl mGroupSpeedCtrl;
     bool mbGroupSpeedCtrl = false;
 
+    bool mbgroupctrl = false;
+    iv::group::groupctrl mgroupctrl;
+
 private:
     //  void changeRoad(int roadNum);