change decition_brain_sf_Jeely_xingyueL. add group speed ctrl.

src/decition/decition_brain_sf_Jeely_xingyueL/decition/brain.cpp

@@ -439,6 +439,21 @@ void iv::decition::BrainDecition::run() {
     if(decitionGps00->mstopbrake < 0.1)decitionGps00->mstopbrake = 0.1;
 
 
+    if(xp.GetParam("GroupSpeedCtrl","false") == "true")
+    {
+        decitionGps00->mbGroupSpeedCtrl = true;
+    }
+
+    if(decitionGps00->mbGroupSpeedCtrl)
+    {
+        decitionGps00->mGroupSpeedCtrl.SetDecMax(xp.GetParam("GroupDecMax",-1.0));
+        decitionGps00->mGroupSpeedCtrl.SetDisMaxDec_Front(xp.GetParam("DisMaxDec_Front",30));
+        decitionGps00->mGroupSpeedCtrl.SetDisMaxDec_Next(xp.GetParam("DisMaxDec_Next",80));
+        decitionGps00->mGroupSpeedCtrl.SetDisStartDec_Front(xp.GetParam("DisStartDec_Front",50));
+        decitionGps00->mGroupSpeedCtrl.SetDisStartDec_Next(xp.GetParam("DisStartDec_Next",60));
+    }
+
+
 
      adjuseGpsLidarPosition();
 

src/decition/decition_brain_sf_Jeely_xingyueL/decition/decide_gps_00.cpp

@@ -1048,6 +1048,24 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     }
 
+    if(mbGroupSpeedCtrl)
+    {
+        if(bgroupgrpc)
+        {
+            double fdec_after = mGroupSpeedCtrl.Getmindec(xgroupgrpcinfo,gpsMapLine,PathPoint);
+            if((fdec_after<-0.001)&(fdec_after < minDecelerate))
+            {
+                minDecelerate = fdec_after;
+            }
+
+            double fdec_front = mGroupSpeedCtrl.Getmindec_front(xgroupgrpcinfo,gpsMapLine,PathPoint);
+            if((fdec_front<-0.001)&(fdec_front < minDecelerate))
+            {
+                minDecelerate = fdec_front;
+            }
+        }
+    }
+
     DecideGps00::lastGpsIndex = PathPoint;
 
 //    double brake_distance=200;

src/decition/decition_brain_sf_Jeely_xingyueL/decition/decide_gps_00.h

@@ -23,6 +23,8 @@
 #include <common/tracepointstation.h>
 #include <decition/adc_planner/spline_planner.h>
 
+#include "decition/groupspeedctrl.h"
+
 namespace iv {
 namespace decition {
 //根据传感器传输来的信息作出决策
@@ -279,10 +281,14 @@ public:
     std::vector<iv::TracePoint> planTrace;
 
     bool roadNowStart=true;
+    GroupSpeedCtrl mGroupSpeedCtrl;
+    bool mbGroupSpeedCtrl = false;
+
 private:
     //  void changeRoad(int roadNum);
 
 
+
 };
 
 }

src/decition/decition_brain_sf_Jeely_xingyueL/decition/decition.pri

@@ -30,7 +30,8 @@ HEADERS += \
     $$PWD/adc_adapter/interpolation_2d.h \
     $$PWD/adc_tools/PolynomialRegression.h \
     $$PWD/adc_tools/polyfit.h \
-    $$PWD/adc_planner/spline_planner.h
+    $$PWD/adc_planner/spline_planner.h \
+    $$PWD/groupspeedctrl.h
 
 SOURCES += \
     $$PWD/decide_gps_00.cpp \
@@ -60,4 +61,5 @@ SOURCES += \
     $$PWD/adc_adapter/sightseeing_adapter.cpp \
     $$PWD/adc_adapter/interpolation_2d.cc \
     $$PWD/adc_tools/polyfit.cpp \
-    $$PWD/adc_planner/spline_planner.cpp
+    $$PWD/adc_planner/spline_planner.cpp \
+    $$PWD/groupspeedctrl.cpp

src/decition/decition_brain_sf_Jeely_xingyueL/decition/groupspeedctrl.cpp

@@ -0,0 +1,242 @@
+#include "groupspeedctrl.h"
+
+#include "common/car_status.h"
+#include "adc_tools/transfer.h"
+
+#include "gnss_coordinate_convert.h"
+
+GroupSpeedCtrl::GroupSpeedCtrl()
+{
+
+}
+
+
+double GroupSpeedCtrl::Getmindec(iv::group::groupinfo & xgroupinfo,
+                                 const std::vector<GPSData> & gpsMapLine,int PathPoint)
+{
+
+    iv::group::vehicleinfo * pvehaffter = NULL;  //a vehicle after current vehicle
+    int i;
+    for(i=0;i<xgroupinfo.mvehinfo_size();i++)
+    {
+        if(xgroupinfo.mutable_mvehinfo(i)->intragroupid() == (ServiceCarStatus.curID+1))
+        {
+            pvehaffter = xgroupinfo.mutable_mvehinfo(i);
+            break;
+        }
+    }
+
+    if(pvehaffter == NULL)
+    {
+        return 0.0;
+    }
+
+    if(gpsMapLine.size()<1)
+    {
+        return 0.0;
+    }
+
+    if(!pvehaffter->has_mgpsimu())
+    {
+        return 0.0;
+    }
+
+    double x,y;
+    GaussProjCal(pvehaffter->mgpsimu().lon(),pvehaffter->mgpsimu().lat(),&x,&y);
+
+    int nearpoint = -1;
+    double neardis = 100000;
+
+    double nearthresh = 100;
+    int nsize = gpsMapLine.size();
+    for(i=0;i<nsize;i++)
+    {
+        if((fabs(x - gpsMapLine[i]->gps_x)<nearthresh) || (fabs(y - gpsMapLine[i]->gps_y)<nearthresh))
+        {
+            double fdis = sqrt(pow(x - gpsMapLine[i]->gps_x,2)+pow(y - gpsMapLine[i]->gps_y,2));
+            double fanglediff = pvehaffter->mgpsimu().heading() - gpsMapLine[i]->ins_heading_angle;
+            while(fanglediff<-180)fanglediff = fanglediff + 360.0;
+            while(fanglediff>180)fanglediff = fanglediff - 360.0;
+            if((fdis<nearthresh) &&(fabs(fanglediff)<90))
+            {
+                if(fdis<neardis)
+                {
+                    neardis = fdis;
+                    nearpoint = i;
+                }
+            }
+        }
+    }
+
+    if(nearpoint <0)
+    {
+        return 0.0;
+    }
+
+
+    double fdistocur = 0;  //after vehicle to now vehicle
+
+    if(PathPoint< nearpoint)  // now after the after vehicle
+    {
+        return 0.0;
+    }
+
+    for(i=(nearpoint+1);i<PathPoint;i++)
+    {
+        double fdistolast = iv::decition::GetDistance(*gpsMapLine[i],*gpsMapLine[i-1]);
+        fdistocur = fdistocur + fdistolast;
+    }
+
+    double decmax = mfDecMax;
+    double dis_startdec = mfDis_StartDec_Next;  //if > 50 start wait
+    double dis_maxdec = mfDis_MaxDec_Next;  //if > 60  max dec
+
+    if(fdistocur>=dis_maxdec)return decmax;
+    else
+    {
+        if(fdistocur<=dis_startdec)return 0.0;
+        else
+        {
+            assert(dis_maxdec > dis_startdec);
+            double fratio = (fdistocur - dis_startdec)/(dis_maxdec - dis_startdec);
+            double fdec = fratio * decmax;
+            return fdec;
+        }
+    }
+
+    return 0.0;
+
+
+
+
+
+}
+
+
+double GroupSpeedCtrl::Getmindec_front(iv::group::groupinfo & xgroupinfo,
+                                 const std::vector<GPSData> & gpsMapLine,int PathPoint)
+{
+
+    iv::group::vehicleinfo * pvehbefore = NULL;  //a vehicle after current vehicle
+    int i;
+    for(i=0;i<xgroupinfo.mvehinfo_size();i++)
+    {
+        if((xgroupinfo.mutable_mvehinfo(i)->intragroupid()+1) == ServiceCarStatus.curID)
+        {
+            pvehbefore = xgroupinfo.mutable_mvehinfo(i);
+            break;
+        }
+    }
+
+    if(pvehbefore == NULL)
+    {
+        return 0.0;
+    }
+
+    if(gpsMapLine.size()<1)
+    {
+        return 0.0;
+    }
+
+    if(!pvehbefore->has_mgpsimu())
+    {
+        return 0.0;
+    }
+
+    double x,y;
+    GaussProjCal(pvehbefore->mgpsimu().lon(),pvehbefore->mgpsimu().lat(),&x,&y);
+
+    int nearpoint = -1;
+    double neardis = 100000;
+
+    double nearthresh = 100;
+    int nsize = gpsMapLine.size();
+    for(i=0;i<nsize;i++)
+    {
+        if((fabs(x - gpsMapLine[i]->gps_x)<nearthresh) || (fabs(y - gpsMapLine[i]->gps_y)<nearthresh))
+        {
+            double fdis = sqrt(pow(x - gpsMapLine[i]->gps_x,2)+pow(y - gpsMapLine[i]->gps_y,2));
+            double fanglediff = pvehbefore->mgpsimu().heading() - gpsMapLine[i]->ins_heading_angle;
+            while(fanglediff<-180)fanglediff = fanglediff + 360.0;
+            while(fanglediff>180)fanglediff = fanglediff - 360.0;
+            if((fdis<nearthresh) &&(fabs(fanglediff)<90))
+            {
+                if(fdis<neardis)
+                {
+                    neardis = fdis;
+                    nearpoint = i;
+                }
+            }
+        }
+    }
+
+    if(nearpoint <0)
+    {
+        return 0.0;
+    }
+
+
+    double fdistocur = 0;  //after vehicle to now vehicle
+
+    if(PathPoint > nearpoint)  // now after the after vehicle
+    {
+        return 0.0;
+    }
+
+    for(i=(PathPoint+1);i<nearpoint;i++)
+    {
+        double fdistolast = iv::decition::GetDistance(*gpsMapLine[i],*gpsMapLine[i-1]);
+        fdistocur = fdistocur + fdistolast;
+    }
+
+    double decmax = mfDecMax;
+    double dis_startdec = mfDis_StartDec_Front;  //if > 50 start wait
+    double dis_maxdec = mfDis_MaxDec_Front;  //if > 60  max dec
+
+    if(fdistocur>=dis_startdec)return 0.0;
+    else
+    {
+        if(fdistocur<=dis_maxdec)return decmax;
+        else
+        {
+            assert(dis_maxdec < dis_startdec);
+            double fratio = ( dis_startdec - fdistocur)/(dis_startdec -dis_maxdec  );
+            double fdec = fratio * decmax;
+            return fdec;
+        }
+    }
+
+    return 0.0;
+
+
+
+
+
+}
+
+
+
+void GroupSpeedCtrl::SetDecMax(double fDecMax)
+{
+    mfDecMax = fDecMax;
+}
+
+void GroupSpeedCtrl::SetDisStartDec_Next(double fDis_StartDec)
+{
+    mfDis_StartDec_Next = fDis_StartDec;
+}
+
+void GroupSpeedCtrl::SetDisMaxDec_Next(double fDis_MaxDec)
+{
+    mfDis_MaxDec_Next = fDis_MaxDec;
+}
+
+void GroupSpeedCtrl::SetDisStartDec_Front(double fDis_StartDec)
+{
+    mfDis_StartDec_Front = fDis_StartDec;
+}
+
+void GroupSpeedCtrl::SetDisMaxDec_Front(double fDis_MaxDec)
+{
+    mfDis_MaxDec_Front = fDis_MaxDec;
+}

src/decition/decition_brain_sf_Jeely_xingyueL/decition/groupspeedctrl.h

@@ -0,0 +1,38 @@
+#ifndef GROUPSPEEDCTRL_H
+#define GROUPSPEEDCTRL_H
+
+#include "groupmsg.pb.h"
+
+#include "common/gps_type.h"
+
+using namespace iv;
+
+class GroupSpeedCtrl
+{
+public:
+    GroupSpeedCtrl();
+
+    void SetDecMax(double fDecMax);
+    void SetDisStartDec_Next(double fDis_StartDec);
+    void SetDisMaxDec_Next(double fDis_MaxDec);
+    void SetDisStartDec_Front(double fDis_StartDec);
+    void SetDisMaxDec_Front(double fDis_MaxDec);
+
+public:
+    double Getmindec(iv::group::groupinfo & xgroupinfo,
+                     const std::vector<GPSData> & gpsMapLine,int PathPoint);
+
+    double  Getmindec_front(iv::group::groupinfo & xgroupinfo,
+                            const std::vector<GPSData> & gpsMapLine,int PathPoint);
+
+
+private:
+    double mfDecMax = -1.0;
+    double mfDis_StartDec_Next = 60.0; //when next vehicle to current more than distance, start dec
+    double mfDis_MaxDec_Next = 80.0 ; //When next vehicle to current more than distance, max dec
+
+    double mfDis_StartDec_Front = 50.0; //when front vehicle to current less than distance, start dec
+    double mfDis_MaxDec_Front = 30.0 ; //When front vehicle to current less than distance, max dec
+};
+
+#endif // GROUPSPEEDCTRL_H