add speed control

autodeploy.sh

@@ -29,12 +29,14 @@ tool_querymsg
 detection_chassis
 ui_ads_hmi
 decition_brain
+decition_brain_sf
 #decition_brain_ge3
 controller_midcar
 driver_map_xodrload
 tool_xodrobj
 ivlog_record
 adciv_record
+adciv_replay
 )
 
 for x in ${app_name[@]}

src/decition/decition_brain_sf/decition/adc_tools/compute_00.cpp

@@ -189,7 +189,153 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
 }
 
 
+int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
+    double distance,deltaphi;
+    //x,y,phi in rad
+    for (int i = 0; i < MapTrace.size(); i++) {   //                                       1225/14:25
+                 std::vector<double> temp;
+                 temp.push_back(0);temp.push_back(0);temp.push_back(0);temp.push_back(0);temp.push_back(0);//先push四个空量，然后就可以给量赋值了
+                 doubledata.push_back(temp);
+                 doubledata[i][0] = MapTrace.at(i)->gps_x;
+                 doubledata[i][1] = MapTrace.at(i)->gps_y;
+                 doubledata[i][2] = (MapTrace.at(i)->ins_heading_angle) / 180 * PI;
+    }
+    // compute delta///////////////////////////////////////////////////////////////
+    for (int i = 0; i < doubledata.size()-1; i++)
+    {
+                deltaphi=doubledata[i+1][2]-doubledata[i][2];
+                if (deltaphi>PI)
+                        {deltaphi=deltaphi-2*PI; }
+                if (deltaphi<-PI)
+                        {deltaphi=deltaphi+2*PI;}
+                distance=sqrt( pow((doubledata[i+1][0]-doubledata[i][0]),2)+pow((doubledata[i+1][1]-doubledata[i][1]),2));
+                doubledata[i][3]=-atan( 4.0* (deltaphi) / distance  );//车辆坐标系和惯导坐标系方向相反，delta变号
+    }
+                doubledata[doubledata.size()-1][3] =  doubledata[doubledata.size()-2][3];
+
+    //delta filter
+    for(int j=10;j<doubledata.size()-10;j++)
+    {
+        double delta_sum=0;
+        for(int k=j-10;k<j+10;k++)
+        {
+            delta_sum+= doubledata[k][3];
+        }
+        doubledata[j][3]=delta_sum/20;
+    }
+}
+
+
+int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
+{
+                int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
+                double straightSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
+                double straightCurveSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
+                double Curve_SmallSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode18,15.0);
+                double Curve_LargeSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode14,8.0);
+                getMapDelta(MapTrace);
+                for(int i=0;i<doubledata.size();i++)
+                {
+                    if((doubledata[i][3]>-0.4)&&(doubledata[i][3]<0.4)){
+                        MapTrace[i]->roadMode=0;
+                    }else if(((doubledata[i][3]>0.4)&&(doubledata[i][3]<4))||((doubledata[i][3]>-4)&&(doubledata[i][3]<-0.4))){
+                        MapTrace[i]->roadMode=5;
+                    }else if(((doubledata[i][3]>4)&&(doubledata[i][3]<10))||((doubledata[i][3]>-10)&&(doubledata[i][3]<-4))){
+                        MapTrace[i]->roadMode=18;
+                    }else if(((doubledata[i][3]>10))||((doubledata[i][3]<-10))){
+                        MapTrace[i]->roadMode=14;
+                    }
+                }
+                for(int i=0;i<MapTrace.size();i++)
+                {
+                    if(MapTrace[i]->roadMode==0){
+                        doubledata[i][4]=straightSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
+                    }else if(MapTrace[i]->roadMode==5){
+                        doubledata[i][4]=straightCurveSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
+                    }else if(MapTrace[i]->roadMode==18){
+                        mode0to5countSum=mode0to5count;
+                        doubledata[i][4]=Curve_SmallSpeed;
+
+                        double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*10/0.6;
+                        int brake_distance=(int)brake_distance_double;
+                        int step_count=0;
+                        if((i>brake_distance)&&(mode0to5countSum>brake_distance))
+                        {
+                            double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+
+                            for(int j=i;j>i-brake_distance;j--){
+                                doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
+                                step_count++;
+                            }
+                        }else if(mode0to5countSum>0){
+                            for(int j=i;j>i-mode0to5countSum;j--){
+                                doubledata[j][4]=Curve_SmallSpeed;
+                                step_count++;
+                            }
+                        }else{
+                            doubledata[i][4]=Curve_SmallSpeed;
+                        }
+                        mode0to5count=0;
+                        //mode0to18count++;
+                        mode18count++;
+                        mode18flash=mode18count;
+                    }else if(MapTrace[i]->roadMode==14){
+                        mode0to18countSum=mode0to5flash+mode18flash;
+                        mode18countSum=mode18count;
+                        double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        int brake_distanceLarge=(int)brake_distanceLarge_double;
+                        int brake_distance0to18=(int)brake_distance0to18_double;
+                        int step_count=0;
+                        doubledata[i][4]=Curve_LargeSpeed;
+
+                        if(mode18countSum>brake_distanceLarge)
+                        {
+                                double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+
+                                for(int j=i;j>i-brake_distanceLarge;j--){
+                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
+                                    step_count++;
+                                }
+
+                        }else if(mode0to18countSum>brake_distance0to18){
+
+                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
+
+                                for(int j=i;j>i-brake_distance0to18;j--){
+                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+                                    step_count++;
+                                }
+                        }else{
+                                doubledata[i][4]=Curve_LargeSpeed;
+                        }
+
+                        mode0to5count=0;
+                        mode18count=0;
+                        mode0to5flash=0;
+                        mode18flash=0;
+                    }
+                }
+
+                for(int i=0;i<MapTrace.size();i++){
+                    //将数据写入文件开始
+                    ofstream outfile;
+                    outfile.open("ctr0108003.txt", ostream::app);        /*以添加模式打开文件*/
+                    outfile<<"Delta"<< ","  <<doubledata[i][3]<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
+                           <<"plan_speed"<< ","  << doubledata[i][4]<< ","<<endl;
+                    outfile.close();                                 /*关闭文件*/
+                    //将数据写入文件结束
+                }
 
+}
 
 
 //首次找点

src/decition/decition_brain_sf/decition/adc_tools/compute_00.h

@@ -35,6 +35,9 @@ namespace iv {
             static	int getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
 
             static int getDesiredSpeed(std::vector<GPSData> gpsMap);
+            static int getMapDelta(std::vector<GPSData> MapTrace);
+            static int getPlanSpeed(std::vector<GPSData> MapTrace);
+
 
             static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
             static double getAveDef(std::vector<Point2D> farTrace);

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -165,6 +165,8 @@ bool startingFlag = true;  //起步标志，在起步时需要进行frenet规划
 bool useFrenet = false;    //标志是否启用frenet算法避障
 bool useOldAvoid = true;   //标志是否用老方法避障
 
+std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，速度规划用变量
+
 enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
                 waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
                 dRever,dRever0,dRever1,dRever2,dRever3,dRever4} vehState;

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -248,6 +248,7 @@ extern double steerSpeed;
 extern bool transferPieriod;
 extern bool transferPieriod2;
 extern double traceDevi;
+extern std::vector<std::vector<double>> doubledata;
 #endif // !  _IV_DECITION__DECIDE_GPS_00_