|
@@ -233,7 +233,133 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
|
|
|
doubledata[j][3]=delta_sum/20;
|
|
|
}
|
|
|
}
|
|
|
+namespace iv {
|
|
|
+namespace decition {
|
|
|
+extern double gplanbrakeacc;
|
|
|
+extern double gplanacc;
|
|
|
+}
|
|
|
+}
|
|
|
+//extern double iv::decition::gplanbrakeacc;
|
|
|
+
|
|
|
+void iv::decition::Compute00::SmoothACC(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata)
|
|
|
+{
|
|
|
+ double acca = iv::decition::gplanacc;
|
|
|
+ int nsize = static_cast<int>(vdoubledata.size());
|
|
|
+ int i;
|
|
|
+ for(i=1;i<(nsize-1);i++)
|
|
|
+ {
|
|
|
+ if((vdoubledata[i+1][4] - vdoubledata[i][4])>1.0)
|
|
|
+ {
|
|
|
+ int j;
|
|
|
+ for(j=i;j<(nsize -1);j++)
|
|
|
+ {
|
|
|
+ double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
|
|
|
+ double vpre = vdoubledata[j][4]/3.6;
|
|
|
+ double fspeed = sqrt((fdis + 0.5*vpre*vpre/acca)*acca/0.5) *3.6;
|
|
|
+ if(fspeed>vdoubledata[j+1][4])
|
|
|
+ {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ vdoubledata[j+1][4] = fspeed;
|
|
|
+ }
|
|
|
+ i = j+1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void iv::decition::Compute00::SmoothHighData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i)
|
|
|
+{
|
|
|
+ if(i==0)return;
|
|
|
+
|
|
|
+// 0.5*v1*v1/a - 0.5*v2*v2/a = dis;
|
|
|
+// v1*v1 = (dis + 0.5*v2*v2/a)*a/0.5
|
|
|
+
|
|
|
+ int j;
|
|
|
+ double brakea = 3.0*iv::decition::gplanbrakeacc;//iv::decition::gplanbrakeacc;
|
|
|
+ if(brakea>5.0)brakea = 5.0;
|
|
|
+ int startvalue = static_cast<int>(i-1);
|
|
|
+
|
|
|
+ double fdispoint = sqrt(pow(MapTrace[i]->gps_x - MapTrace[i-1]->gps_x,2)+pow(MapTrace[i]->gps_y - MapTrace[i-1]->gps_y,2));
|
|
|
+ if(fdispoint < 0.1)fdispoint = 0.1;
|
|
|
+ int nprebrake = static_cast<int>(10.0/fdispoint);
|
|
|
+ if((MapTrace[i]->roadMode == 14) || (MapTrace[i]->roadMode == 15))
|
|
|
+ {
|
|
|
+ nprebrake = static_cast<int>(5.0/fdispoint);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ nprebrake = static_cast<int>(3.0/fdispoint);
|
|
|
+ }
|
|
|
+ for(j=startvalue;j>=0;j--)
|
|
|
+ {
|
|
|
+ if(vdoubledata[j][4] < vdoubledata[j+1][4])
|
|
|
+ {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ if((startvalue - j) < nprebrake)
|
|
|
+ {
|
|
|
+ vdoubledata[j][4] = vdoubledata[j+1][4];
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
|
|
|
+ double vpre = vdoubledata[j+1][4]/3.6;
|
|
|
+ double fspeed = sqrt((fdis + 0.5*vpre*vpre/brakea)*brakea/0.5) *3.6;
|
|
|
+ if(fspeed>vdoubledata[j][4])
|
|
|
+ {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ vdoubledata[j][4] = fspeed;
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void iv::decition::Compute00::SmoothData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i)
|
|
|
+{
|
|
|
+ if(i==0)return;
|
|
|
+
|
|
|
+// 0.5*v1*v1/a - 0.5*v2*v2/a = dis;
|
|
|
+// v1*v1 = (dis + 0.5*v2*v2/a)*a/0.5
|
|
|
+
|
|
|
+ int j;
|
|
|
+ double brakea = iv::decition::gplanbrakeacc;//iv::decition::gplanbrakeacc;
|
|
|
+ int startvalue = static_cast<int>(i-1);
|
|
|
+
|
|
|
+ double fdispoint = sqrt(pow(MapTrace[i]->gps_x - MapTrace[i-1]->gps_x,2)+pow(MapTrace[i]->gps_y - MapTrace[i-1]->gps_y,2));
|
|
|
+ if(fdispoint < 0.1)fdispoint = 0.1;
|
|
|
+ int nprebrake = static_cast<int>(10.0/fdispoint);
|
|
|
+ if((MapTrace[i]->roadMode == 14) || (MapTrace[i]->roadMode == 15))
|
|
|
+ {
|
|
|
+ nprebrake = static_cast<int>(5.0/fdispoint);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ nprebrake = static_cast<int>(3.0/fdispoint);
|
|
|
+ }
|
|
|
+ for(j=startvalue;j>=0;j--)
|
|
|
+ {
|
|
|
+ if(vdoubledata[j][4] < vdoubledata[j+1][4])
|
|
|
+ {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ if((startvalue - j) < nprebrake)
|
|
|
+ {
|
|
|
+ vdoubledata[j][4] = vdoubledata[j+1][4];
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
|
|
|
+ double vpre = vdoubledata[j+1][4]/3.6;
|
|
|
+ double fspeed = sqrt((fdis + 0.5*vpre*vpre/brakea)*brakea/0.5) *3.6;
|
|
|
+ if(fspeed>vdoubledata[j][4])
|
|
|
+ {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ vdoubledata[j][4] = fspeed;
|
|
|
+ }
|
|
|
|
|
|
+}
|
|
|
|
|
|
/*int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
|
|
|
{
|
|
@@ -358,6 +484,7 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
|
|
|
}*/
|
|
|
int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
|
|
|
{
|
|
|
+ int nSmoothMode = 1;
|
|
|
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);
|
|
@@ -366,14 +493,156 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
|
|
|
getMapDelta(MapTrace);
|
|
|
for(int i=0;i<doubledata.size();i++)
|
|
|
{
|
|
|
- if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
|
|
|
- MapTrace[i]->roadMode=5;
|
|
|
- }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
|
|
|
- MapTrace[i]->roadMode=18;
|
|
|
- }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
|
|
|
- MapTrace[i]->roadMode=14;
|
|
|
+
|
|
|
+ if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15))
|
|
|
+ {
|
|
|
+ int a = 1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
|
|
|
+ MapTrace[i]->roadMode=5;
|
|
|
+ }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
|
|
|
+ MapTrace[i]->roadMode=18;
|
|
|
+ }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
|
|
|
+ MapTrace[i]->roadMode=14;
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
+ for(int i=1;i<MapTrace.size()-1;i++)
|
|
|
+ {
|
|
|
+ if((MapTrace[i-1]->roadMode==5)&&(MapTrace[i+1]->roadMode==5))
|
|
|
+ {
|
|
|
+ if(MapTrace[i]->roadMode!=5)
|
|
|
+ MapTrace[i]->roadMode=5;
|
|
|
+
|
|
|
+ }
|
|
|
+ else if ((MapTrace[i-1]->roadMode==18)&&(MapTrace[i+1]->roadMode==18))
|
|
|
+ {
|
|
|
+ if(MapTrace[i]->roadMode==5)
|
|
|
+ MapTrace[i]->roadMode=18;
|
|
|
+ if(MapTrace[i]->roadMode==14)
|
|
|
+ MapTrace[i]->roadMode=18;
|
|
|
+ }
|
|
|
+ else if ((MapTrace[i-1]->roadMode==14)&&(MapTrace[i+1]->roadMode==14))
|
|
|
+ {
|
|
|
+ if(MapTrace[i]->roadMode==5)
|
|
|
+ MapTrace[i]->roadMode=14;
|
|
|
+ if(MapTrace[i]->roadMode==18)
|
|
|
+ MapTrace[i]->roadMode=14;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(nSmoothMode == 0)
|
|
|
+ {
|
|
|
+
|
|
|
+ for(int i=0;i<MapTrace.size();i++)
|
|
|
+ {
|
|
|
+ if(MapTrace[i]->roadMode==0){
|
|
|
+ doubledata[i][4]=straightSpeed;
|
|
|
+ if(MapTrace[i]->speed>0)
|
|
|
+ {
|
|
|
+ // double fSpeed = MapTrace[i]->speed;
|
|
|
+ doubledata[i][4] = MapTrace[i]->speed*3.6;
|
|
|
+ }
|
|
|
+ mode0to5count++;
|
|
|
+ //mode0to18count++;
|
|
|
+ mode18count=0;
|
|
|
+ mode0to5flash=mode0to5count;
|
|
|
+ }else if(MapTrace[i]->roadMode==5){
|
|
|
+ doubledata[i][4]=straightCurveSpeed;
|
|
|
+ if(MapTrace[i]->speed>0)
|
|
|
+ {
|
|
|
+ // double fSpeed = MapTrace[i]->speed;
|
|
|
+ doubledata[i][4] = MapTrace[i]->speed*3.6;
|
|
|
+ }
|
|
|
+ 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))*5/0.6; //1228
|
|
|
+ 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){
|
|
|
+ double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;//1228
|
|
|
+ for(int j=i;j>=i-mode0to5countSum;j--){
|
|
|
+ doubledata[j][4]=Curve_SmallSpeed;//min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);//1228Curve_SmallSpeed;
|
|
|
+ step_count++;
|
|
|
+ }
|
|
|
+ }else{
|
|
|
+ doubledata[i][4]=Curve_SmallSpeed;
|
|
|
+ }
|
|
|
+ mode0to5count=0;
|
|
|
+ //mode0to18count++;
|
|
|
+ mode18count++;
|
|
|
+ mode18flash=mode18count;
|
|
|
+ }else if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15)){
|
|
|
+ mode0to18countSum=mode0to5flash+mode18flash;
|
|
|
+ mode18countSum=mode18count;
|
|
|
+ double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*5/0.6; //1228
|
|
|
+ double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*5/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--){
|
|
|
+ if(MapTrace[j]->roadMode==18)//20230106,fangzhi 18moshi de sudu dayu yuzhi
|
|
|
+ continue;
|
|
|
+ doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
|
|
|
+ step_count++;
|
|
|
+ }
|
|
|
+ }else if(mode0to18countSum>0){
|
|
|
+ double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;//1228
|
|
|
+ for(int j=i;j>=i-mode0to18countSum;j--){
|
|
|
+ doubledata[j][4]=Curve_LargeSpeed;//min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);//1228Curve_LargeSpeed;
|
|
|
+ step_count++;
|
|
|
+ }
|
|
|
+ }else{
|
|
|
+ doubledata[i][4]=Curve_LargeSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+ mode0to5count=0;
|
|
|
+ mode18count=0;
|
|
|
+ mode0to5flash=0;
|
|
|
+ mode18flash=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
for(int i=0;i<MapTrace.size();i++)
|
|
|
{
|
|
|
if(MapTrace[i]->roadMode==0){
|
|
@@ -457,6 +726,7 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
|
|
|
mode18flash=0;
|
|
|
}
|
|
|
}
|
|
|
+ }
|
|
|
//
|
|
|
/*for(int i=0;i<MapTrace.size();i++){
|
|
|
//将数据写入文件开始
|