|
|
@@ -0,0 +1,3015 @@
|
|
|
+#include <decition/adc_tools/compute_00.h>
|
|
|
+#include <decition/decide_gps_00.h>
|
|
|
+#include <decition/adc_tools/gps_distance.h>
|
|
|
+#include <decition/decition_type.h>
|
|
|
+#include <decition/adc_tools/transfer.h>
|
|
|
+#include <common/constants.h>
|
|
|
+#include <math.h>
|
|
|
+#include <iostream>
|
|
|
+#include <fstream>
|
|
|
+#include <control/can.h>
|
|
|
+
|
|
|
+#include "common/tracepointstation.h"
|
|
|
+
|
|
|
+#include "ivlog.h"
|
|
|
+extern iv::Ivlog * givlog;
|
|
|
+using namespace std;
|
|
|
+
|
|
|
+#define PI (3.1415926535897932384626433832795)
|
|
|
+
|
|
|
+iv::decition::Compute00::Compute00() {
|
|
|
+
|
|
|
+}
|
|
|
+iv::decition::Compute00::~Compute00() {
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::angleLimit = 700;
|
|
|
+double iv::decition::Compute00::lastEA = 0;
|
|
|
+double iv::decition::Compute00::lastEP = 0;
|
|
|
+double iv::decition::Compute00::decision_Angle = 0;
|
|
|
+double iv::decition::Compute00::lastAng = 0;
|
|
|
+int iv::decition::Compute00::lastEsrID = -1;
|
|
|
+int iv::decition::Compute00::lastEsrCount = 0;
|
|
|
+int iv::decition::Compute00::lastEsrIDAvoid = -1;
|
|
|
+int iv::decition::Compute00::lastEsrCountAvoid = 0;
|
|
|
+
|
|
|
+iv::GPS_INS iv::decition::Compute00::nearTpoint;
|
|
|
+iv::GPS_INS iv::decition::Compute00::farTpoint;
|
|
|
+double iv::decition::Compute00::bocheAngle;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+iv::GPS_INS iv::decition::Compute00::dTpoint0;
|
|
|
+iv::GPS_INS iv::decition::Compute00::dTpoint1;
|
|
|
+iv::GPS_INS iv::decition::Compute00::dTpoint2;
|
|
|
+iv::GPS_INS iv::decition::Compute00::dTpoint3;
|
|
|
+double iv::decition::Compute00::dBocheAngle;
|
|
|
+
|
|
|
+
|
|
|
+std::vector<int> lastEsrIdVector;
|
|
|
+std::vector<int> lastEsrCountVector;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
|
|
|
+{
|
|
|
+ int startIndex = 0; // startIndex = 0 则每一次都是遍历整条地图路线
|
|
|
+ int endIndex = gpsMap.size() - 1;
|
|
|
+ static double FrontTotalFive=0,FrontAveFive=0,BackAveFive=0,BackTotalFive=0;
|
|
|
+ static int FrontCount=0,BackCount=0;
|
|
|
+ static int CurrentIndex=0,MarkingIndex=0,MarkingCount=0;
|
|
|
+ int MarkedIndex=0,CurveContinue=0;
|
|
|
+
|
|
|
+ for (int j = startIndex; j < endIndex; j++)
|
|
|
+ {
|
|
|
+ int i = (j + gpsMap.size()) % gpsMap.size();
|
|
|
+ if((*gpsMap[i]).roadMode!=6)
|
|
|
+ (*gpsMap[i]).roadMode=5;
|
|
|
+ }
|
|
|
+ for (int j = startIndex; j < (endIndex-40); j+=40)
|
|
|
+ {
|
|
|
+ int i = (j + gpsMap.size()) % gpsMap.size();
|
|
|
+ for(int front_k=i;front_k<i+20;front_k++)
|
|
|
+ {
|
|
|
+ if(fabs(((*gpsMap[front_k]).ins_heading_angle)-((*gpsMap[i]).ins_heading_angle))<10)
|
|
|
+ {
|
|
|
+ FrontTotalFive+=(*gpsMap[front_k]).ins_heading_angle;
|
|
|
+ FrontCount++;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ i+=20;
|
|
|
+ FrontAveFive=FrontTotalFive/FrontCount;
|
|
|
+ FrontTotalFive=0;
|
|
|
+ FrontCount=0;
|
|
|
+ for(int back_k=i;back_k<i+20;back_k++)
|
|
|
+ {
|
|
|
+ if(fabs((*gpsMap[back_k]).ins_heading_angle-(*gpsMap[i]).ins_heading_angle)<10)
|
|
|
+ {
|
|
|
+ BackTotalFive+=(*gpsMap[back_k]).ins_heading_angle;
|
|
|
+ BackCount++;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ i+=20;
|
|
|
+ CurrentIndex=i-1;
|
|
|
+ BackAveFive=BackTotalFive/BackCount;
|
|
|
+ BackTotalFive=0;
|
|
|
+ BackCount=0;
|
|
|
+ if(fabs(FrontAveFive-BackAveFive)<50)
|
|
|
+ {
|
|
|
+ if(fabs(FrontAveFive-BackAveFive)>4)
|
|
|
+ {
|
|
|
+ CurveContinue+=1;
|
|
|
+ if(CurveContinue>=1)
|
|
|
+ {
|
|
|
+ MarkingCount=0;
|
|
|
+ for(MarkingIndex=CurrentIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
|
|
|
+ {
|
|
|
+ if((*gpsMap[MarkingIndex]).roadMode!=6)
|
|
|
+ {
|
|
|
+ if(MarkingCount<150)
|
|
|
+ {
|
|
|
+ if((BackAveFive-FrontAveFive)<=4.0)
|
|
|
+ {
|
|
|
+ (*gpsMap[MarkingIndex]).roadMode=14; //弯道，2米,14
|
|
|
+ }
|
|
|
+ else if((BackAveFive-FrontAveFive)>4.0)
|
|
|
+ {
|
|
|
+ (*gpsMap[MarkingIndex]).roadMode=15;
|
|
|
+ }
|
|
|
+
|
|
|
+ } //else if((MarkingCount>=100)&&(MarkingCount<150)){(*gpsMap[MarkingIndex]).roadMode=18; //超低速，10米,1}
|
|
|
+ else if((MarkingCount>=150)&&(MarkingCount<320))
|
|
|
+ {
|
|
|
+ (*gpsMap[MarkingIndex]).roadMode=5; //低速，20米,5
|
|
|
+ }
|
|
|
+ else if((MarkingCount>=320)&&(MarkingCount<620))
|
|
|
+ {
|
|
|
+ (*gpsMap[MarkingIndex]).roadMode=0; //常速，60米
|
|
|
+ }
|
|
|
+ else if(MarkingCount>=620)
|
|
|
+ {
|
|
|
+ (*gpsMap[MarkingIndex]).roadMode=11; //高速/疯狂加速，大于60米
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ MarkingCount++;
|
|
|
+ }
|
|
|
+ MarkedIndex=CurrentIndex;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(fabs(FrontAveFive-BackAveFive)<=4)
|
|
|
+ {
|
|
|
+ CurveContinue=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ FrontAveFive=0;
|
|
|
+ BackAveFive=0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(MarkedIndex<endIndex)
|
|
|
+ {
|
|
|
+ MarkingCount=0;
|
|
|
+ for(MarkingIndex=endIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
|
|
|
+ {
|
|
|
+ if((*gpsMap[MarkingIndex]).roadMode!=6)
|
|
|
+ {
|
|
|
+ if(MarkingCount<100)
|
|
|
+ {
|
|
|
+ if((BackAveFive-FrontAveFive)<3)
|
|
|
+ {
|
|
|
+ (*gpsMap[MarkingIndex]).roadMode=14; //弯道，2米,14
|
|
|
+ }
|
|
|
+ else if((BackAveFive-FrontAveFive)>3)
|
|
|
+ {
|
|
|
+ (*gpsMap[MarkingIndex]).roadMode=15;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if((MarkingCount>=100)&&(MarkingCount<150))
|
|
|
+ {
|
|
|
+ (*gpsMap[MarkingIndex]).roadMode=18; //超低速，10米
|
|
|
+ }
|
|
|
+ else if((MarkingCount>=150)&&(MarkingCount<320))
|
|
|
+ {
|
|
|
+ (*gpsMap[MarkingIndex]).roadMode=5; //低速，30米
|
|
|
+ }
|
|
|
+ else if((MarkingCount>=320)&&(MarkingCount<620))
|
|
|
+ {
|
|
|
+ (*gpsMap[MarkingIndex]).roadMode=0; //常速，60米
|
|
|
+ }
|
|
|
+ else if(MarkingCount>=620)
|
|
|
+ {
|
|
|
+ (*gpsMap[MarkingIndex]).roadMode=11; //高速/疯狂加速，大于60米
|
|
|
+ }
|
|
|
+ }
|
|
|
+ MarkingCount++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ return 1;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
|
|
|
+ double distance,deltaphi;
|
|
|
+ //x,y,phi in rad
|
|
|
+ doubledata.clear();
|
|
|
+ for (int i = 0; i < MapTrace.size(); i++) { // 1225/14:25
|
|
|
+ std::vector<double> temp;
|
|
|
+ temp.clear();
|
|
|
+ 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;
|
|
|
+ doubledata[i][3]=0;
|
|
|
+ doubledata[i][4]=0;
|
|
|
+ }
|
|
|
+ // compute delta///////////////////////////////////////////////////////////////
|
|
|
+ for (int i = 0; i < doubledata.size()-10; i++)
|
|
|
+ {
|
|
|
+ deltaphi=doubledata[i+10][2]-doubledata[i][2];
|
|
|
+ if (deltaphi>PI)
|
|
|
+ {deltaphi=deltaphi-2*PI; }
|
|
|
+ if (deltaphi<-PI)
|
|
|
+ {deltaphi=deltaphi+2*PI;}
|
|
|
+ distance=sqrt( pow((doubledata[i+10][0]-doubledata[i][0]),2)+pow((doubledata[i+10][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;
|
|
|
+ }
|
|
|
+
|
|
|
+ //2022,1227,begin,通过s判定各模式对应的距离是否满足刹车距离
|
|
|
+// ///计算全局地图S
|
|
|
+// bool flag=false;//保证S逐渐增加
|
|
|
+// if(fabs((*MapTrace[MapTrace.size()-1]).frenet_s)<1.0e-6){
|
|
|
+// double sum_s=0.0;
|
|
|
+// for(int i=0;i<MapTrace.size();i++)
|
|
|
+// {
|
|
|
+// if ((i==0)&&(flag==false))
|
|
|
+// {
|
|
|
+// sum_s=0.0;
|
|
|
+// (*MapTrace[i]).frenet_s=0;
|
|
|
+// flag=true;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+
|
|
|
+// sum_s = sum_s+sqrt(pow(((*MapTrace[i]).gps_x-(*MapTrace[i-1]).gps_x),2) + pow(((*MapTrace[i]).gps_y-(*MapTrace[i-1]).gps_y),2));
|
|
|
+// (*MapTrace[i]).frenet_s=sum_s;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// for(int i=0;i<MapTrace.size();i++){
|
|
|
+// //将数据写入文件开始
|
|
|
+// ofstream outfile;
|
|
|
+// outfile.open("log.txt", ostream::app);
|
|
|
+// outfile<<"gps_x"<< "," <<MapTrace[i]->gps_x<<"," <<"gps_y"<< "," <<MapTrace[i]->gps_y <<","<<"roadMode"<< "," <<MapTrace[i]->roadMode<< ","
|
|
|
+// <<"plan_speed"<< "," << doubledata[i][4]<< ","<<"num,curve"<< "," <<i<< ","
|
|
|
+// <<MapTrace[i]->mfCurvature<<","<<"frenet_s"<< "," <<MapTrace[i]->frenet_s<<endl;
|
|
|
+// outfile.close();
|
|
|
+// //将数据写入文件结束
|
|
|
+// }
|
|
|
+ //2022,1227,end
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+/*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<doubledata.size();i++)
|
|
|
+ {
|
|
|
+ if((doubledata[i][3]>-0.26)&&(doubledata[i][3]<0.26)){
|
|
|
+ MapTrace[i]->roadMode=5;
|
|
|
+ }else if(((doubledata[i][3]>=0.26)&&(doubledata[i][3]<=1.0))||((doubledata[i][3]>=-1.0)&&(doubledata[i][3]<=-0.26))){
|
|
|
+ MapTrace[i]->roadMode=18;
|
|
|
+ }else if(((doubledata[i][3]>1.0))||((doubledata[i][3]<-1.0))){
|
|
|
+ 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]<< ","<<"num,curve"<< "," <<i<< ","
|
|
|
+ <<MapTrace[i]->mfCurvature<<endl;
|
|
|
+ outfile.close();
|
|
|
+ //将数据写入文件结束
|
|
|
+ }
|
|
|
+
|
|
|
+}*/
|
|
|
+//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((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=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 if(mode0to18countSum>0){
|
|
|
+//// for(int j=i;j>=i-mode0to18countSum;j--){
|
|
|
+//// doubledata[j][4]=Curve_LargeSpeed;
|
|
|
+//// step_count++;
|
|
|
+//// }
|
|
|
+//// }else{
|
|
|
+//// doubledata[i][4]=Curve_LargeSpeed;
|
|
|
+//// }
|
|
|
+
|
|
|
+//// mode0to5count=0;
|
|
|
+//// mode18count=0;
|
|
|
+//// mode0to5flash=0;
|
|
|
+//// mode18flash=0;
|
|
|
+//// }
|
|
|
+//// }
|
|
|
+//// //1227，begin
|
|
|
+// double dis_mode0to5countSum_frenet_s = 0.0;
|
|
|
+// double mode0_start_frenet_s=0.0;
|
|
|
+// double mode18_start_frenet_s=0.0;
|
|
|
+// double mode18countSum_frenet_s=0.0;
|
|
|
+// double mode0to18countSum_frenet_s=0.0;
|
|
|
+
|
|
|
+// for(int i=0;i<MapTrace.size();i++)
|
|
|
+// {
|
|
|
+// if(MapTrace[i]->roadMode==0)
|
|
|
+// {
|
|
|
+// doubledata[i][4]=straightSpeed;
|
|
|
+// if(mode0to5count==0)
|
|
|
+// {
|
|
|
+// mode0_start_frenet_s =MapTrace[i]->frenet_s;
|
|
|
+// }
|
|
|
+// mode0to5count++;
|
|
|
+// //mode0to18count++;
|
|
|
+// mode18count=0;
|
|
|
+// mode0to5flash=mode0to5count;
|
|
|
+// }
|
|
|
+// else if(MapTrace[i]->roadMode==5)
|
|
|
+// {
|
|
|
+// doubledata[i][4]=straightCurveSpeed;
|
|
|
+// if(mode0to5count==0)
|
|
|
+// {
|
|
|
+// mode0_start_frenet_s =MapTrace[i]->frenet_s;
|
|
|
+// }
|
|
|
+// mode0to5count++;
|
|
|
+// //mode0to18count++;
|
|
|
+// mode18count=0;
|
|
|
+// mode0to5flash=mode0to5count;
|
|
|
+// }
|
|
|
+// else if(MapTrace[i]->roadMode==18)
|
|
|
+// {
|
|
|
+// mode0to5countSum=mode0to5count;
|
|
|
+// doubledata[i][4]=Curve_SmallSpeed;
|
|
|
+// if(mode18count==0)
|
|
|
+// {
|
|
|
+// mode18_start_frenet_s=MapTrace[i]->frenet_s;
|
|
|
+// }
|
|
|
+
|
|
|
+// double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*5/0.6; //frenet 将10改成1
|
|
|
+// int brake_distance=(int)brake_distance_double;
|
|
|
+// int step_count=0;
|
|
|
+// //1227,begin
|
|
|
+// if(mode0to5countSum>0)
|
|
|
+// {
|
|
|
+// dis_mode0to5countSum_frenet_s=MapTrace[i]->frenet_s-mode0_start_frenet_s;
|
|
|
+// }
|
|
|
+
|
|
|
+
|
|
|
+// if((MapTrace[i]->frenet_s>brake_distance)&&(dis_mode0to5countSum_frenet_s>brake_distance)&&(mode0to5countSum>0))
|
|
|
+// {
|
|
|
+// double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
|
|
|
+
|
|
|
+// for(int j=i;MapTrace[j]->frenet_s>MapTrace[i]->frenet_s-brake_distance;j--){
|
|
|
+// doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
|
|
|
+// step_count++;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else if(mode0to5countSum>0)
|
|
|
+// {
|
|
|
+// for(int j=i;MapTrace[j]->frenet_s>MapTrace[i]->frenet_s-dis_mode0to5countSum_frenet_s;j--)
|
|
|
+// {
|
|
|
+// doubledata[j][4]=Curve_SmallSpeed;
|
|
|
+// step_count++;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// doubledata[i][4]=Curve_SmallSpeed;
|
|
|
+// }
|
|
|
+
|
|
|
+// //1227，end
|
|
|
+
|
|
|
+
|
|
|
+// mode0to5count=0;
|
|
|
+// mode0_start_frenet_s=0.0;
|
|
|
+// dis_mode0to5countSum_frenet_s=0.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))*1/0.6;
|
|
|
+// double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*1/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>0||mode0to18countSum>0)
|
|
|
+// {
|
|
|
+// mode18countSum_frenet_s = MapTrace[i]->frenet_s - mode18_start_frenet_s;
|
|
|
+// mode0to18countSum_frenet_s = MapTrace[i]->frenet_s-mode0_start_frenet_s;
|
|
|
+// }
|
|
|
+
|
|
|
+// if((mode18countSum_frenet_s>brake_distanceLarge)&&(mode18countSum>0))
|
|
|
+// {
|
|
|
+// double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
|
|
|
+
|
|
|
+// for(int j=i;MapTrace[j]->frenet_s>MapTrace[i]->frenet_s-brake_distanceLarge;j--){
|
|
|
+// doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
|
|
|
+// step_count++;
|
|
|
+// }
|
|
|
+
|
|
|
+// }
|
|
|
+// else if(mode0to18countSum_frenet_s>brake_distance0to18)
|
|
|
+// {
|
|
|
+
|
|
|
+// double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
|
|
|
+
|
|
|
+// for(int j=i;MapTrace[j]->frenet_s>MapTrace[i]->frenet_s-brake_distance0to18;j--){
|
|
|
+// doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
|
|
|
+// step_count++;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else if(mode0to18countSum>0)
|
|
|
+// {
|
|
|
+// // for(int j=i;j>=i-mode0to18countSum;j--){
|
|
|
+// for(int j=i;MapTrace[j]->frenet_s>=MapTrace[i]->frenet_s-mode0to18countSum_frenet_s;j--){
|
|
|
+// doubledata[j][4]=Curve_LargeSpeed;
|
|
|
+// step_count++;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// doubledata[i][4]=Curve_LargeSpeed;
|
|
|
+// }
|
|
|
+
|
|
|
+// mode0to5count=0;
|
|
|
+// mode0_start_frenet_s=0.0;
|
|
|
+// mode18_start_frenet_s=0.0;
|
|
|
+// mode18countSum_frenet_s = 0.0;
|
|
|
+// mode0to18countSum_frenet_s = 0.0;
|
|
|
+// mode18count=0;
|
|
|
+// mode0to5flash=0;
|
|
|
+// mode18flash=0;
|
|
|
+// }
|
|
|
+// }
|
|
|
+
|
|
|
+// //1227,end
|
|
|
+
|
|
|
+// 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]<< ","<<"num,curve"<< "," <<i<< ","
|
|
|
+// <<MapTrace[i]->mfCurvature<<","<<"frenet_s"<< "," <<MapTrace[i]->frenet_s<<endl;
|
|
|
+// outfile.close();
|
|
|
+// //将数据写入文件结束
|
|
|
+// }
|
|
|
+
|
|
|
+// /*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]<< ","<<"num,curve"<< "," <<i<< ","
|
|
|
+// <<MapTrace[i]->mfCurvature<<endl;
|
|
|
+// outfile.close();
|
|
|
+// //将数据写入文件结束
|
|
|
+// }*/
|
|
|
+
|
|
|
+//}
|
|
|
+
|
|
|
+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,13.0);
|
|
|
+
|
|
|
+ ///计算全局地图S
|
|
|
+ bool flag=false;//保证S逐渐增加
|
|
|
+ if(fabs((*MapTrace[MapTrace.size()-1]).frenet_s)<1.0e-6){
|
|
|
+ double sum_s=0.0;
|
|
|
+ for(int i=0;i<MapTrace.size();i++)
|
|
|
+ {
|
|
|
+ if ((i==0)&&(flag==false))
|
|
|
+ {
|
|
|
+ sum_s=0.0;
|
|
|
+ (*MapTrace[i]).frenet_s=0;
|
|
|
+ flag=true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+
|
|
|
+ sum_s = sum_s+sqrt(pow(((*MapTrace[i]).gps_x-(*MapTrace[i-1]).gps_x),2) + pow(((*MapTrace[i]).gps_y-(*MapTrace[i-1]).gps_y),2));
|
|
|
+ (*MapTrace[i]).frenet_s=sum_s;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ }
|
|
|
+// //1227，begin
|
|
|
+ double dis_mode0to5countSum_frenet_s = -1.0;
|
|
|
+ double mode0_start_frenet_s=-1.0;
|
|
|
+ double mode18_start_frenet_s=-1.0;
|
|
|
+ double mode18countSum_frenet_s=-1.0;
|
|
|
+ double mode0to18countSum_frenet_s=-1.0;
|
|
|
+
|
|
|
+ for(int i=0;i<MapTrace.size();i++)
|
|
|
+ {
|
|
|
+ if(MapTrace[i]->roadMode==0)
|
|
|
+ {
|
|
|
+ doubledata[i][4]=straightSpeed;
|
|
|
+ if(mode0to5count==0)
|
|
|
+ {
|
|
|
+ mode0_start_frenet_s =MapTrace[i]->frenet_s;
|
|
|
+ }
|
|
|
+ mode0to5count++;
|
|
|
+ //mode0to18count++;
|
|
|
+ mode18count=0;
|
|
|
+ mode0to5flash=mode0to5count;
|
|
|
+ }
|
|
|
+ else if(MapTrace[i]->roadMode==5)
|
|
|
+ {
|
|
|
+ doubledata[i][4]=straightCurveSpeed;
|
|
|
+ if(mode0to5count==0)
|
|
|
+ {
|
|
|
+ mode0_start_frenet_s =MapTrace[i]->frenet_s;
|
|
|
+ }
|
|
|
+ mode0to5count++;
|
|
|
+ //mode0to18count++;
|
|
|
+ mode18count=0;
|
|
|
+ mode0to5flash=mode0to5count;
|
|
|
+ }
|
|
|
+ else if(MapTrace[i]->roadMode==18)
|
|
|
+ {
|
|
|
+ mode0to5countSum=mode0to5count;
|
|
|
+ doubledata[i][4]=Curve_SmallSpeed;
|
|
|
+ if(mode18count==0)
|
|
|
+ {
|
|
|
+ mode18_start_frenet_s=MapTrace[i]->frenet_s;
|
|
|
+ }
|
|
|
+
|
|
|
+ double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*1/0.6; //frenet 将10改成1
|
|
|
+ int brake_distance=(int)brake_distance_double;
|
|
|
+ int step_count=0;
|
|
|
+ //1227,begin
|
|
|
+
|
|
|
+ if((mode0to5countSum>0)&&(mode0_start_frenet_s>=0.0))
|
|
|
+ {
|
|
|
+ dis_mode0to5countSum_frenet_s=MapTrace[i]->frenet_s-mode0_start_frenet_s;
|
|
|
+ }
|
|
|
+
|
|
|
+ if((MapTrace[i]->frenet_s>brake_distance)&&(mode0to5countSum>0)&&(dis_mode0to5countSum_frenet_s>brake_distance))
|
|
|
+ {
|
|
|
+ double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
|
|
|
+
|
|
|
+ for(int j=i;MapTrace[j]->frenet_s>MapTrace[i]->frenet_s-brake_distance;j--){
|
|
|
+ if(MapTrace[j]->roadMode==5)
|
|
|
+ doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
|
|
|
+ else if(MapTrace[j]->roadMode==14)
|
|
|
+ doubledata[j][4]= min((Curve_SmallSpeed+step_count*step_speed),Curve_LargeSpeed);
|
|
|
+ step_count++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(mode0to5countSum>0)
|
|
|
+ {
|
|
|
+ for(int j=i;j>=i-mode0to5countSum;j--){
|
|
|
+ double v0 = sqrt(0.6*1*(MapTrace[i]->frenet_s-MapTrace[j]->frenet_s)+pow(Curve_SmallSpeed,2));
|
|
|
+ doubledata[j][4]=min(v0,straightCurveSpeed);//Curve_SmallSpeed;
|
|
|
+ step_count++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ doubledata[i][4]=Curve_SmallSpeed;
|
|
|
+ }
|
|
|
+ //1227，end
|
|
|
+
|
|
|
+
|
|
|
+ mode0to5count=0;
|
|
|
+ mode0_start_frenet_s=-1.0;
|
|
|
+ dis_mode0to5countSum_frenet_s=-1.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))*1/0.6;
|
|
|
+ double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*1/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>0)&&(mode18_start_frenet_s>=0.0))
|
|
|
+ {
|
|
|
+ mode18countSum_frenet_s = MapTrace[i]->frenet_s - mode18_start_frenet_s;
|
|
|
+ }
|
|
|
+ if((mode0to18countSum>0)&&(mode0_start_frenet_s>=0.0))
|
|
|
+ {
|
|
|
+ mode0to18countSum_frenet_s = MapTrace[i]->frenet_s-mode0_start_frenet_s;
|
|
|
+ }
|
|
|
+
|
|
|
+ if((mode18countSum_frenet_s>brake_distanceLarge)&&(mode18countSum>0)&&(i>=1)&&(MapTrace[i-1]->roadMode==18))
|
|
|
+ {
|
|
|
+ double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
|
|
|
+
|
|
|
+ for(int j=i;MapTrace[j]->frenet_s>MapTrace[i]->frenet_s-brake_distanceLarge;j--){
|
|
|
+ doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
|
|
|
+ step_count++;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ else if(mode0to18countSum_frenet_s>=brake_distance0to18)
|
|
|
+ {
|
|
|
+
|
|
|
+ double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
|
|
|
+
|
|
|
+ for(int j=i;MapTrace[j]->frenet_s>MapTrace[i]->frenet_s-brake_distance0to18;j--){
|
|
|
+ doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
|
|
|
+ step_count++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(mode0to18countSum>0)
|
|
|
+ {
|
|
|
+ for(int j=i;j>=i-mode0to18countSum;j--){
|
|
|
+ doubledata[j][4]=Curve_LargeSpeed;
|
|
|
+ step_count++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ doubledata[i][4]=Curve_LargeSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+ mode0to5count=0;
|
|
|
+ mode0_start_frenet_s=-1.0;
|
|
|
+ mode18_start_frenet_s=-1.0;
|
|
|
+ mode18countSum_frenet_s = -1.0;
|
|
|
+ mode0to18countSum_frenet_s = -1.0;
|
|
|
+ mode18count=0;
|
|
|
+ mode0to5flash=0;
|
|
|
+ mode18flash=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ //1227,end
|
|
|
+
|
|
|
+ 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]<< ","<<"num,curve"<< "," <<i<< ","
|
|
|
+ <<MapTrace[i]->mfCurvature<<","<<"frenet_s"<< "," <<MapTrace[i]->frenet_s<<endl;
|
|
|
+ outfile.close();
|
|
|
+ //将数据写入文件结束
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+//首次找点
|
|
|
+
|
|
|
+int iv::decition::Compute00::getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
|
|
|
+{
|
|
|
+ int index = -1;
|
|
|
+ // DecideGps00().minDis = iv::MaxValue;
|
|
|
+ float minDis = 10;
|
|
|
+ double maxAng = iv::MaxValue;
|
|
|
+
|
|
|
+ int startIndex = 0; // startIndex = 0 则每一次都是遍历整条地图路线
|
|
|
+ int endIndex = gpsMap.size() - 1;
|
|
|
+
|
|
|
+ for (int j = startIndex; j < endIndex; j++)
|
|
|
+ {
|
|
|
+ int i = (j + gpsMap.size()) % gpsMap.size();
|
|
|
+ double tmpdis = GetDistance(rp, (*gpsMap[i]));
|
|
|
+
|
|
|
+ if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
|
|
|
+ || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
|
|
|
+ || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
|
|
|
+ )
|
|
|
+ {
|
|
|
+ index = i;
|
|
|
+ minDis = tmpdis;
|
|
|
+ maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
|
|
|
+ maxAng = min(maxAng, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ DecideGps00().maxAngle=maxAng;
|
|
|
+ DecideGps00().minDis=minDis;
|
|
|
+ return index;
|
|
|
+}
|
|
|
+
|
|
|
+//search pathpoint
|
|
|
+int iv::decition::Compute00::getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
|
|
|
+{
|
|
|
+ int index = -1;
|
|
|
+ float minDis = 10;
|
|
|
+ double maxAng = iv::MaxValue;
|
|
|
+ int map_size=gpsMap.size();
|
|
|
+ int preDistance=max(100,(int)(rp.speed*10));
|
|
|
+ preDistance=min(500,preDistance);
|
|
|
+
|
|
|
+ //int startIndex = max((int)(lastIndex - 100),(int)(lastIndex-map_size)); // startIndex = 0 则每一次都是遍历整条地图路线
|
|
|
+ int endIndex = min((int)(lastIndex + preDistance ),(int)(map_size-1));
|
|
|
+ int startIndex=0;
|
|
|
+ if(lastIndex>100){
|
|
|
+ startIndex = max((int)(lastIndex - 100),(int)(lastIndex-map_size));
|
|
|
+ }else{
|
|
|
+ startIndex=0;
|
|
|
+ }
|
|
|
+
|
|
|
+ for (int j = startIndex; j < endIndex; j++)
|
|
|
+ {
|
|
|
+ int i = (j + map_size) % map_size;
|
|
|
+ double tmpdis = GetDistance(rp, (*gpsMap[i]));
|
|
|
+
|
|
|
+ if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
|
|
|
+ || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
|
|
|
+ || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
|
|
|
+ )
|
|
|
+ {
|
|
|
+ index = i;
|
|
|
+ minDis = tmpdis;
|
|
|
+ maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
|
|
|
+ maxAng = min(maxAng, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ DecideGps00().maxAngle=maxAng;
|
|
|
+ DecideGps00().minDis=minDis;
|
|
|
+ return index;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::getAveDef(std::vector<Point2D> farTrace)
|
|
|
+{
|
|
|
+ double sum_x = 0;
|
|
|
+ double sum_y = 0;
|
|
|
+
|
|
|
+ for (int i = 0; i < min(5, (int)farTrace.size()); i++)
|
|
|
+ {
|
|
|
+ sum_x += farTrace[i].x;
|
|
|
+ sum_y += abs(farTrace[i].y);
|
|
|
+ }
|
|
|
+ double average_y = sum_y / min(5, (int)farTrace.size());
|
|
|
+ double average_x = sum_x / min(5, (int)farTrace.size());
|
|
|
+
|
|
|
+
|
|
|
+ return atan(average_x / average_y) / PI * 180;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX)
|
|
|
+{
|
|
|
+ double sum_x = 0;
|
|
|
+ double sum_y = 0;
|
|
|
+
|
|
|
+ for (int i = 0; i < min(5, (int)farTrace.size()); i++)
|
|
|
+ {
|
|
|
+ sum_x += farTrace[i].x;
|
|
|
+ sum_y += abs(farTrace[i].y);
|
|
|
+ }
|
|
|
+ double average_y = sum_y / min(5, (int)farTrace.size());
|
|
|
+ double average_x = sum_x / min(5, (int)farTrace.size());
|
|
|
+
|
|
|
+
|
|
|
+ return atan(average_x + avoidX / average_y) / PI * 180;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::getDecideAngle(std::vector<Point2D> gpsTrace, double realSpeed) {
|
|
|
+
|
|
|
+ double ang = 0;
|
|
|
+ double EPos = 0, EAng = 0;
|
|
|
+
|
|
|
+ // double KEang = 14, KEPos = 10, DEang = 3, DEPos = 1; // double KEang = 14, KEPos = 10, DEang = 10, DEPos = 10;
|
|
|
+ double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
|
|
|
+
|
|
|
+ if(transferPieriod&& !transferPieriod2){
|
|
|
+ DEang = 200;
|
|
|
+ DEPos = 150;
|
|
|
+ }
|
|
|
+
|
|
|
+ // double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
|
|
|
+
|
|
|
+ double PreviewDistance;//预瞄距离
|
|
|
+ realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
|
|
|
+ if(changeRoad ||transferPieriod){
|
|
|
+ PreviewDistance=PreviewDistance+avoidX;
|
|
|
+ }
|
|
|
+ if(realSpeed <15){
|
|
|
+ PreviewDistance = max(4.0, realSpeed *0.4) ;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (gpsTrace[0].v1 == 1)
|
|
|
+ {
|
|
|
+ KEang = 14; KEPos = 10;
|
|
|
+ }
|
|
|
+ else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
|
|
|
+ {
|
|
|
+ KEang = 14; KEPos = 10;
|
|
|
+ }
|
|
|
+ else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
|
|
|
+ {
|
|
|
+ KEang = 14; KEPos = 10;
|
|
|
+ }
|
|
|
+ else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
|
|
|
+ {
|
|
|
+ KEang = 18; KEPos = 50; PreviewDistance = 3;
|
|
|
+ }
|
|
|
+ else if (gpsTrace[0].v1 == 7)
|
|
|
+ {
|
|
|
+ KEang = 20; KEPos = 50; PreviewDistance = 4;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (realSpeed > 40) KEang = 10; KEPos = 8;
|
|
|
+ if (realSpeed > 50) KEang = 5;
|
|
|
+
|
|
|
+ double sumdis = 0;
|
|
|
+ int gpsIndex = 0;
|
|
|
+ std::vector<Point2D> farTrace;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ for (int i = 1; i < gpsTrace.size() - 1; i++)
|
|
|
+ {
|
|
|
+ sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
|
|
|
+ if (sumdis > PreviewDistance)
|
|
|
+ {
|
|
|
+ gpsIndex = i;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ EPos = gpsTrace[gpsIndex].x;
|
|
|
+
|
|
|
+ for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
|
|
|
+ farTrace.push_back(gpsTrace[gpsIndex]);
|
|
|
+ }
|
|
|
+ if (farTrace.size() == 0) {
|
|
|
+ EAng = 0;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ EAng = getAveDef(farTrace);
|
|
|
+ }
|
|
|
+
|
|
|
+ ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
|
|
|
+
|
|
|
+ lastEA = EAng;
|
|
|
+ lastEP = EPos;
|
|
|
+
|
|
|
+ if (ang > angleLimit) {
|
|
|
+ ang = angleLimit;
|
|
|
+ }
|
|
|
+ else if (ang < -angleLimit) {
|
|
|
+ ang = -angleLimit;
|
|
|
+ }
|
|
|
+ if (lastAng != iv::MaxValue) {
|
|
|
+ ang = 0.2 * lastAng + 0.8 * ang;
|
|
|
+ //ODS("lastAng：%d\n", lastAng);
|
|
|
+ }
|
|
|
+ lastAng = ang;
|
|
|
+ return ang;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+int iv::decition::Compute00::getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed)
|
|
|
+{
|
|
|
+ int index = 1;
|
|
|
+ double sumdis = 0;
|
|
|
+ while (index < gpsTrace.size() && sumdis < realSpeed)
|
|
|
+ sumdis += GetDistance(gpsTrace[index - 1], gpsTrace[index++]);
|
|
|
+
|
|
|
+ if (index == gpsTrace.size())
|
|
|
+ return index - 1;
|
|
|
+
|
|
|
+ if (abs(sumdis - realSpeed) > abs(sumdis - GetDistance(gpsTrace[index - 1], gpsTrace[index]) - realSpeed))
|
|
|
+ index--;
|
|
|
+ return index;
|
|
|
+}
|
|
|
+
|
|
|
+/*iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
|
|
|
+
|
|
|
+ iv::Point2D obsPoint(-1, -1);
|
|
|
+ vector<Point2D> gpsTraceLeft;
|
|
|
+ vector<Point2D> gpsTraceRight;
|
|
|
+ float xiuzheng=0;
|
|
|
+ if(!ServiceCarStatus.useMobileEye){
|
|
|
+ xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
|
|
|
+ }
|
|
|
+
|
|
|
+ ServiceCarStatus.obsTraceLeft.clear();
|
|
|
+ ServiceCarStatus.obsTraceRight.clear();
|
|
|
+
|
|
|
+ for (int j = 0; j < gpsTrace.size(); j++)
|
|
|
+ {
|
|
|
+ double sumx1 = 0, sumy1 = 0, count1 = 0;
|
|
|
+ double sumx2 = 0, sumy2 = 0, count2 = 0;
|
|
|
+ for (int k = max(0, j - 4); k <= j; k++)
|
|
|
+ {
|
|
|
+ count1 = count1 + 1;
|
|
|
+ sumx1 += gpsTrace[k].x;
|
|
|
+ sumy1 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
|
|
|
+ {
|
|
|
+ count2 = count2 + 1;
|
|
|
+ sumx2 += gpsTrace[k].x;
|
|
|
+ sumy2 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ sumx1 /= count1; sumy1 /= count1;
|
|
|
+ sumx2 /= count2; sumy2 /= count2;
|
|
|
+
|
|
|
+ double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
|
|
|
+
|
|
|
+ double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
|
|
|
+ double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
|
|
|
+
|
|
|
+ Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
|
|
|
+ carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));
|
|
|
+
|
|
|
+ Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
|
|
|
+ carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));
|
|
|
+
|
|
|
+ gpsTraceLeft.push_back(ptLeft);
|
|
|
+ gpsTraceRight.push_back(ptRight);
|
|
|
+
|
|
|
+
|
|
|
+ TracePoint obsptleft(ptLeft.x,ptLeft.y);
|
|
|
+ ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
|
|
|
+
|
|
|
+ TracePoint obsptright(ptRight.x,ptRight.y);
|
|
|
+ ServiceCarStatus.obsTraceLeft.push_back(obsptright);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ bool isRemove = false;
|
|
|
+
|
|
|
+ for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
|
|
|
+ {
|
|
|
+ int count = 0;
|
|
|
+
|
|
|
+ for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
|
|
|
+ {
|
|
|
+ double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+
|
|
|
+ // int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx; //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
|
|
|
+ // int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
|
|
|
+ int dx = (ptx + gridwide*(double)centerx)/gridwide;
|
|
|
+ int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
|
|
|
+
|
|
|
+ if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
|
|
|
+ {
|
|
|
+ // if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
|
|
|
+ if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
|
|
|
+ {
|
|
|
+ count++; obsPoint.x = ptx; obsPoint.y = pty;
|
|
|
+ }
|
|
|
+ if (count >= 2)
|
|
|
+ {
|
|
|
+ obsPoint.x = gpsTrace[j].x;
|
|
|
+ obsPoint.y = gpsTrace[j].y;
|
|
|
+
|
|
|
+ //int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
|
|
|
+ //int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
|
|
|
+
|
|
|
+ obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
|
|
|
+ obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
|
|
|
+ obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
|
|
|
+
|
|
|
+ isRemove = true;
|
|
|
+// givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
|
|
|
+// obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
|
|
|
+
|
|
|
+ // DecideGps00().lidarDistance = obsPoint.y;
|
|
|
+ return obsPoint;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ j++;
|
|
|
+
|
|
|
+ for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
|
|
|
+ {
|
|
|
+ double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+
|
|
|
+ // int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
|
|
|
+ // int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
|
|
|
+ int dx = (ptx + gridwide*(double)centerx)/gridwide;
|
|
|
+ int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
|
|
|
+
|
|
|
+ if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
|
|
|
+ {
|
|
|
+ // if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
|
|
|
+ if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
|
|
|
+ {
|
|
|
+ count++; obsPoint.x = ptx; obsPoint.y = pty;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (count >= 2)
|
|
|
+ {
|
|
|
+ obsPoint.x = gpsTrace[j].x;
|
|
|
+ obsPoint.y = gpsTrace[j].y;
|
|
|
+
|
|
|
+ //int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
|
|
|
+ //int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
|
|
|
+
|
|
|
+ obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
|
|
|
+ obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
|
|
|
+ obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
|
|
|
+
|
|
|
+ isRemove = true;
|
|
|
+// givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
|
|
|
+// obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
|
|
|
+
|
|
|
+ // DecideGps00().lidarDistance = obsPoint.y;
|
|
|
+ return obsPoint;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+// if (count >= 2)
|
|
|
+// {
|
|
|
+// obsPoint.x = gpsTrace[j].x;
|
|
|
+// obsPoint.y = gpsTrace[j].y;
|
|
|
+
|
|
|
+// int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
|
|
|
+// int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
|
|
|
+
|
|
|
+// obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
|
|
|
+// obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
|
|
|
+// obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
|
|
|
+
|
|
|
+// isRemove = true;
|
|
|
+// givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
|
|
|
+// obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
|
|
|
+// // DecideGps00().lidarDistance = obsPoint.y;
|
|
|
+// return obsPoint;
|
|
|
+// }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // DecideGps00().lidarDistance = obsPoint.y;
|
|
|
+ return obsPoint;
|
|
|
+}*/
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
|
|
|
+
|
|
|
+ iv::Point2D obsPoint(-1, -1);
|
|
|
+ vector<Point2D> gpsTraceLeft;
|
|
|
+ vector<Point2D> gpsTraceRight;
|
|
|
+ float xiuzheng=0;
|
|
|
+ if(!ServiceCarStatus.useMobileEye){
|
|
|
+ xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
|
|
|
+ }
|
|
|
+
|
|
|
+// ServiceCarStatus.obsTraceLeft.clear();
|
|
|
+// ServiceCarStatus.obsTraceRight.clear();
|
|
|
+
|
|
|
+ for (int j = 0; j < gpsTrace.size(); j++)
|
|
|
+ {
|
|
|
+ double sumx1 = 0, sumy1 = 0, count1 = 0;
|
|
|
+ double sumx2 = 0, sumy2 = 0, count2 = 0;
|
|
|
+ for (int k = max(0, j - 4); k <= j; k++)
|
|
|
+ {
|
|
|
+ count1 = count1 + 1;
|
|
|
+ sumx1 += gpsTrace[k].x;
|
|
|
+ sumy1 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
|
|
|
+ {
|
|
|
+ count2 = count2 + 1;
|
|
|
+ sumx2 += gpsTrace[k].x;
|
|
|
+ sumy2 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ sumx1 /= count1; sumy1 /= count1;
|
|
|
+ sumx2 /= count2; sumy2 /= count2;
|
|
|
+
|
|
|
+ double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
|
|
|
+
|
|
|
+ double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
|
|
|
+ double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
|
|
|
+
|
|
|
+ Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
|
|
|
+ carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));
|
|
|
+
|
|
|
+ Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
|
|
|
+ carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));
|
|
|
+
|
|
|
+ gpsTraceLeft.push_back(ptLeft);
|
|
|
+ gpsTraceRight.push_back(ptRight);
|
|
|
+
|
|
|
+
|
|
|
+// TracePoint obsptleft(ptLeft.x,ptLeft.y);
|
|
|
+// ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
|
|
|
+
|
|
|
+// TracePoint obsptright(ptRight.x,ptRight.y);
|
|
|
+// ServiceCarStatus.obsTraceRight.push_back(obsptright);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ bool isRemove = false;
|
|
|
+
|
|
|
+ for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
|
|
|
+ {
|
|
|
+ int count = 0;
|
|
|
+
|
|
|
+ for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
|
|
|
+ {
|
|
|
+ double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ int dx = (ptx + gridwide*(double)centerx)/gridwide;
|
|
|
+ int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
|
|
|
+
|
|
|
+ if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
|
|
|
+ {
|
|
|
+ if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
|
|
|
+ {
|
|
|
+ count++; obsPoint.x = ptx; obsPoint.y = pty;
|
|
|
+ }
|
|
|
+ if (count >= 2)
|
|
|
+ {
|
|
|
+ obsPoint.x = gpsTrace[j].x;
|
|
|
+ obsPoint.y = gpsTrace[j].y;
|
|
|
+ obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
|
|
|
+ obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
|
|
|
+ obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
|
|
|
+ isRemove = true;
|
|
|
+ return obsPoint;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ j++;
|
|
|
+
|
|
|
+ for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
|
|
|
+ {
|
|
|
+ double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ int dx = (ptx + gridwide*(double)centerx)/gridwide;
|
|
|
+ int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
|
|
|
+
|
|
|
+ if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
|
|
|
+ {
|
|
|
+ if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
|
|
|
+ {
|
|
|
+ count++; obsPoint.x = ptx; obsPoint.y = pty;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (count >= 2)
|
|
|
+ {
|
|
|
+ obsPoint.x = gpsTrace[j].x;
|
|
|
+ obsPoint.y = gpsTrace[j].y;
|
|
|
+
|
|
|
+ obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
|
|
|
+ obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
|
|
|
+ obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
|
|
|
+
|
|
|
+ isRemove = true;
|
|
|
+ return obsPoint;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return obsPoint;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+iv::Point2D iv::decition::Compute00::getLidarObsPointXY(std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight, iv::LidarGridPtr lidarGridPtr) {
|
|
|
+
|
|
|
+ iv::Point2D obsPoint(-1, -1);
|
|
|
+ float xiuzheng=0;
|
|
|
+ if(!ServiceCarStatus.useMobileEye){
|
|
|
+ xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
|
|
|
+ }
|
|
|
+
|
|
|
+ bool isRemove = false;
|
|
|
+
|
|
|
+ ServiceCarStatus.obsTraceLeft.clear();
|
|
|
+ ServiceCarStatus.obsTraceRight.clear();
|
|
|
+
|
|
|
+ for(int j=0;j<gpsTraceLeft.size();j++)
|
|
|
+ {
|
|
|
+ TracePoint obsptleft(gpsTraceLeft[j].x,gpsTraceLeft[j].y);
|
|
|
+ ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
|
|
|
+ }
|
|
|
+ for(int j=0;j<gpsTraceRight.size();j++)
|
|
|
+ {
|
|
|
+ TracePoint obsptright(gpsTraceRight[j].x,gpsTraceRight[j].y);
|
|
|
+ ServiceCarStatus.obsTraceRight.push_back(obsptright);
|
|
|
+ }
|
|
|
+ for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
|
|
|
+ {
|
|
|
+ int count = 0;
|
|
|
+
|
|
|
+ for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
|
|
|
+ {
|
|
|
+ double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ int dx = (ptx + gridwide*(double)centerx)/gridwide;
|
|
|
+ int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
|
|
|
+
|
|
|
+ if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
|
|
|
+ {
|
|
|
+ if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
|
|
|
+ {
|
|
|
+ count++; obsPoint.x = ptx; obsPoint.y = pty;
|
|
|
+ }
|
|
|
+ if (count >= 2)
|
|
|
+ {
|
|
|
+ obsPoint.x = gpsTrace[j].x;
|
|
|
+ obsPoint.y = gpsTrace[j].y;
|
|
|
+ obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
|
|
|
+ obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
|
|
|
+ obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
|
|
|
+ isRemove = true;
|
|
|
+ return obsPoint;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ j++;
|
|
|
+
|
|
|
+ for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
|
|
|
+ {
|
|
|
+ double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ int dx = (ptx + gridwide*(double)centerx)/gridwide;
|
|
|
+ int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
|
|
|
+
|
|
|
+ if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
|
|
|
+ {
|
|
|
+ if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
|
|
|
+ {
|
|
|
+ count++; obsPoint.x = ptx; obsPoint.y = pty;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (count >= 2)
|
|
|
+ {
|
|
|
+ obsPoint.x = gpsTrace[j].x;
|
|
|
+ obsPoint.y = gpsTrace[j].y;
|
|
|
+
|
|
|
+ obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
|
|
|
+ obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
|
|
|
+ obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
|
|
|
+
|
|
|
+ isRemove = true;
|
|
|
+ return obsPoint;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return obsPoint;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//1220
|
|
|
+iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
|
|
|
+
|
|
|
+ iv::Point2D obsPoint(-1, -1);
|
|
|
+ vector<Point2D> gpsTraceLeft;
|
|
|
+ vector<Point2D> gpsTraceRight;
|
|
|
+ float xiuzheng=0;
|
|
|
+ if(!ServiceCarStatus.useMobileEye){
|
|
|
+ xiuzheng=0-ServiceCarStatus.msysparam.rearLidarGpsXiuzheng;
|
|
|
+ }
|
|
|
+ for (int j = 0; j < gpsTrace.size(); j++)
|
|
|
+ {
|
|
|
+ double sumx1 = 0, sumy1 = 0, count1 = 0;
|
|
|
+ double sumx2 = 0, sumy2 = 0, count2 = 0;
|
|
|
+ for (int k = max(0, j - 4); k <= j; k++)
|
|
|
+ {
|
|
|
+ count1 = count1 + 1;
|
|
|
+ sumx1 += gpsTrace[k].x;
|
|
|
+ sumy1 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
|
|
|
+ {
|
|
|
+ count2 = count2 + 1;
|
|
|
+ sumx2 += gpsTrace[k].x;
|
|
|
+ sumy2 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ sumx1 /= count1; sumy1 /= count1;
|
|
|
+ sumx2 /= count2; sumy2 /= count2;
|
|
|
+
|
|
|
+ double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
|
|
|
+
|
|
|
+ double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
|
|
|
+ double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
|
|
|
+
|
|
|
+ Point2D ptLeft(carFrontx + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * cos(anglevalue + PI / 2),
|
|
|
+ carFronty + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * sin(anglevalue + PI / 2));
|
|
|
+
|
|
|
+ Point2D ptRight(carFrontx + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * cos(anglevalue - PI / 2),
|
|
|
+ carFronty + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * sin(anglevalue - PI / 2));
|
|
|
+
|
|
|
+ gpsTraceLeft.push_back(ptLeft);
|
|
|
+ gpsTraceRight.push_back(ptRight);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ bool isRemove = false;
|
|
|
+
|
|
|
+ for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (!isRemove && gpsTrace[j].y<(0-ServiceCarStatus.msysparam.rearGpsXiuzheng) )
|
|
|
+ {
|
|
|
+ int count = 0;
|
|
|
+
|
|
|
+ for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
|
|
|
+ {
|
|
|
+ double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+
|
|
|
+ // int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx; //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
|
|
|
+ // int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
|
|
|
+ int dx = (ptx + gridwide*(double)centerx)/gridwide;
|
|
|
+
|
|
|
+ dx=grx-dx;//1227
|
|
|
+
|
|
|
+ int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
|
|
|
+
|
|
|
+ if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
|
|
|
+ {
|
|
|
+ // if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
|
|
|
+ if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
|
|
|
+ {
|
|
|
+ count++; obsPoint.x = ptx; obsPoint.y = pty;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ j++;
|
|
|
+
|
|
|
+ for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
|
|
|
+ {
|
|
|
+ double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+
|
|
|
+ // int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
|
|
|
+ // int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
|
|
|
+ int dx = (ptx + gridwide*(double)centerx)/gridwide;
|
|
|
+
|
|
|
+ dx=grx-dx;//1227
|
|
|
+ int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
|
|
|
+
|
|
|
+ if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
|
|
|
+ {
|
|
|
+ // if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
|
|
|
+ if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
|
|
|
+ {
|
|
|
+ count++; obsPoint.x = ptx; obsPoint.y = pty;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (count >= 2)
|
|
|
+ {
|
|
|
+ obsPoint.x = gpsTrace[j].x;
|
|
|
+ obsPoint.y = gpsTrace[j].y;
|
|
|
+
|
|
|
+ isRemove = true;
|
|
|
+ // DecideGps00().lidarDistance = obsPoint.y;
|
|
|
+ return obsPoint;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // DecideGps00().lidarDistance = obsPoint.y;
|
|
|
+ return obsPoint;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
|
|
|
+
|
|
|
+ iv::Point2D obsPoint(-1, -1);
|
|
|
+ vector<Point2D> gpsTraceLeft;
|
|
|
+ vector<Point2D> gpsTraceRight;
|
|
|
+ for (int j = 0; j < gpsTrace.size(); j++)
|
|
|
+ {
|
|
|
+ double sumx1 = 0, sumy1 = 0, count1 = 0;
|
|
|
+ double sumx2 = 0, sumy2 = 0, count2 = 0;
|
|
|
+ for (int k = max(0, j - 4); k <= j; k++)
|
|
|
+ {
|
|
|
+ count1 = count1 + 1;
|
|
|
+ sumx1 += gpsTrace[k].x;
|
|
|
+ sumy1 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
|
|
|
+ {
|
|
|
+ count2 = count2 + 1;
|
|
|
+ sumx2 += gpsTrace[k].x;
|
|
|
+ sumy2 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ sumx1 /= count1; sumy1 /= count1;
|
|
|
+ sumx2 /= count2; sumy2 /= count2;
|
|
|
+
|
|
|
+ double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
|
|
|
+
|
|
|
+ double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
|
|
|
+ double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
|
|
|
+
|
|
|
+ //1127 fanwei xiuzheng
|
|
|
+ float buchang=0;
|
|
|
+ Point2D ptLeft(carFrontx + (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
|
|
|
+ carFronty + (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
|
|
|
+
|
|
|
+ Point2D ptRight(carFrontx + (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
|
|
|
+ carFronty + (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
|
|
|
+
|
|
|
+ gpsTraceLeft.push_back(ptLeft);
|
|
|
+ gpsTraceRight.push_back(ptRight);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ bool isRemove = false;
|
|
|
+
|
|
|
+ for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (!isRemove && gpsTrace[j].y>2.5 && gpsTraceLeft[j].y>2.5 && gpsTraceRight[j].y>2.5)
|
|
|
+ {
|
|
|
+ int count = 0;
|
|
|
+
|
|
|
+ for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
|
|
|
+ {
|
|
|
+ double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+
|
|
|
+ int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx; //*2左右多出一半的车宽（1米）
|
|
|
+ int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
|
|
|
+
|
|
|
+ if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
|
|
|
+ {
|
|
|
+ if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
|
|
|
+ {
|
|
|
+ count++; obsPoint.x = ptx; obsPoint.y = pty;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ j++;
|
|
|
+
|
|
|
+ for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
|
|
|
+ {
|
|
|
+ double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+ double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
|
|
|
+
|
|
|
+ int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
|
|
|
+ int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
|
|
|
+
|
|
|
+ if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
|
|
|
+ {
|
|
|
+ if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
|
|
|
+ {
|
|
|
+ count++; obsPoint.x = ptx; obsPoint.y = pty;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (count >= 2)
|
|
|
+ {
|
|
|
+ obsPoint.x = gpsTrace[j].x;
|
|
|
+ obsPoint.y = gpsTrace[j].y;
|
|
|
+
|
|
|
+ isRemove = true;
|
|
|
+ DecideGps00().lidarDistanceAvoid = obsPoint.y;
|
|
|
+ return obsPoint;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // DecideGps00().lidarDistanceAvoid = obsPoint.y;
|
|
|
+
|
|
|
+ return obsPoint;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//int iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace, std::vector<ObstacleBasic> esrRadars) {
|
|
|
+// bool isRemove = false;
|
|
|
+//
|
|
|
+// for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
|
|
|
+// {
|
|
|
+//
|
|
|
+// for (int i = 0; i < esrRadars.size(); i++)
|
|
|
+// if ((esrRadars[i].nomal_y) != 0)
|
|
|
+// {
|
|
|
+// double xxx = esrRadars[i].nomal_x + Esr_Offset;
|
|
|
+// double yyy = esrRadars[i].nomal_y;
|
|
|
+//
|
|
|
+// if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
|
|
|
+// {
|
|
|
+//
|
|
|
+// if (lastEsrID == (esrRadars[i]).esr_ID)
|
|
|
+// {
|
|
|
+// lastEsrCount++;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// lastEsrCount = 0;
|
|
|
+// }
|
|
|
+//
|
|
|
+// if (lastEsrCount >= 3)
|
|
|
+// {
|
|
|
+// return i;
|
|
|
+// }
|
|
|
+//
|
|
|
+// lastEsrID = (esrRadars[i]).esr_ID;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// return -1;
|
|
|
+//}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+int iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum,int *esrPathpoint) {
|
|
|
+ bool isRemove = false;
|
|
|
+
|
|
|
+ float xiuzheng=0;
|
|
|
+ if(!ServiceCarStatus.useMobileEye){
|
|
|
+ xiuzheng=ServiceCarStatus.msysparam.radarGpsXiuzheng;
|
|
|
+ }
|
|
|
+
|
|
|
+ float fxiuzhengCs = DecideGps00().xiuzhengCs;
|
|
|
+ int nsize = gpsTrace.size();
|
|
|
+ for (int j = 1; j < nsize - 1 && !isRemove; j++)
|
|
|
+ {
|
|
|
+
|
|
|
+ for (int i = 0; i < 64; i++)
|
|
|
+ if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
|
|
|
+ {
|
|
|
+ double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
|
|
|
+ double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ xiuzheng;
|
|
|
+
|
|
|
+ /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
|
|
|
+ ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
|
|
|
+
|
|
|
+//优化
|
|
|
+// if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*ServiceCarStatus.msysparam.vehWidth / 2.0+DecideGps00().xiuzhengCs)){
|
|
|
+// *esrPathpoint = j;
|
|
|
+// return i;
|
|
|
+// }
|
|
|
+
|
|
|
+
|
|
|
+ if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+fxiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (lastEsrID == i)
|
|
|
+ {
|
|
|
+ lastEsrCount++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ lastEsrCount = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(yyy>50 ){
|
|
|
+ if (lastEsrCount >=200)
|
|
|
+ {
|
|
|
+ return i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (lastEsrCount >= 1)
|
|
|
+ {
|
|
|
+ return i;
|
|
|
+ }
|
|
|
+
|
|
|
+ lastEsrID = i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return -1;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+int iv::decition::Compute00::getRearEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
|
|
|
+ bool isRemove = false;
|
|
|
+
|
|
|
+ float xiuzheng = ServiceCarStatus.msysparam.rearRadarGpsXiuzheng;
|
|
|
+ for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
|
|
|
+ {
|
|
|
+
|
|
|
+ for (int i = 0; i < 64; i++)
|
|
|
+ if ((ServiceCarStatus.obs_rear_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_rear_radar[i].valid))
|
|
|
+ {
|
|
|
+ double xxx = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_x + Esr_Offset);
|
|
|
+ double yyy = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_y+ xiuzheng);
|
|
|
+
|
|
|
+ if(ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
|
|
|
+ xxx=0-xxx;
|
|
|
+ }
|
|
|
+
|
|
|
+ /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
|
|
|
+ ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
|
|
|
+
|
|
|
+
|
|
|
+ if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (lastEsrID == i)
|
|
|
+ {
|
|
|
+ lastEsrCount++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ lastEsrCount = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(yyy>50 ){
|
|
|
+ if (lastEsrCount >=200)
|
|
|
+ {
|
|
|
+ return i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (lastEsrCount >= 1)
|
|
|
+ {
|
|
|
+ return i;
|
|
|
+ }
|
|
|
+
|
|
|
+ lastEsrID = i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return -1;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//int iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
|
|
|
+// bool isRemove = false;
|
|
|
+
|
|
|
+// for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
|
|
|
+// {
|
|
|
+
|
|
|
+// for (int i = 0; i < 64; i++)
|
|
|
+// if ((examed_obs_radar[i].nomal_y) != 0 && (examed_obs_radar[i].valid))
|
|
|
+// {
|
|
|
+// double xxx = examed_obs_radar[i].nomal_x + Esr_Offset;
|
|
|
+// double yyy = examed_obs_radar[i].nomal_y+ Esr_Y_Offset;
|
|
|
+
|
|
|
+// /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
|
|
|
+// ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
|
|
|
+
|
|
|
+
|
|
|
+// if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
|
|
|
+// {
|
|
|
+
|
|
|
+// if (lastEsrID == i)
|
|
|
+// {
|
|
|
+// lastEsrCount++;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// lastEsrCount = 0;
|
|
|
+// }
|
|
|
+
|
|
|
+// if(yyy>50 ){
|
|
|
+// if (lastEsrCount >=200)
|
|
|
+// {
|
|
|
+// return i;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else if (lastEsrCount >= 3)
|
|
|
+// {
|
|
|
+// return i;
|
|
|
+// }
|
|
|
+
|
|
|
+// lastEsrID = i;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// return -1;
|
|
|
+//}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+int iv::decition::Compute00::getEsrIndexAvoid(std::vector<Point2D> gpsTrace) {
|
|
|
+ bool isRemove = false;
|
|
|
+
|
|
|
+ for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
|
|
|
+ {
|
|
|
+
|
|
|
+ for (int i = 0; i < 64; i++)
|
|
|
+ if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
|
|
|
+ {
|
|
|
+ double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
|
|
|
+ double yyy = ServiceCarStatus.obs_radar[i].nomal_y;
|
|
|
+
|
|
|
+ if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (lastEsrIDAvoid == i)
|
|
|
+ {
|
|
|
+ lastEsrCountAvoid++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ lastEsrCountAvoid = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (lastEsrCountAvoid >= 6)
|
|
|
+ {
|
|
|
+ return i;
|
|
|
+ }
|
|
|
+
|
|
|
+ lastEsrIDAvoid = i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return -1;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, std::vector<ObstacleBasic> esrRadars,double realSecSpeed) {
|
|
|
+// double obsSpeed = 0 - realSecSpeed;
|
|
|
+// double minDis = iv::MaxValue;
|
|
|
+// for (int i = 0; i < esrRadars.size(); i++)
|
|
|
+// if ((esrRadars[i].nomal_y) != 0)
|
|
|
+// {
|
|
|
+// double xxx = esrRadars[i].nomal_x + Esr_Offset;
|
|
|
+// double yyy = esrRadars[i].nomal_y;
|
|
|
+//
|
|
|
+// if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
|
|
|
+// {
|
|
|
+// double tmpDis =sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
|
|
|
+// if (tmpDis < minDis)
|
|
|
+// {
|
|
|
+// minDis = tmpDis;
|
|
|
+// obsSpeed = esrRadars[i].speed_y;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+//
|
|
|
+// return obsSpeed;
|
|
|
+//
|
|
|
+//
|
|
|
+//}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, double realSecSpeed) {
|
|
|
+ double obsSpeed = 0 - realSecSpeed;
|
|
|
+ double minDis = iv::MaxValue;
|
|
|
+ for (int i = 0; i < 64; i++)
|
|
|
+ if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
|
|
|
+ {
|
|
|
+ double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
|
|
|
+ double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
|
|
|
+
|
|
|
+ if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
|
|
|
+ {
|
|
|
+ double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
|
|
|
+ if (tmpDis < minDis)
|
|
|
+ {
|
|
|
+ minDis = tmpDis;
|
|
|
+ obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return obsSpeed;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::getDecideAvoidAngle(std::vector<Point2D> gpsTrace, double realSpeed, float avoidX) {
|
|
|
+
|
|
|
+ double ang = 0;
|
|
|
+ double EPos = 0, EAng = 0;
|
|
|
+
|
|
|
+ double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
|
|
|
+
|
|
|
+ double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
|
|
|
+
|
|
|
+
|
|
|
+ if (gpsTrace[0].v1 == 1)
|
|
|
+ {
|
|
|
+ KEang = 10; KEPos = 8;
|
|
|
+ if (realSpeed > 60) KEang = 5;
|
|
|
+ }
|
|
|
+ else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
|
|
|
+ {
|
|
|
+ KEang = 14; KEPos = 10;
|
|
|
+ }
|
|
|
+ else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
|
|
|
+ {
|
|
|
+ KEang = 14; KEPos = 10;
|
|
|
+ }
|
|
|
+ else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
|
|
|
+ {
|
|
|
+ KEang = 18; KEPos = 50; PreviewDistance = 3;
|
|
|
+ }
|
|
|
+ else if (gpsTrace[0].v1 == 7)
|
|
|
+ {
|
|
|
+ KEang = 20; KEPos = 50; PreviewDistance = 4;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ double sumdis = 0;
|
|
|
+ int gpsIndex = 0;
|
|
|
+ std::vector<Point2D> farTrace;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ for (int i = 1; i < gpsTrace.size() - 1; i++)
|
|
|
+ {
|
|
|
+ sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
|
|
|
+ if (sumdis > PreviewDistance)
|
|
|
+ {
|
|
|
+ gpsIndex = i;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if ((DecideGps00().readyParkMode) && (gpsIndex + 10>DecideGps00().gpsLineParkIndex))
|
|
|
+ {
|
|
|
+ gpsIndex = DecideGps00().gpsLineParkIndex;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ EPos = gpsTrace[gpsIndex].x + avoidX;
|
|
|
+
|
|
|
+ for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
|
|
|
+ farTrace.push_back(gpsTrace[gpsIndex]);
|
|
|
+ }
|
|
|
+ if (farTrace.size() == 0) {
|
|
|
+ EAng = 0;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ EAng = getAvoidAveDef(farTrace, avoidX);
|
|
|
+ }
|
|
|
+
|
|
|
+ ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
|
|
|
+
|
|
|
+ lastEA = EAng;
|
|
|
+ lastEP = EPos;
|
|
|
+
|
|
|
+ if (ang > angleLimit) {
|
|
|
+ ang = angleLimit;
|
|
|
+ }
|
|
|
+ else if (ang < -angleLimit) {
|
|
|
+ ang = -angleLimit;
|
|
|
+ }
|
|
|
+ if (lastAng != iv::MaxValue) {
|
|
|
+ ang = 0.2 * lastAng + 0.8 * ang;
|
|
|
+ //ODS("lastAng：%d\n", lastAng);
|
|
|
+ }
|
|
|
+ lastAng = ang;
|
|
|
+
|
|
|
+ return ang;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+std::vector<iv::GPSData> iv::decition::Compute00::getBesideGpsMapLine(iv::GPS_INS now_gps_ins, vector<iv::GPSData>gpsMapLine, float avoidX) {
|
|
|
+
|
|
|
+ vector<vector<iv::GPSData>> maps;
|
|
|
+ vector<iv::GPSData> gpsMapLineBeside;
|
|
|
+ int sizeN = gpsMapLine.size();
|
|
|
+ for (int i = 1; i < sizeN; i++)
|
|
|
+ {
|
|
|
+ iv::GPSData gpsData(new GPS_INS);
|
|
|
+ double xx = gpsMapLine[i]->gps_x - now_gps_ins.gps_x;
|
|
|
+ double yy = gpsMapLine[i]->gps_y - now_gps_ins.gps_y;
|
|
|
+ double lng = ServiceCarStatus.location->ins_heading_angle;
|
|
|
+
|
|
|
+
|
|
|
+ double x0 = xx * cos(lng * PI / 180) - yy * sin(lng * PI / 180);
|
|
|
+ double y0 = xx * sin(lng * PI / 180) + yy * cos(lng * PI / 180);
|
|
|
+ double k1 = sin((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
|
|
|
+ double k2 = cos((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
|
|
|
+
|
|
|
+ // memcpy(&gpsData, &gpsMapLine[i], sizeof(gpsData));
|
|
|
+
|
|
|
+ gpsData->speed_mode = gpsMapLine[i]->speed_mode;
|
|
|
+ gpsData->gps_x = x0 + k1 * avoidX;
|
|
|
+ gpsData->gps_y = y0 + k2 * avoidX;
|
|
|
+ gpsMapLineBeside.push_back(gpsData);
|
|
|
+
|
|
|
+ }
|
|
|
+ return gpsMapLineBeside;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//double iv::decition::Compute00::getDecideAngleByLane(double realSpeed) {
|
|
|
+
|
|
|
+// double ang = 0;
|
|
|
+// double EPos = 0, EAng = 0;
|
|
|
+
|
|
|
+// // double KEang = 14, KEpos = 10, DEang = 0, DEpos = 0;
|
|
|
+// double KEang = 5, KEPos = 30, DEang = 0, DEPos = 0;
|
|
|
+
|
|
|
+// // double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
|
|
|
+
|
|
|
+// double PreviewDistance;//预瞄距离
|
|
|
+// realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//// if (realSpeed > 40) KEang = 10; KEpos = 8;
|
|
|
+//// if (realSpeed > 50) KEang = 5;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
|
|
|
+//double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
|
|
|
+//double a = ServiceCarStatus.Lane.curvature;
|
|
|
+//double b = ServiceCarStatus.Lane.heading;
|
|
|
+//double c = (c1+c2)*0.5;
|
|
|
+//double x= PreviewDistance;
|
|
|
+//double y;
|
|
|
+
|
|
|
+
|
|
|
+//y=a*x*x+b*x+c;
|
|
|
+
|
|
|
+// // EPos = y;
|
|
|
+//EPos=c;
|
|
|
+
|
|
|
+
|
|
|
+// // EAng=atan(2*a*x+b) / PI * 180;
|
|
|
+// EAng=ServiceCarStatus.Lane.yaw;
|
|
|
+// ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
|
|
|
+
|
|
|
+// lastEA = EAng;
|
|
|
+// lastEP = EPos;
|
|
|
+
|
|
|
+
|
|
|
+// std::cout << "\nEPos:%f\n" << EPos << std::endl;
|
|
|
+// std::cout << "\nEAng:%f\n" << EAng << std::endl;
|
|
|
+
|
|
|
+
|
|
|
+// if (ang > angleLimit) {
|
|
|
+// ang = angleLimit;
|
|
|
+// }
|
|
|
+// else if (ang < -angleLimit) {
|
|
|
+// ang = -angleLimit;
|
|
|
+// }
|
|
|
+// if (lastAng != iv::MaxValue) {
|
|
|
+// ang = 0.2 * lastAng + 0.8 * ang;
|
|
|
+// //ODS("lastAng：%d\n", lastAng);
|
|
|
+// }
|
|
|
+// lastAng = ang;
|
|
|
+// return ang;
|
|
|
+// }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double IEPos = 0, IEang = 0;
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::getDecideAngleByLanePID(double realSpeed) {
|
|
|
+
|
|
|
+ double ang = 0;
|
|
|
+ double EPos = 0, EAng = 0;
|
|
|
+ double Curve=0;
|
|
|
+ double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
|
|
|
+ double KCurve=120;
|
|
|
+ double KIEPos = 0, KIEang = 0;
|
|
|
+ // double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
|
|
|
+
|
|
|
+ double PreviewDistance;//预瞄距离
|
|
|
+
|
|
|
+ int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
|
|
|
+ int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
|
|
|
+ int conf =min(confL,confR);
|
|
|
+
|
|
|
+ realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (realSpeed > 40) KEang = 10; KEPos = 8;
|
|
|
+ if (realSpeed > 50) KEang = 5;
|
|
|
+
|
|
|
+ KEPos = 20;
|
|
|
+ KEang = 200;
|
|
|
+ //KEang = 15;
|
|
|
+
|
|
|
+ double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
|
|
|
+ double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
|
|
|
+ double a = ServiceCarStatus.Lane.curvature;
|
|
|
+ double b = ServiceCarStatus.Lane.heading;
|
|
|
+ double c = (c1+c2)*0.5;
|
|
|
+ double yaw= ServiceCarStatus.Lane.yaw;
|
|
|
+
|
|
|
+ double x= PreviewDistance;
|
|
|
+ double y;
|
|
|
+
|
|
|
+
|
|
|
+ y=c-(a*x*x+b*x);
|
|
|
+ double difa=0-(atan(2*a*x+b) / PI * 180);
|
|
|
+ Curve=0-a;
|
|
|
+
|
|
|
+ //EAng=difa;
|
|
|
+ //EPos=y;
|
|
|
+ EAng= 0-b;
|
|
|
+ EPos = c;
|
|
|
+ DEang = 10;
|
|
|
+ DEPos = 20;
|
|
|
+ //DEang = 20;
|
|
|
+ //DEPos = 10;
|
|
|
+
|
|
|
+ IEang = EAng+0.7*IEang;
|
|
|
+ IEPos = EPos+0.7*IEPos;
|
|
|
+ KIEang = 0;
|
|
|
+ //KIEang = 0.5;
|
|
|
+ KIEPos =2;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(abs(confL)>=2&&abs(confR)>=2){
|
|
|
+ //ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
|
|
|
+ ang = KEang * EAng + KEPos * EPos +KCurve*Curve+ DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
|
|
|
+ }else{
|
|
|
+ ang=lastAng;
|
|
|
+ }
|
|
|
+ //if(lastAng!=0&&abs(ang-lastAng)>20)ang=lastAng;
|
|
|
+
|
|
|
+ lastEA = EAng;
|
|
|
+ lastEP = EPos;
|
|
|
+
|
|
|
+ if (ang > angleLimit) {
|
|
|
+ ang = angleLimit;
|
|
|
+ }
|
|
|
+ else if (ang < -angleLimit) {
|
|
|
+ ang = -angleLimit;
|
|
|
+ }
|
|
|
+ if (lastAng != iv::MaxValue) {
|
|
|
+ ang = 0.2 * lastAng + 0.8 * ang;
|
|
|
+ //ODS("lastAng：%d\n", lastAng);
|
|
|
+ }
|
|
|
+ lastAng = ang;
|
|
|
+ return ang;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
|
|
|
+
|
|
|
+ GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
|
|
|
+ Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ double x_1 = pt.x;
|
|
|
+ double y_1 = pt.y;
|
|
|
+ double angle_1 = getQieXianAngle(nowGps,aimGps);
|
|
|
+ double x_2 = 0.0, y_2 = 0.0;
|
|
|
+ double steering_angle;
|
|
|
+ double l = 2.950;
|
|
|
+ double r =6;
|
|
|
+ double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
|
|
|
+ double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
|
|
|
+ double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
|
|
|
+ double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
|
|
|
+ double g_1 = tan(angle_1);
|
|
|
+ double car_pos[3] = { x_1,y_1,g_1 };
|
|
|
+ double parking_pos[2] = { x_2,y_2 };
|
|
|
+ double g_3;
|
|
|
+ double t[4][2];
|
|
|
+ double p[4];
|
|
|
+ double s1, s2; //切点与车起始位置的距离
|
|
|
+ double min;
|
|
|
+ int min_i;
|
|
|
+
|
|
|
+ //g_3 = 0 - 0.5775;
|
|
|
+ g_3 = pingfenxian_xielv(x_1, y_1, x_2, y_2, angle_1);
|
|
|
+ //交点
|
|
|
+ x_3 = 0.0;//(y_1 - y_2 + g_2*x_2 - g_1*x_1) / (g_2 - g_1);
|
|
|
+ y_3 = y_1 - g_1 * x_1;
|
|
|
+ //圆心1
|
|
|
+ x_o_1 = r;
|
|
|
+ y_o_1 = g_3 * r + y_3;
|
|
|
+ //圆形1的切点1
|
|
|
+ x_t_1 = 0.0;
|
|
|
+ y_t_1 = g_3 * r + y_3;
|
|
|
+ //圆形1的切点2
|
|
|
+ if (g_1 == 0)
|
|
|
+ {
|
|
|
+ x_t_2 = r;
|
|
|
+ y_t_2 = y_1 - g_1 * x_1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ y_t_2 = (y_1 + g_1 * x_o_1 + y_o_1 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
|
|
|
+ x_t_2 = (y_t_2 + g_1 * x_1 - y_1) / g_1;
|
|
|
+
|
|
|
+ }
|
|
|
+ //圆心2
|
|
|
+ x_o_2 = 0 - r;
|
|
|
+ y_o_2 = y_3 - g_3 * r;
|
|
|
+ //圆形2的切点1
|
|
|
+ x_t_3 = 0;
|
|
|
+ y_t_3 = y_3 - g_3 * r;
|
|
|
+ //圆形2的切点2
|
|
|
+ if (g_1 == 0)
|
|
|
+ {
|
|
|
+ x_t_4 = 0 - r;
|
|
|
+ y_t_4 = y_1 - g_1 * x_1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ y_t_4 = (y_1 + g_1 * x_o_2 + y_o_2 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
|
|
|
+ x_t_4 = (y_t_4 + g_1 * x_1 - y_1) / g_1;
|
|
|
+
|
|
|
+ }
|
|
|
+ t[0][0] = x_t_1;
|
|
|
+ t[0][1] = y_t_1;
|
|
|
+ t[1][0] = x_t_2;
|
|
|
+ t[1][1] = y_t_2;
|
|
|
+ t[2][0] = x_t_3;
|
|
|
+ t[2][1] = y_t_3;
|
|
|
+ t[3][0] = x_t_4;
|
|
|
+ t[3][1] = y_t_4;
|
|
|
+ for (int i = 0; i < 4; i++)
|
|
|
+ {
|
|
|
+
|
|
|
+ p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
|
|
|
+
|
|
|
+ }
|
|
|
+ min = p[0];
|
|
|
+ min_i = 0;
|
|
|
+ for (int i = 1; i < 4; i++)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (p[i] < min)
|
|
|
+ {
|
|
|
+ min = p[i]; min_i = i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (min_i < 2)
|
|
|
+ {
|
|
|
+ x_o = x_o_1;
|
|
|
+ y_o = y_o_1;
|
|
|
+ s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
|
|
|
+ s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
|
|
|
+ if (s1 < s2)
|
|
|
+ {
|
|
|
+ x_t_n = x_t_1;
|
|
|
+ y_t_n = y_t_1;
|
|
|
+ x_t_f = x_t_2;
|
|
|
+ y_t_f = y_t_2;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ x_t_n = x_t_2;
|
|
|
+ y_t_n = y_t_2;
|
|
|
+ x_t_f = x_t_1;
|
|
|
+ y_t_f = y_t_1;
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ x_o = x_o_2;
|
|
|
+ y_o = y_o_2;
|
|
|
+ s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
|
|
|
+ s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
|
|
|
+
|
|
|
+ if (s1 < s2)
|
|
|
+ {
|
|
|
+
|
|
|
+ x_t_n = x_t_3;
|
|
|
+ y_t_n = y_t_3;
|
|
|
+ x_t_f = x_t_4;
|
|
|
+ y_t_f = y_t_4;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ x_t_n = x_t_4;
|
|
|
+ y_t_n = y_t_4;
|
|
|
+ x_t_f = x_t_3;
|
|
|
+ y_t_f = y_t_3;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ steering_angle = atan2(l, r);
|
|
|
+
|
|
|
+ if (x_t_n < 0)
|
|
|
+ {
|
|
|
+ steering_angle = 0 - steering_angle;
|
|
|
+ }
|
|
|
+
|
|
|
+ nearTpoint=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
|
|
|
+ farTpoint = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
|
|
|
+ bocheAngle = steering_angle*180/PI;
|
|
|
+
|
|
|
+ cout << "近切点：x_t_n=" << x_t_n << endl;
|
|
|
+ cout << "近切点：y_t_n=" << y_t_n << endl;
|
|
|
+ cout << "远切点：x_t_f=" << x_t_f << endl;
|
|
|
+ cout << "远切点：y_t_f=" << y_t_f << endl;
|
|
|
+ cout << "航向角：" << steering_angle << endl;
|
|
|
+
|
|
|
+
|
|
|
+ // if (x_1 < 0 && y_1 > 0 && x_1 < x_t_n &&y_t_f > 0.1) {
|
|
|
+ // return 1;
|
|
|
+ // }
|
|
|
+ Point2D ptN = Coordinate_Transfer(nearTpoint.gps_x, nearTpoint.gps_y, nowGps);
|
|
|
+ double disA = GetDistance(aimGps,nowGps);
|
|
|
+ if(y_t_n>0 && ptN.y<0 && y_t_f>0.1 && disA<40){
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//返回垂直平分线的斜率
|
|
|
+double iv::decition::Compute00::pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1) {
|
|
|
+ double angl, x_3, angle_3;
|
|
|
+ if (tan(angle_1 == 0))
|
|
|
+ {
|
|
|
+ if ((x_1 - x_2) > 0 && ((y_1 - y_2) > 0))
|
|
|
+ {
|
|
|
+ angle_3 = 0 - 1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ angle_3 = 1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ x_3 = (tan(angle_1)*x_1 - y_1) / tan(angle_1);//车所在直线与x轴交点
|
|
|
+ angl = tan(angle_1);//车所在直线的斜率
|
|
|
+ if ((x_1 - x_2)>0 && ((y_1 - y_2)>0))//第一象限
|
|
|
+ {
|
|
|
+ if ((angl *x_3)<0)//车斜率与车直线的x轴交点异号
|
|
|
+ {
|
|
|
+ if (angl < 0)
|
|
|
+ {
|
|
|
+ angle_3 = tan(PI*0.5 + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ angle_3 = tan(PI*0.5 + (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ else//第二象限
|
|
|
+ {
|
|
|
+ if ((angl*x_3)<0)//车斜率与车直线的x轴交点异号
|
|
|
+ {
|
|
|
+ if (angl < 0)
|
|
|
+ {
|
|
|
+ angle_3 = tan(PI*0.5 - (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ angle_3 = tan(atan(fabs(angl)) + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return angle_3;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps) {
|
|
|
+ double heading = nowGps.ins_heading_angle *PI/180;
|
|
|
+ double x1 = nowGps.gps_x;
|
|
|
+ double y1 = nowGps.gps_y;
|
|
|
+ if (heading<=PI*0.5)
|
|
|
+ {
|
|
|
+ heading = 0.5*PI - heading;
|
|
|
+ }
|
|
|
+ else if (heading>PI*0.5 && heading<=PI*1.5) {
|
|
|
+ heading = 1.5*PI - heading;
|
|
|
+ }
|
|
|
+ else if (heading>PI*1.5) {
|
|
|
+ heading = 2.5*PI - heading;
|
|
|
+ }
|
|
|
+ double k1 = tan(heading);
|
|
|
+ double x = x1+10;
|
|
|
+ double y = k1 * x + y1 - (k1 * x1);
|
|
|
+ Point2D pt1 = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
|
|
|
+ Point2D pt2 = Coordinate_Transfer(x, y, aimGps);
|
|
|
+ double xielv = (pt1.y - pt2.y) / (pt1.x - pt2.x);
|
|
|
+ double angle = atan(abs(xielv));
|
|
|
+ if (xielv<0)
|
|
|
+ {
|
|
|
+ angle = PI - angle;
|
|
|
+ }
|
|
|
+ return angle;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+/*
|
|
|
+ chuizhicheweiboche
|
|
|
+ */
|
|
|
+
|
|
|
+
|
|
|
+int iv::decition::Compute00::bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps)
|
|
|
+{
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ double l=2.95;//轴距
|
|
|
+ double x_0 = 0, y_0 = 0.5;
|
|
|
+ double x_1, y_1;//车起点坐标
|
|
|
+ double ange1;//车航向角弧度
|
|
|
+ double x_2, y_2;//另一条与车直线在angle2和R_M 固定情况下过坐标点，第二个近切点
|
|
|
+ double real_rad;;//另一条直线的航向角弧度
|
|
|
+ double angle_3;//垂直平分线弧度
|
|
|
+ double x_3, y_3;//垂直平分线交点
|
|
|
+ double x_4, y_4;//另一条直线的远切点坐标，第二个远切点，已知
|
|
|
+ double x_o_1, y_o_1;//圆形1坐标
|
|
|
+ double x_o_2, y_o_2;//圆形2坐标
|
|
|
+ double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
|
|
|
+ double min_rad;
|
|
|
+ double R_M; //后轴中点的转弯半径
|
|
|
+ double steering_angle;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
|
|
|
+ Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
|
|
|
+ x_1=pt.x;
|
|
|
+ y_1=pt.y;
|
|
|
+ ange1=getQieXianAngle(nowGps,aimGps);
|
|
|
+
|
|
|
+ min_rad_zhuanxiang(&R_M , &min_rad);
|
|
|
+ qiedian_n(x_1,y_1,R_M,min_rad,&x_2 , &y_2, &real_rad);//计算另一条与车直线在angle2和R_M 固定情况下近切点：x_2, y_2
|
|
|
+ liangzhixian_jiaodian( x_1, y_1, x_2, y_2,ange1,real_rad,&x_3 , &y_3);
|
|
|
+ chuizhipingfenxian_xielv( x_1, y_1, ange1, real_rad, min_rad,&angle_3);
|
|
|
+ yuanxin( x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_o_1,&y_o_1,&x_o_2,&y_o_2);
|
|
|
+ yuanxin_qiedian( ange1, x_o_1, y_o_1, x_o_2, y_o_2,
|
|
|
+ x_1, y_1, x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_t_n,&y_t_n,&x_t_f, &y_t_f);
|
|
|
+ steering_angle = atan2(l, R_M);
|
|
|
+ x_4 = 0.5;
|
|
|
+ y_4 = 0;
|
|
|
+ //for (int i = 0; i < 4; i++)
|
|
|
+ //{
|
|
|
+ //for (int j = 0; j < 2; j++)
|
|
|
+ //{
|
|
|
+ // cout << t[i][j] << endl;
|
|
|
+ //}
|
|
|
+ //}
|
|
|
+ //cout << "min_rad:" << min_rad<< endl;
|
|
|
+ //cout << "jiaodian：x=" << x_3 << endl;
|
|
|
+ //cout << "jiaodian：y=" << y_3 << endl;
|
|
|
+ // cout << "R-M：" << R_M << endl;
|
|
|
+ cout << "x_0:" << x_0 << endl;
|
|
|
+ cout << "y_0:" << y_0 << endl;
|
|
|
+ cout << "x_2:" << x_2 << endl;
|
|
|
+ cout << "y_2:" << y_2 << endl;
|
|
|
+ cout << "近切点：x_t_n="<< x_t_n << endl;
|
|
|
+ cout << "近切点：y_t_n=" << y_t_n << endl;
|
|
|
+ cout << "远切点：x_t_f=" << x_t_f << endl;
|
|
|
+ cout << "远切点：y_t_f=" << y_t_f << endl;
|
|
|
+ //cout << "航向角：" << steering_angle << endl;
|
|
|
+ //cout << "圆心1横坐标=" << x_o_1 << endl;
|
|
|
+ //cout << "圆心1纵坐标=" << y_o_1 << endl;
|
|
|
+ //cout << "圆心2横坐标=" << x_o_2 << endl;
|
|
|
+ //cout << "圆心2纵坐标=" << y_o_2 << endl;
|
|
|
+ //cout << "平分线弧度=" << angle_3 << endl;
|
|
|
+ //cout << " min_rad=" << min_rad << endl;
|
|
|
+ //cout << " real_rad=" << real_rad << endl;
|
|
|
+ // system("PAUSE");
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ dTpoint0=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
|
|
|
+ dTpoint1 = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
|
|
|
+ dTpoint2 = Coordinate_UnTransfer(x_2, y_2, aimGps);
|
|
|
+ dTpoint3 = Coordinate_UnTransfer(x_0, y_0, aimGps);
|
|
|
+ dBocheAngle = steering_angle*180/PI;
|
|
|
+
|
|
|
+ double disA = GetDistance(aimGps,nowGps);
|
|
|
+
|
|
|
+ if(pt.y>y_t_n && x_t_f<x_2 && y_t_f>y_2&&disA<40){
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::min_rad_zhuanxiang(double *R_M, double *min_rad) {
|
|
|
+ double L_c = 4.749;//车长
|
|
|
+ double rad_1;
|
|
|
+ double rad_2;
|
|
|
+ double L_k = 1.931;//车宽
|
|
|
+ double L = 2.95;//轴距
|
|
|
+ double L_f =1.2 ;//前悬
|
|
|
+ double L_r =0.7 ;//后悬
|
|
|
+ double R_min =6.5 ;//最小转弯半径
|
|
|
+ *R_M = fabs(sqrt(R_min*R_min - (L + L_f)*(L + L_f))) - L_k*0.5;//double R_M ;//后轴中点的转弯半径
|
|
|
+ //rad_1 = atan2(sqrt(R_min*R_min - (R_M - L_k*0.5)*(R_M - L_k*0.5)), R_M - L_k*0.5);
|
|
|
+ //rad_2 = atan2(L + L_f, R_M + L_k*0.5);
|
|
|
+ *min_rad = 45 * PI / 180;// rad_1 - rad_2;
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::qiedian_n(double x_1, double y_1, double R_M,double min_rad, double *x_2, double *y_2, double *real_rad ) {
|
|
|
+
|
|
|
+ if (x_1 > 0 && y_1 > 0)
|
|
|
+ {
|
|
|
+ *real_rad = PI*0.5 - min_rad;
|
|
|
+ *x_2 = R_M - R_M*cos(min_rad);
|
|
|
+ *y_2 = R_M*sin(min_rad) + 0.5;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ *real_rad = PI*0.5 + min_rad;
|
|
|
+ *x_2 = R_M*cos(min_rad) - R_M;
|
|
|
+ *y_2 = R_M*sin(min_rad) + 0.5;
|
|
|
+ }
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,double ange1,double real_rad,double *x_3,double *y_3) {
|
|
|
+ double b1, b2;
|
|
|
+ double k1, k2;
|
|
|
+ if (ange1!=(PI*0.5))
|
|
|
+ {
|
|
|
+ k1 = tan(ange1);
|
|
|
+ b1 = y_1 - k1*x_1;
|
|
|
+ k2 = tan(real_rad);
|
|
|
+ b2 = y_2 - k2*x_2;
|
|
|
+ *x_3 = (b2 - b1) / (k1 - k2);
|
|
|
+ *y_3 = k2*(*x_3) + b2;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ k2 = tan(real_rad);
|
|
|
+ b2 = y_2 - k2*x_2;
|
|
|
+ *x_3 = x_1;
|
|
|
+ *y_3 = k2*(*x_3) + b2;
|
|
|
+ }
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,double min_rad,double *angle_3) {
|
|
|
+ double k1, k2;
|
|
|
+ double angle_j;
|
|
|
+ k2 = tan(real_rad);
|
|
|
+ if (ange1 != (PI*0.5))
|
|
|
+ {
|
|
|
+ k1 = tan(ange1);
|
|
|
+ angle_j = atan(fabs((k2 - k1) / (1 + k2*k1)));//两直线夹角
|
|
|
+
|
|
|
+ if (x_1 > 0 && y_1 > 0)
|
|
|
+ {
|
|
|
+ *angle_3 = angle_j*0.5 - min_rad + PI;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ *angle_3 = min_rad - angle_j*0.5;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ angle_j = min_rad;//两直线夹角
|
|
|
+ if (x_1 > 0 && y_1 > 0)
|
|
|
+ {
|
|
|
+ *angle_3 = angle_j*0.5 - min_rad + PI;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ *angle_3 = min_rad - angle_j*0.5;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
|
|
|
+ double *x_o_1,double *y_o_1,double *x_o_2,double *y_o_2) {
|
|
|
+ double b2, b3, k2, k3;
|
|
|
+ b2 = y_2 - tan(real_rad)*x_2;
|
|
|
+ b3 = y_3 - tan(angle_3)*x_3;
|
|
|
+ k2 = tan(real_rad);
|
|
|
+ k3 = tan(angle_3);
|
|
|
+ *x_o_1 = (sqrt(k2*k2 + 1)*R_M + b3 - b2) / (k2 - k3);
|
|
|
+ *y_o_1 = k3*(*x_o_1) + b3;
|
|
|
+
|
|
|
+ *x_o_2 = (b3 - b2 - (sqrt(k2*k2 + 1)*R_M)) / (k2 - k3);
|
|
|
+ *y_o_2 = k3*(*x_o_2) + b3;
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::yuanxin_qiedian(double ange1,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
|
|
|
+ double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
|
|
|
+ double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f)
|
|
|
+{
|
|
|
+ double x_o, y_o;
|
|
|
+ double b2, b3, k1, k2, k3;
|
|
|
+ //double car_pos[3] = { x_1,y_1,k1 };
|
|
|
+ double parking_pos[2] = { x_2,y_2 };
|
|
|
+ //double t[4][2];
|
|
|
+ double p[4];
|
|
|
+ double s1, s2; //切点与车起始位置的距离
|
|
|
+ double min;
|
|
|
+ int min_i;
|
|
|
+ double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
|
|
|
+ double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
|
|
|
+ double t[4][2];
|
|
|
+ k1 = tan(ange1);
|
|
|
+ b2 = y_2 - tan(real_rad)*x_2;
|
|
|
+ b3 = y_3 - tan(real_rad)*x_3;
|
|
|
+ k2 = tan(real_rad);//另一条直线斜率
|
|
|
+ k3 = tan(angle_3);//垂直平分线斜率
|
|
|
+ //圆心1和2切点*********************************************
|
|
|
+ if (x_1 > 0 && y_1 > 0)//第一象限
|
|
|
+ {
|
|
|
+ if (ange1 == (PI*0.5))
|
|
|
+ {
|
|
|
+ x_t_1 = x_1;
|
|
|
+ y_t_1 = y_o_1;
|
|
|
+ y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
|
|
|
+ x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
|
|
|
+
|
|
|
+ x_t_3 = x_1;
|
|
|
+ y_t_3 = y_o_2;
|
|
|
+ y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
|
|
|
+ x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
|
|
|
+ x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
|
|
|
+ y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
|
|
|
+ x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
|
|
|
+
|
|
|
+ y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
|
|
|
+ x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
|
|
|
+ y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
|
|
|
+ x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if (ange1 == 0)
|
|
|
+ {
|
|
|
+ x_t_1 = 0 - x_1;
|
|
|
+ y_t_1 = y_o_1;
|
|
|
+ y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
|
|
|
+ x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
|
|
|
+
|
|
|
+ x_t_3 = 0 - x_1;
|
|
|
+ y_t_3 = y_o_2;
|
|
|
+ y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
|
|
|
+ x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
|
|
|
+ x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
|
|
|
+ y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
|
|
|
+ x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
|
|
|
+
|
|
|
+ y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
|
|
|
+ x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
|
|
|
+ y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
|
|
|
+ x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ //圆心1和2切点*********************************************
|
|
|
+
|
|
|
+ t[0][0] = x_t_1;
|
|
|
+ t[0][1] = y_t_1;
|
|
|
+ t[1][0] = x_t_2;
|
|
|
+ t[1][1] = y_t_2;
|
|
|
+ t[2][0] = x_t_3;
|
|
|
+ t[2][1] = y_t_3;
|
|
|
+ t[3][0] = x_t_4;
|
|
|
+ t[3][1] = y_t_4;
|
|
|
+ for (int i = 0; i < 4; i++)
|
|
|
+ {
|
|
|
+
|
|
|
+ p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
|
|
|
+
|
|
|
+ }
|
|
|
+ min = p[0];
|
|
|
+ min_i = 0;
|
|
|
+ for (int i = 1; i < 4; i++)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (p[i] < min)
|
|
|
+ {
|
|
|
+ min = p[i]; min_i = i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (min_i < 2)
|
|
|
+ {
|
|
|
+ x_o = x_o_1;
|
|
|
+ y_o = y_o_1;
|
|
|
+ s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
|
|
|
+ s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
|
|
|
+ if (s1 < s2)
|
|
|
+ {
|
|
|
+ *x_t_n = x_t_1;
|
|
|
+ *y_t_n = y_t_1;
|
|
|
+ *x_t_f = x_t_2;
|
|
|
+ *y_t_f = y_t_2;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ *x_t_n = x_t_2;
|
|
|
+ *y_t_n = y_t_2;
|
|
|
+ *x_t_f = x_t_1;
|
|
|
+ *y_t_f = y_t_1;
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ x_o = x_o_2;
|
|
|
+ y_o = y_o_2;
|
|
|
+ s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
|
|
|
+ s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
|
|
|
+
|
|
|
+ if (s1 < s2)
|
|
|
+ {
|
|
|
+
|
|
|
+ *x_t_n = x_t_3;
|
|
|
+ *y_t_n = y_t_3;
|
|
|
+ *x_t_f = x_t_4;
|
|
|
+ *y_t_f = y_t_4;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ *x_t_n = x_t_4;
|
|
|
+ *y_t_n = y_t_4;
|
|
|
+ *x_t_f = x_t_3;
|
|
|
+ *y_t_f = y_t_3;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+int iv::decition::Compute00::getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap)
|
|
|
+{
|
|
|
+ int index = -1;
|
|
|
+ int startIndex = 0; // startIndex = 0 则每一次都是遍历整条地图路线
|
|
|
+ int endIndex = gpsMap.size() - 1;
|
|
|
+ float minDis=20;
|
|
|
+
|
|
|
+ for (int j = startIndex; j < endIndex; j++)
|
|
|
+ {
|
|
|
+ int i = (j + gpsMap.size()) % gpsMap.size();
|
|
|
+ double tmpdis = GetDistance(rp, (*gpsMap[i]));
|
|
|
+
|
|
|
+ if (tmpdis < minDis)
|
|
|
+ {
|
|
|
+ index = i;
|
|
|
+ minDis=tmpdis;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return index;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::Compute00::getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const vector<Point2D> gpsTrace, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps) {
|
|
|
+ double obsSpeed = 0 - realSecSpeed;
|
|
|
+ double minDis = iv::MaxValue;
|
|
|
+ FrenetPoint esr_obs_F_point;
|
|
|
+ for (int i = 0; i < 64; i++)
|
|
|
+ if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
|
|
|
+ {
|
|
|
+ double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
|
|
|
+ double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
|
|
|
+
|
|
|
+ if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
|
|
|
+ {
|
|
|
+ double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
|
|
|
+ if (tmpDis < minDis)
|
|
|
+ {
|
|
|
+ minDis = tmpDis;
|
|
|
+// esr_obs_F_point = iv::decition::FrenetPlanner::XY2Frenet(xxx, yyy, gpsTrace);
|
|
|
+ esr_obs_F_point = iv::decition::FrenetPlanner::getFrenetfromXY(xxx, yyy, gpsTrace,gpsMap,pathpoint,nowGps);
|
|
|
+// obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
|
|
|
+ double speedx=ServiceCarStatus.obs_radar[i].speed_x; //障碍物相对于车辆x轴的速度
|
|
|
+ double speedy=ServiceCarStatus.obs_radar[i].speed_y; //障碍物相对于车辆y轴的速度
|
|
|
+ double speed_combine = sqrt(speedx*speedx+speedy*speedy); //将x、y轴两个方向的速度求矢量和
|
|
|
+ //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
|
|
|
+ //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
|
|
|
+ double Etheta = esr_obs_F_point.tangent_Ang - atan2(speedy,speedx);
|
|
|
+
|
|
|
+ obsSpeed = speed_combine*cos(Etheta); //由speed_combine分解的s轴方向上的速度
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return obsSpeed;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+int iv::decition::Compute00::getEsrIndexByFrenet(std::vector<Point2D> gpsTrace, FrenetPoint &esrObsPoint, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps){
|
|
|
+ double minDistance = numeric_limits<double>::max();
|
|
|
+ int minDis_index=-1;
|
|
|
+
|
|
|
+ for(int i=0; i<64; ++i){
|
|
|
+ if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid)){
|
|
|
+ //毫米波在车头，故要加上毫米波与惯导的相对距离。(xxx,yyy)才是障碍物在 车辆坐标系下的坐标。
|
|
|
+ double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
|
|
|
+ double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;
|
|
|
+
|
|
|
+ //将毫米波障碍物位置转换到frenet坐标系下
|
|
|
+// esrObsPoint = iv::decition::FrenetPlanner::XY2Frenet(xxx,yyy,gpsTrace);
|
|
|
+ esrObsPoint = iv::decition::FrenetPlanner::getFrenetfromXY(xxx,yyy,gpsTrace,gpsMap,pathpoint,nowGps);
|
|
|
+
|
|
|
+ //如果障碍物与道路的横向距离d<=3.0*ServiceCarStatus.msysparam.vehWidth / 4.0，则认为道路上有障碍物。
|
|
|
+ //s则可理解为障碍物距离。为障碍物与车辆沿着道路方向的距离，而不是空间上的x或y坐标或者直线距离。
|
|
|
+ //minDistance、minDis_index用来统计最近的障碍物信息。
|
|
|
+ if(abs(esrObsPoint.d)<=(3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs)){
|
|
|
+ if(esrObsPoint.s<minDistance){
|
|
|
+ minDistance = esrObsPoint.s;
|
|
|
+ minDis_index = i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return minDis_index;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+std::vector<std::vector<iv::GPSData>> gmapsL;
|
|
|
+std::vector<std::vector<iv::GPSData>> gmapsR;
|
chenxiaowei