|
|
@@ -1,5 +1,9 @@
|
|
|
#include "ivdecision_brain.h"
|
|
|
|
|
|
+#include "common/obs_predict.h"
|
|
|
+#include "ivlog.h"
|
|
|
+
|
|
|
+extern iv::Ivlog * givlog;
|
|
|
|
|
|
bool handPark;
|
|
|
long handParkTime;
|
|
|
@@ -14,6 +18,8 @@ bool transferPieriod2;
|
|
|
double traceDevi;
|
|
|
|
|
|
|
|
|
+using namespace iv::decition;
|
|
|
+
|
|
|
namespace iv {
|
|
|
|
|
|
|
|
|
@@ -44,7 +50,7 @@ int ivdecision_brain::getdecision(brain::decition &xdecition)
|
|
|
|
|
|
switch (mvehState) {
|
|
|
case VehState::normalRun:
|
|
|
- return getdecision_normal(xdecition);
|
|
|
+// return getdecision_normal(xdecition);
|
|
|
break;
|
|
|
default:
|
|
|
break;
|
|
|
@@ -54,18 +60,3767 @@ int ivdecision_brain::getdecision(brain::decition &xdecition)
|
|
|
}
|
|
|
|
|
|
|
|
|
-int ivdecision_brain::getdecision_normal(brain::decition &xdecition)
|
|
|
-{
|
|
|
+//int ivdecision_brain::getdecision_normal(brain::decition &xdecition)
|
|
|
+//{
|
|
|
+
|
|
|
+// iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
|
|
|
+
|
|
|
+// double obsDistance = -1;
|
|
|
+// int esrLineIndex = -1;
|
|
|
+// double lidarDistance = -1;
|
|
|
+// int roadPre = -1;
|
|
|
+
|
|
|
+// return 0;
|
|
|
+//}
|
|
|
+
|
|
|
+//std::vector<iv::Perception::PerceptionOutput> lidar_per,
|
|
|
+
|
|
|
|
|
|
+
|
|
|
+//std::vector<iv::Perception::PerceptionOutput> lidar_per,
|
|
|
+iv::decition::Decition ivdecision_brain::getDecideFromGPS(GPS_INS now_gps_ins,
|
|
|
+ const std::vector<GPSData> gpsMapLine,
|
|
|
+ iv::LidarGridPtr lidarGridPtr,
|
|
|
+ std::vector<iv::Perception::PerceptionOutput> lidar_per,
|
|
|
+ iv::TrafficLight trafficLight)
|
|
|
+{
|
|
|
+ // QTime xTime;
|
|
|
+ // xTime.start();
|
|
|
+ //39.14034649083606 117.0863975920476 20507469.630314853 4334165.046101382 353
|
|
|
iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
|
|
|
+ // vector<iv::Point2D> fpTraceTmp;
|
|
|
|
|
|
- double obsDistance = -1;
|
|
|
- int esrLineIndex = -1;
|
|
|
- double lidarDistance = -1;
|
|
|
- int roadPre = -1;
|
|
|
|
|
|
- return 0;
|
|
|
-}
|
|
|
+ //如果useFrenet、useOldAvoid两者不互为相异，则采用原来的方法。“^”为异或运算符。
|
|
|
+ if(!(useFrenet^useOldAvoid)){
|
|
|
+ useFrenet = false;
|
|
|
+ useOldAvoid = true;
|
|
|
+ }
|
|
|
+
|
|
|
+ // //如果useFrenet、useOldAvoid两者均为真或均为假，则采用原来的方法
|
|
|
+ // if(useFrenet&&useOldAvoid || !useFrenet&&!useOldAvoid){
|
|
|
+ // useFrenet = false;
|
|
|
+ // useOldAvoid = true;
|
|
|
+ // }
|
|
|
+
|
|
|
+ // ServiceCarStatus.group_control=false;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // GPS_INS gps= Coordinate_UnTransfer(0,1.5,now_gps_ins);
|
|
|
+ // now_gps_ins.gps_x=gps.gps_x;
|
|
|
+ // now_gps_ins.gps_y=gps.gps_y;
|
|
|
+
|
|
|
+ // GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y,&now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
|
|
|
+
|
|
|
+
|
|
|
+ if (isFirstRun)
|
|
|
+ {
|
|
|
+ initMethods();
|
|
|
+ vehState = normalRun;
|
|
|
+ startAvoid_gps_ins = now_gps_ins;
|
|
|
+ init();
|
|
|
+ PathPoint = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
|
|
|
+ lastGpsIndex,
|
|
|
+ minDis,
|
|
|
+ maxAngle);
|
|
|
+ lastGpsIndex = PathPoint;
|
|
|
+
|
|
|
+ if(ServiceCarStatus.speed_control==true){
|
|
|
+ iv::decition::Compute00().getDesiredSpeed(gpsMapLine); //add by zj
|
|
|
+ }
|
|
|
+
|
|
|
+ // ServiceCarStatus.carState = 1;
|
|
|
+ // roadOri = gpsMapLine[PathPoint]->roadOri;
|
|
|
+ // roadNow = roadOri;
|
|
|
+ // roadSum = gpsMapLine[PathPoint]->roadSum;
|
|
|
+ // busMode = false;
|
|
|
+ // vehState = dRever;
|
|
|
+
|
|
|
+ double dis = iv::decition::GetDistance(*gpsMapLine[0], *gpsMapLine[gpsMapLine.size()-1]);
|
|
|
+ if(dis<15){
|
|
|
+ circleMode=true; //201200102
|
|
|
+ }else{
|
|
|
+ circleMode=false;
|
|
|
+ }
|
|
|
+ // circleMode = true;
|
|
|
+
|
|
|
+
|
|
|
+ getV2XTrafficPositionVector(gpsMapLine);
|
|
|
+ // group_ori_gps=*gpsMapLine[0];
|
|
|
+ ServiceCarStatus.bocheMode=false;
|
|
|
+ ServiceCarStatus.daocheMode=false;
|
|
|
+ parkMode=false;
|
|
|
+ readyParkMode=false;
|
|
|
+ finishPointNum=-1;
|
|
|
+ isFirstRun = false;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(ServiceCarStatus.msysparam.gpsOffset_x!=0 || ServiceCarStatus.msysparam.gpsOffset_y!=0 ){
|
|
|
+ GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
|
|
|
+ now_gps_ins.gps_x=gpsOffset.gps_x;
|
|
|
+ now_gps_ins.gps_y=gpsOffset.gps_y;
|
|
|
+ GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y, &now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
|
|
|
+ }
|
|
|
+
|
|
|
+ //1227
|
|
|
+ // ServiceCarStatus.daocheMode=true;
|
|
|
+
|
|
|
+ //1220
|
|
|
+ changingDangWei=false;
|
|
|
+
|
|
|
+ minDecelerate=0;
|
|
|
+ if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90){
|
|
|
+ // int a=0;
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+
|
|
|
+ gps_decition->accelerator = -0.5;
|
|
|
+ gps_decition->brake=10;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //1220
|
|
|
+
|
|
|
+
|
|
|
+ if(ServiceCarStatus.daocheMode){
|
|
|
+
|
|
|
+ now_gps_ins.ins_heading_angle=now_gps_ins.ins_heading_angle+180;
|
|
|
+ if( now_gps_ins.ins_heading_angle>=360)
|
|
|
+ now_gps_ins.ins_heading_angle= now_gps_ins.ins_heading_angle-360;
|
|
|
+ }
|
|
|
+ //1220 end
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //1125 traficc
|
|
|
+ traffic_light_gps.gps_lat=ServiceCarStatus.iTrafficeLightLat;
|
|
|
+ traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
|
|
|
+ GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);
|
|
|
+
|
|
|
+
|
|
|
+ //xiesi
|
|
|
+ ///kkcai, 2020-01-03
|
|
|
+ //ServiceCarStatus.busmode=true;
|
|
|
+ //ServiceCarStatus.busmode=false;//20200102
|
|
|
+ ///////////////////////////////////////////////////
|
|
|
+
|
|
|
+
|
|
|
+ //busMode = ServiceCarStatus.busmode;
|
|
|
+ nearStation=false;
|
|
|
+
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ // qDebug("heading is %g",now_gps_ins.ins_heading_angle);
|
|
|
+
|
|
|
+
|
|
|
+ aim_gps_ins.gps_lat= ServiceCarStatus.mfParkLat;
|
|
|
+ aim_gps_ins.gps_lng= ServiceCarStatus.mfParkLng;
|
|
|
+
|
|
|
+ aim_gps_ins.ins_heading_angle= ServiceCarStatus.mfParkHeading;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ GaussProjCal(aim_gps_ins.gps_lng,aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
|
|
|
+ is_arrivaled = false;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // xiuzhengCs=-0.8; //1026
|
|
|
+
|
|
|
+ xiuzhengCs=0;
|
|
|
+ // if (parkMode)
|
|
|
+ // {
|
|
|
+
|
|
|
+
|
|
|
+ // handBrakePark(gps_decition,10000,now_gps_ins);
|
|
|
+ // return gps_decition;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ realspeed = now_gps_ins.speed;
|
|
|
+
|
|
|
+ secSpeed = realspeed / 3.6;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //sidePark
|
|
|
+
|
|
|
+ if(ServiceCarStatus.mnParkType==1){
|
|
|
+
|
|
|
+ if( vehState!=dRever && vehState!=dRever0 && vehState!=dRever1 && vehState!=dRever2 && vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr ){
|
|
|
+ int bocheEN=iv::decition::Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
|
|
|
+ if(bocheEN==1){
|
|
|
+ ServiceCarStatus.bocheEnable=1;
|
|
|
+ }else if(bocheEN==0){
|
|
|
+ ServiceCarStatus.bocheEnable=0;
|
|
|
+ }
|
|
|
+ }else{
|
|
|
+ ServiceCarStatus.bocheEnable=2;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(ServiceCarStatus.bocheMode && vehState!=dRever && vehState!=dRever0 && vehState!=dRever1 &&
|
|
|
+ vehState!=dRever2 && vehState!=dRever3 && vehState!=dRever4&& vehState!=reverseArr ){
|
|
|
+ if(abs(realspeed)>0.1){
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ gps_decition->wheel_angle=0;
|
|
|
+ return gps_decition;
|
|
|
+ }else{
|
|
|
+ if(trumpet()>3000){
|
|
|
+ trumpetFirstCount=true;
|
|
|
+ vehState=dRever;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ // ServiceCarStatus.bocheMode=false;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ //chuizhiPark
|
|
|
+
|
|
|
+ if(ServiceCarStatus.mnParkType==0){
|
|
|
+ if( vehState!=reverseCar && vehState!=reversing && vehState!=reverseCircle && vehState!=reverseDirect && vehState!=reverseArr ){
|
|
|
+ int bocheEN=iv::decition::Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
|
|
|
+ if(bocheEN==1){
|
|
|
+ ServiceCarStatus.bocheEnable=1;
|
|
|
+ }else if(bocheEN==0){
|
|
|
+ ServiceCarStatus.bocheEnable=0;
|
|
|
+ }
|
|
|
+ }else{
|
|
|
+ ServiceCarStatus.bocheEnable=2;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(ServiceCarStatus.bocheMode && vehState!=reverseCar && vehState!=reversing && vehState!=reverseCircle && vehState!=reverseDirect&& vehState!=reverseArr ){
|
|
|
+
|
|
|
+ if(abs(realspeed)>0.1){
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ gps_decition->wheel_angle=0;
|
|
|
+ return gps_decition;
|
|
|
+ }else{
|
|
|
+ if(trumpet()>3000){
|
|
|
+ trumpetFirstCount=true;
|
|
|
+ vehState=reverseCar;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ // ServiceCarStatus.bocheMode=false;
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // ChuiZhiTingChe
|
|
|
+ if (vehState == reverseCar)
|
|
|
+ {
|
|
|
+ iv::decition::Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
|
|
|
+
|
|
|
+ vehState = reversing;
|
|
|
+ qiedianCount=true;
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ if (vehState == reversing)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, iv::decition::Compute00().nearTpoint);
|
|
|
+ if (dis<2.0)//0.5
|
|
|
+ {
|
|
|
+ vehState = reverseCircle;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ controlAng = 0;
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ // gps_decition->accelerator = 0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (vehState == reverseCircle)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, iv::decition::Compute00().farTpoint);
|
|
|
+ double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
|
|
|
+ if((((angdis<5)||(angdis>355)))&&(dis<3.0))
|
|
|
+ // if((((angdis<4)||(angdis>356)))&&(dis<2.0))
|
|
|
+ {
|
|
|
+ vehState = reverseDirect;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ if (qiedianCount && trumpet()<0)
|
|
|
+ // if (qiedianCount && trumpet()<1500)
|
|
|
+ {
|
|
|
+
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+
|
|
|
+
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ controlAng = iv::decition::Compute00().bocheAngle*16.5;
|
|
|
+ gps_decition->wheel_angle = 0 - controlAng*1.05;
|
|
|
+
|
|
|
+ cout<<"farTpointDistance================"<<dis<<endl;
|
|
|
+
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (vehState == reverseDirect)
|
|
|
+ {
|
|
|
+ // double dis = GetDistance(now_gps_ins, aim_gps_ins);
|
|
|
+ Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
|
|
|
+
|
|
|
+ if ((pt.y)<0.5)
|
|
|
+ {
|
|
|
+
|
|
|
+ qiedianCount=true;
|
|
|
+ vehState=reverseArr;
|
|
|
+ cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ // gps_decition->accelerator = -3;
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ gps_decition->wheel_angle=0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ return gps_decition;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+
|
|
|
+ //if (qiedianCount && trumpet()<0)
|
|
|
+ if (qiedianCount && trumpet()<1000)
|
|
|
+ {
|
|
|
+
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+ dSpeed = 1;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ controlAng = 0;
|
|
|
+
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState == reverseArr)
|
|
|
+ {
|
|
|
+ //
|
|
|
+
|
|
|
+ ServiceCarStatus.bocheMode=false;
|
|
|
+ if (qiedianCount && trumpet()<1500)
|
|
|
+ {
|
|
|
+
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ ServiceCarStatus.bocheMode=false;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+ ServiceCarStatus.bocheEnable=0;
|
|
|
+ vehState=normalRun;
|
|
|
+ ServiceCarStatus.mbRunPause=true;
|
|
|
+ ServiceCarStatus.mnBocheComplete=100;
|
|
|
+ cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ gps_decition->wheel_angle = 0 ;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ return gps_decition;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //ceFangWeiBoChe
|
|
|
+
|
|
|
+ if (vehState == dRever)
|
|
|
+ {
|
|
|
+ GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
|
|
|
+
|
|
|
+ iv::decition::Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
|
|
|
+
|
|
|
+ vehState = dRever0;
|
|
|
+ qiedianCount=true;
|
|
|
+
|
|
|
+
|
|
|
+ std::cout<<"enter side boche mode"<<std::endl;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ if (vehState == dRever0)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint0);
|
|
|
+ if (dis<2.0)
|
|
|
+ {
|
|
|
+ vehState = dRever1;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ controlAng = 0;
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ // gps_decition->accelerator = 0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState == dRever1)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint1);
|
|
|
+
|
|
|
+ if(dis<2.0)
|
|
|
+ {
|
|
|
+ vehState = dRever2;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ if (qiedianCount && trumpet()<1000)
|
|
|
+
|
|
|
+ {
|
|
|
+
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+
|
|
|
+
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ controlAng = iv::decition::Compute00().dBocheAngle*16.5;
|
|
|
+ gps_decition->wheel_angle = 0 - controlAng;
|
|
|
+
|
|
|
+ cout<<"farTpointDistance================"<<dis<<endl;
|
|
|
+
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState == dRever2)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint2);
|
|
|
+ Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
|
|
|
+ Point2D pt2 = Coordinate_Transfer(iv::decition::Compute00().dTpoint2.gps_x,iv::decition::Compute00().dTpoint2.gps_y, aim_gps_ins);
|
|
|
+ double xx= (pt1.x-pt2.x);
|
|
|
+ // if(xx>0)
|
|
|
+ if(xx>-0.5)
|
|
|
+ {
|
|
|
+ GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
|
|
|
+ Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
|
|
|
+ double xxx=ptt.x;
|
|
|
+ double yyy =ptt.y;
|
|
|
+ // if(xxx<-1.5||xx>1){
|
|
|
+ // int a=0;
|
|
|
+ // }
|
|
|
+ vehState = dRever3;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ cout<<"xxx+++++++++++++++++++++++++++++++++"<<xxx<<endl;
|
|
|
+ cout<<"yyy+++++++++++++++++++++++++++++++++"<<yyy<<endl;
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ if (qiedianCount && trumpet()<1000)
|
|
|
+
|
|
|
+ {
|
|
|
+
|
|
|
+ /* gps_decition->brake = 10;
|
|
|
+ gps_decition->torque = 0; */
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+
|
|
|
+
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ gps_decition->wheel_angle = 0 ;
|
|
|
+
|
|
|
+ cout<<"farTpointDistance================"<<dis<<endl;
|
|
|
+
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState == dRever3)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint3);
|
|
|
+ double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
|
|
|
+ if((((angdis<5)||(angdis>355)))&&(dis<10.0))
|
|
|
+ {
|
|
|
+ vehState = dRever4;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ if (qiedianCount && trumpet()<1000)
|
|
|
+
|
|
|
+ {
|
|
|
+
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+
|
|
|
+
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ controlAng = 0-iv::decition::Compute00().dBocheAngle*16.5;
|
|
|
+ gps_decition->wheel_angle = 0 - controlAng*0.95;
|
|
|
+
|
|
|
+ cout<<"farTpointDistance================"<<dis<<endl;
|
|
|
+
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState == dRever4)
|
|
|
+ {
|
|
|
+ // double dis = GetDistance(now_gps_ins, aim_gps_ins);
|
|
|
+ Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
|
|
|
+
|
|
|
+ if ((pt.y)<0.5)
|
|
|
+ {
|
|
|
+
|
|
|
+ qiedianCount=true;
|
|
|
+ vehState=reverseArr;
|
|
|
+ cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ // gps_decition->accelerator = -3;
|
|
|
+ // gps_decition->brake =10 ;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+
|
|
|
+ gps_decition->wheel_angle=0;
|
|
|
+ return gps_decition;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+
|
|
|
+ //if (qiedianCount && trumpet()<0)
|
|
|
+ if (qiedianCount && trumpet()<1000)
|
|
|
+ {
|
|
|
+
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ controlAng = 0;
|
|
|
+
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState == reverseArr)
|
|
|
+ {
|
|
|
+ //
|
|
|
+ ServiceCarStatus.bocheMode=false;
|
|
|
+
|
|
|
+ if (qiedianCount && trumpet()<1500)
|
|
|
+ {
|
|
|
+
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+ ServiceCarStatus.bocheEnable=0;
|
|
|
+ vehState=normalRun;
|
|
|
+ ServiceCarStatus.mbRunPause=true;
|
|
|
+ ServiceCarStatus.mnBocheComplete=100;
|
|
|
+ cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ gps_decition->wheel_angle = 0 ;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ return gps_decition;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ obsDistance = -1;
|
|
|
+ esrIndex = -1;
|
|
|
+ lidarDistance = -1;
|
|
|
+
|
|
|
+ obsDistanceAvoid = -1;
|
|
|
+ esrIndexAvoid = -1;
|
|
|
+ roadPre = -1;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ gpsTraceOri.clear();
|
|
|
+ gpsTraceNow.clear();
|
|
|
+ gpsTraceAim.clear();
|
|
|
+ gpsTraceAvoid.clear();
|
|
|
+ gpsTraceBack.clear();
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (!isFirstRun)
|
|
|
+ {
|
|
|
+ // PathPoint = iv::decition::Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, maxAngle);
|
|
|
+ // if(PathPoint<0){
|
|
|
+ PathPoint = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, maxAngle);
|
|
|
+ // }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (PathPoint<0)
|
|
|
+ {
|
|
|
+
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
|
|
|
+ return gps_decition;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ lastGpsIndex = PathPoint;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //2020-01-03, kkcai
|
|
|
+ //if(!circleMode && PathPoint>gpsMapLine.size()-200){
|
|
|
+ if(!circleMode && PathPoint>gpsMapLine.size()-100){
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ std::cout<<"map finish brake"<<std::endl;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(!ServiceCarStatus.inRoadAvoid){
|
|
|
+ roadOri = gpsMapLine[PathPoint]->roadOri;
|
|
|
+ roadSum = gpsMapLine[PathPoint]->roadSum;
|
|
|
+ }else{
|
|
|
+ roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
|
|
|
+ roadSum = gpsMapLine[PathPoint]->roadSum*3;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ServiceCarStatus.avoidObs){
|
|
|
+ gpsMapLine[PathPoint]->runMode=1;
|
|
|
+ }else{
|
|
|
+ gpsMapLine[PathPoint]->runMode=0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(roadNowStart){
|
|
|
+ roadNow=roadOri;
|
|
|
+ roadNowStart=false;
|
|
|
+ }
|
|
|
+
|
|
|
+ // avoidX = (roadOri-roadNow)*roadWidth;
|
|
|
+ avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
|
|
|
+
|
|
|
+
|
|
|
+ if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
|
|
|
+ ||vehState == avoiding||vehState == backOri||vehState ==preBack||vehState ==waitAvoid ) )
|
|
|
+ {
|
|
|
+ vehState = normalRun;
|
|
|
+ roadNow = roadOri;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // if (PathPoint!=-1&&((now_gps_ins.rtk_status==6)||(now_gps_ins.rtk_status==5))&&GetDistance(now_gps_ins,*gpsMapLine[PathPoint])<30)
|
|
|
+ // {
|
|
|
+ // lastGpsIndex = PathPoint;
|
|
|
+ // gpsMissCount = 0;
|
|
|
+
|
|
|
+ // }
|
|
|
+ // else
|
|
|
+ // {
|
|
|
+ // gpsMissCount++;
|
|
|
+ // }
|
|
|
+
|
|
|
+ gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
|
|
|
+ // gpsTraceOri = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
|
|
|
+ if(gpsTraceOri.empty()){
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+
|
|
|
+ gps_decition->accelerator = -0.5;
|
|
|
+ gps_decition->brake=10;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ traceDevi=gpsTraceOri[0].x;
|
|
|
+
|
|
|
+ //起步进行frenet规划后，车辆行驶至地图路线上，起步操作完毕。
|
|
|
+ //如果车辆本身启动位置在地图路线上，可不用进行frenet规划。
|
|
|
+ // if(GetDistance(now_gps_ins,*gpsMapLine[PathPoint]) < 1){
|
|
|
+ // startingFlag = false;
|
|
|
+ // }
|
|
|
+ startingFlag = false;
|
|
|
+ if(startingFlag){
|
|
|
+ // gpsTraceAim = gpsTraceOri;
|
|
|
+ if(abs(gpsTraceOri[0].x)>1){
|
|
|
+ cout<<"frenetPlanner->getPath:pre"<<endl;
|
|
|
+ gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
|
|
|
+ cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
|
|
|
+ if(gpsTraceNow.size()==0){
|
|
|
+ gps_decition->accelerator = 0;
|
|
|
+ gps_decition->brake=10;
|
|
|
+ gps_decition->wheel_angle = lastAngle;
|
|
|
+ gps_decition->speed = 0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }else{
|
|
|
+ startingFlag = false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(useFrenet){
|
|
|
+ //如果车辆在变道中，启用frenet规划局部路径
|
|
|
+ if(vehState == avoiding || vehState == backOri){
|
|
|
+ //avoidX = (roadOri - roadNow)*roadWidth;
|
|
|
+ avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
|
|
|
+ gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
|
|
|
+ //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
|
|
|
+ gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ if(gpsTraceNow.size()==0){
|
|
|
+ gps_decition->accelerator = 0;
|
|
|
+ gps_decition->brake=10;
|
|
|
+ gps_decition->wheel_angle = lastAngle;
|
|
|
+ gps_decition->speed = 0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(gpsTraceNow.size()==0){
|
|
|
+ if (roadNow==roadOri)
|
|
|
+ {
|
|
|
+ gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
|
|
|
+ //gpsTraceNow = getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex);
|
|
|
+ }else
|
|
|
+ {
|
|
|
+ // gpsTraceNow = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-roadNow))], lastGpsIndex);
|
|
|
+ gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ //gpsTraceNow = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ // dSpeed = getSpeed(gpsTraceNow);
|
|
|
+ dSpeed = 80;
|
|
|
+
|
|
|
+ // planTrace = gpsTraceNow; //Add By YuChuli, 2020.11.26
|
|
|
+
|
|
|
+ planTrace.clear();
|
|
|
+ for(int i=0;i<gpsTraceNow.size();i++){
|
|
|
+ TracePoint pt(gpsTraceNow[i].x,gpsTraceNow[i].y);
|
|
|
+ planTrace.push_back(pt);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ dSpeed = limitSpeed(controlAng, dSpeed);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
|
|
|
+ if(!circleMode && PathPoint>(gpsMapLine.size()-100)){
|
|
|
+ controlAng=0;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //1220
|
|
|
+ if(ServiceCarStatus.daocheMode){
|
|
|
+ controlAng=0-controlAng;
|
|
|
+ }
|
|
|
+
|
|
|
+ //2020-0106
|
|
|
+ if(vehState==avoiding){
|
|
|
+ controlAng=max(-150.0,controlAng);
|
|
|
+ controlAng=min(150.0,controlAng);
|
|
|
+ }
|
|
|
+ if(vehState==backOri){
|
|
|
+ controlAng=max(-120.0,controlAng);
|
|
|
+ controlAng=min(120.0,controlAng);
|
|
|
+ }
|
|
|
+
|
|
|
+ //准备驻车
|
|
|
+
|
|
|
+ if (readyZhucheMode)
|
|
|
+ {
|
|
|
+
|
|
|
+ cout<<"ready ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ cout<<"zhuche point : "<<zhuchePointNum<<endl;
|
|
|
+
|
|
|
+ double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);
|
|
|
+
|
|
|
+ if (dis<35)
|
|
|
+ {
|
|
|
+ Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
|
|
|
+ double zhucheDistance = pt.y;
|
|
|
+#if 0
|
|
|
+
|
|
|
+ if (dSpeed > 15)
|
|
|
+ {
|
|
|
+ dSpeed = 15;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (zhucheDistance < 20 && dis < 25)
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed, 5.0);
|
|
|
+ }
|
|
|
+#else
|
|
|
+ if (dSpeed > 12)
|
|
|
+ {
|
|
|
+ dSpeed = 12;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (zhucheDistance < 9 && dis < 9)
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed, 5.0);
|
|
|
+ }
|
|
|
+#endif
|
|
|
+
|
|
|
+
|
|
|
+ if (zhucheDistance < 3.0 && dis < 3.5)
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed, 5.0);
|
|
|
+ zhucheMode = true;
|
|
|
+ readyZhucheMode = false;
|
|
|
+ cout<<"jinru ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+
|
|
|
+ //1125
|
|
|
+ // gps_decition->brake=20;
|
|
|
+ // gps_decition->accelerator = -3;
|
|
|
+ // gps_decition->wheel_angle = 0-controlAng;
|
|
|
+ // gps_decition->speed = 0;
|
|
|
+ // gps_decition->torque=0;
|
|
|
+ // return gps_decition;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (readyParkMode)
|
|
|
+ {
|
|
|
+ double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
|
|
|
+ hasZhuched = true;
|
|
|
+
|
|
|
+ if (parkDis < 25)
|
|
|
+ {
|
|
|
+ Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
|
|
|
+ double parkDistance = pt.y;
|
|
|
+
|
|
|
+
|
|
|
+ if (dSpeed > 15)
|
|
|
+ {
|
|
|
+ dSpeed = 15;
|
|
|
+ }
|
|
|
+
|
|
|
+ //dSpeed = 15;//////////////////////////////
|
|
|
+
|
|
|
+ if (parkDistance < 15 && parkDistance >= 12.5 && parkDis<20)
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed, 8.0);
|
|
|
+ }else if (parkDistance < 12.5 && parkDistance >= 8.5 && parkDis<15.0){
|
|
|
+ dSpeed = min(dSpeed, 5.0);
|
|
|
+ }else if (parkDistance < 8.5 && parkDistance >= 5.5 && parkDis<9.0){
|
|
|
+ dSpeed = min(dSpeed, 3.0);
|
|
|
+ }else if(parkDistance < 5.5 && parkDis<6.0){
|
|
|
+ dSpeed = min(dSpeed, 1.0);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ // float stopDis=2;
|
|
|
+ // if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
|
|
|
+ // stopDis=1.6;
|
|
|
+ // } else if(ServiceCarStatus.msysparam.mvehtype=="vv7"){
|
|
|
+ // stopDis=1.8;
|
|
|
+ // }
|
|
|
+
|
|
|
+ if (parkDistance < 1.6 && parkDis<2.0)
|
|
|
+ {
|
|
|
+ // dSpeed = 0;
|
|
|
+ parkMode = true;
|
|
|
+ readyParkMode = false;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (gpsMapLine[PathPoint]->roadMode ==0)
|
|
|
+ {
|
|
|
+ //dSpeed = min(10.0,dSpeed);
|
|
|
+
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+
|
|
|
+ }else if (gpsMapLine[PathPoint]->roadMode == 5)
|
|
|
+ {
|
|
|
+ //dSpeed = min(8.0,dSpeed);
|
|
|
+
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+
|
|
|
+ }else if (gpsMapLine[PathPoint]->roadMode == 11)
|
|
|
+ {
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+
|
|
|
+ }else if (gpsMapLine[PathPoint]->roadMode == 14)
|
|
|
+ {
|
|
|
+ //dSpeed = min(8.0,dSpeed);
|
|
|
+
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
|
|
|
+ gps_decition->leftlamp = true;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }else if (gpsMapLine[PathPoint]->roadMode == 15)
|
|
|
+ {
|
|
|
+ //dSpeed = min(8.0,dSpeed);
|
|
|
+
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = true;
|
|
|
+ }else if (gpsMapLine[PathPoint]->roadMode == 16)
|
|
|
+ {
|
|
|
+ // dSpeed = min(10.0,dSpeed);
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
|
|
|
+ //gps_decition->leftlamp = true;
|
|
|
+ //gps_decition->rightlamp = false;
|
|
|
+ }else if (gpsMapLine[PathPoint]->roadMode == 17)
|
|
|
+ {
|
|
|
+ // dSpeed = min(10.0,dSpeed);
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
|
|
|
+ //gps_decition->leftlamp = false;
|
|
|
+ //gps_decition->rightlamp = true;
|
|
|
+ }else if (gpsMapLine[PathPoint]->roadMode == 18)
|
|
|
+ {
|
|
|
+ // dSpeed = min(10.0,dSpeed);
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+
|
|
|
+ /*if(gpsMapLine[PathPoint]->light_left_or_right == 1)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = true;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ else if(gpsMapLine[PathPoint]->light_left_or_right == 2)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = true;
|
|
|
+ }*/
|
|
|
+ }else if (gpsMapLine[PathPoint]->roadMode == 1)
|
|
|
+ {
|
|
|
+ dSpeed = min(10.0,dSpeed);
|
|
|
+ }else if (gpsMapLine[PathPoint]->roadMode == 2)
|
|
|
+ {
|
|
|
+ dSpeed = min(15.0,dSpeed);
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 7)
|
|
|
+ {
|
|
|
+ dSpeed = min(15.0,dSpeed);
|
|
|
+ xiuzhengCs=1.5;
|
|
|
+ }else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
|
|
|
+ {
|
|
|
+ dSpeed = min(4.0,dSpeed);
|
|
|
+
|
|
|
+ }else{
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (gpsMapLine[PathPoint]->speed_mode == 2)
|
|
|
+ {
|
|
|
+ dSpeed = min(25.0,dSpeed);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ if((gpsMapLine[PathPoint]->speed)>0.001)
|
|
|
+ {
|
|
|
+ dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ std::cout<<"juecesudu2="<<dSpeed<<std::endl;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(vehState==changingRoad){
|
|
|
+ if(gpsTraceNow[0].x>1.0){
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = true;
|
|
|
+ }else if(gpsTraceNow[0].x<-1.0){
|
|
|
+ gps_decition->leftlamp = true;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }else{
|
|
|
+ vehState==normalRun;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // qDebug("decide0 time is %d",xTime.elapsed());
|
|
|
+
|
|
|
+ //1220
|
|
|
+ if(!ServiceCarStatus.daocheMode){
|
|
|
+ computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
|
|
|
+ }else{
|
|
|
+ gpsTraceRear.clear();
|
|
|
+ for(int i=0;i<gpsTraceNow.size();i++){
|
|
|
+ Point2D pt;
|
|
|
+ pt.x=0-gpsTraceNow[i].x;
|
|
|
+ pt.y=0-gpsTraceNow[i].y;
|
|
|
+ gpsTraceRear.push_back(pt);
|
|
|
+ }
|
|
|
+ computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
|
|
|
+ // obsDistance=-1; //1227
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //old_bz--------------------------------------------------------------------------------------------------
|
|
|
+
|
|
|
+ if (vehState == avoiding)
|
|
|
+ {
|
|
|
+ cout<<"\n==================avoiding=================\n"<<endl;
|
|
|
+ // vehState=normalRun; //1025
|
|
|
+ if (dSpeed > 10) {
|
|
|
+ dSpeed = 10;
|
|
|
+ }
|
|
|
+ //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
|
|
|
+ if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
|
|
|
+ vehState = normalRun;
|
|
|
+ // useFrenet = false;
|
|
|
+ // useOldAvoid = true;
|
|
|
+ }
|
|
|
+ else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
|
|
|
+ // vehState = avoidObs; 0929
|
|
|
+ vehState = normalRun;
|
|
|
+ }
|
|
|
+ else if (gpsTraceNow[0].x>0)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = true;
|
|
|
+ }
|
|
|
+ else if(gpsTraceNow[0].x<0)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = true;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState==preBack)
|
|
|
+ {
|
|
|
+ vehState = backOri;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState==backOri)
|
|
|
+ {
|
|
|
+ cout<<"\n================backOri===========\n"<<endl;
|
|
|
+ // vehState=normalRun; //1025
|
|
|
+ if (dSpeed > 10) {
|
|
|
+ dSpeed = 10;
|
|
|
+ }
|
|
|
+
|
|
|
+ //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
|
|
|
+ if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
|
|
|
+ vehState = normalRun;
|
|
|
+ // useFrenet = false;
|
|
|
+ // useOldAvoid = true;
|
|
|
+ }
|
|
|
+ else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
|
|
|
+ // vehState = avoidObs; 0929
|
|
|
+ vehState = normalRun;
|
|
|
+ }
|
|
|
+ else if (gpsTraceNow[0].x>0)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = true;
|
|
|
+ }
|
|
|
+ else if (gpsTraceNow[0].x<0)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = true;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
|
|
|
+
|
|
|
+ cout<<"\n当前RoadNum:%d\n"<<roadNow<<endl;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // 计算回归原始路线
|
|
|
+
|
|
|
+ if (roadNow!=roadOri)
|
|
|
+ {
|
|
|
+ // useFrenet = true;
|
|
|
+ // useOldAvoid = false;
|
|
|
+
|
|
|
+ if(useFrenet){
|
|
|
+ if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
|
|
|
+ {
|
|
|
+ double offsetL = -(roadSum - 1)*roadWidth;
|
|
|
+ double offsetR = (roadOri - 0)*roadWidth;
|
|
|
+ roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(useOldAvoid){
|
|
|
+ roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
|
|
|
+ // avoidX = (roadOri - roadNow)*roadWidth; //1012
|
|
|
+ avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (roadNow != roadOri && roadPre!=-1)
|
|
|
+ {
|
|
|
+
|
|
|
+ roadNow = roadPre;
|
|
|
+ // avoidX = (roadOri - roadNow)*roadWidth;
|
|
|
+ avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
|
|
|
+ gpsTraceNow.clear();
|
|
|
+ if(useOldAvoid){
|
|
|
+ gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ }
|
|
|
+ else if(useFrenet){
|
|
|
+ gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
|
|
|
+ }
|
|
|
+
|
|
|
+ vehState = backOri;
|
|
|
+ hasCheckedBackLidar = false;
|
|
|
+ // checkBackObsTimes = 0;
|
|
|
+
|
|
|
+ cout<<"\n回归RoadNum:%d\n"<< roadNow<<endl;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ //shiyanbizhang 0929
|
|
|
+ if(obsDistance>0&& (obsDistance<ServiceCarStatus.socfDis+20.0) &&(abs(realspeed)<1.0) &&
|
|
|
+ (vehState==normalRun||vehState==backOri || vehState==avoiding) &&(ObsTimeStart==-1)
|
|
|
+ && (gpsMapLine[PathPoint]->runMode==1)){
|
|
|
+ ObsTimeStart=GetTickCount();
|
|
|
+ cout<<"\n====================preAvoid time count start==================\n"<<endl;
|
|
|
+ }
|
|
|
+ if(ObsTimeStart!=-1){
|
|
|
+ ObsTimeEnd=GetTickCount();
|
|
|
+ }
|
|
|
+ if(ObsTimeEnd!=-1){
|
|
|
+ ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
|
|
|
+ }
|
|
|
+ if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
|
|
|
+ vehState=avoidObs;
|
|
|
+ ObsTimeStart=-1;
|
|
|
+ ObsTimeEnd=-1;
|
|
|
+ ObsTimeSpan=-1;
|
|
|
+ cout<<"\n====================preAvoid time count finish==================\n"<<endl;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(obsDistance<0 ||obsDistance>ServiceCarStatus.socfDis){
|
|
|
+ ObsTimeStart=-1;
|
|
|
+ ObsTimeEnd=-1;
|
|
|
+ ObsTimeSpan=-1;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ //避障模式
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState == avoidObs || vehState==waitAvoid ) {
|
|
|
+
|
|
|
+ // if (obsDistance <20 && obsDistance >0)
|
|
|
+ if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
|
|
|
+ {
|
|
|
+ dSpeed = min(7.0,dSpeed);
|
|
|
+ avoidTimes++;
|
|
|
+ // if (avoidTimes>=6)
|
|
|
+ if (avoidTimes>=ServiceCarStatus.aocfTimes)
|
|
|
+ {
|
|
|
+ vehState = preAvoid;
|
|
|
+ avoidTimes = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ // else if (obsDistance < 35 && obsDistance >0 && dSpeed>10)
|
|
|
+ // {
|
|
|
+ // dSpeed = 10;
|
|
|
+ // avoidTimes = 0;
|
|
|
+ // }
|
|
|
+ else if (obsDistance < 40 && obsDistance >0 && dSpeed>15)
|
|
|
+ {
|
|
|
+ dSpeed = min(15.0,dSpeed);
|
|
|
+ avoidTimes = 0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ avoidTimes = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState == preAvoid)
|
|
|
+ {
|
|
|
+ cout<<"\n====================preAvoid==================\n"<<endl;
|
|
|
+ /* if (obsDisVector[roadNow]>30)*/ //if (obsDisVector[roadNow]>30||obsDisVector[roadNow]<0)
|
|
|
+ if (obsDisVector[roadNow]>ServiceCarStatus.aocfDis)
|
|
|
+ {
|
|
|
+ // vehState = avoidObs; 0929
|
|
|
+ vehState=normalRun;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine);
|
|
|
+ if(useOldAvoid){
|
|
|
+ roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine,lidar_per);
|
|
|
+ // avoidX = (roadOri - roadNow)*roadWidth; //1012
|
|
|
+ avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
|
|
|
+ }
|
|
|
+ else if(useFrenet){
|
|
|
+ double offsetL = -(roadSum - 1)*roadWidth;
|
|
|
+ double offsetR = (roadOri - 0)*roadWidth;
|
|
|
+ roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (roadPre == -1 && obsDisVector[roadNow]<1.5 && obsDisVector[roadNow]>0)
|
|
|
+ {
|
|
|
+ // vehState = waitAvoid; 0929
|
|
|
+ vehState = avoidObs;
|
|
|
+ }
|
|
|
+ else if (roadPre != -1)
|
|
|
+ {
|
|
|
+ if(useOldAvoid){
|
|
|
+ roadNow = roadPre;
|
|
|
+ // avoidX = (roadOri - roadNow)*roadWidth;
|
|
|
+ avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
|
|
|
+ gpsTraceNow.clear();
|
|
|
+ gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ }
|
|
|
+ else if(useFrenet){
|
|
|
+ if(roadPre != roadNow){
|
|
|
+ avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
|
|
|
+ startAvoidGpsInsVector[roadNow] = now_gps_ins;
|
|
|
+ }
|
|
|
+ roadNow = roadPre;
|
|
|
+ // avoidX = (roadOri - roadNow)*roadWidth;
|
|
|
+ avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
|
|
|
+ gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ vehState = avoiding;
|
|
|
+
|
|
|
+ hasCheckedAvoidLidar = false;
|
|
|
+
|
|
|
+ cout<<"\n避障到RoadNum:%d\n"<<roadNow<<endl;
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ //--------------------------------------------------------------------------------old_bz_end
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //TOUCHEPINGBI
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (parkMode)
|
|
|
+ {
|
|
|
+ minDecelerate=-0.4;
|
|
|
+
|
|
|
+ if(finishPointNum<0||finishPointNum>=gpsMapLine.size()){
|
|
|
+ parkMode=false;
|
|
|
+ }else if(GetDistance(now_gps_ins, *gpsMapLine[finishPointNum])>4){
|
|
|
+ parkMode=false;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //驻车
|
|
|
+
|
|
|
+ if (zhucheMode)
|
|
|
+ {
|
|
|
+ int mzcTime = ServiceCarStatus.msysparam.mzhuchetime * 1000;
|
|
|
+
|
|
|
+ // if(trumpet()<16000)
|
|
|
+ if (trumpet()<mzcTime)
|
|
|
+ {
|
|
|
+ // if (trumpet()<12000){
|
|
|
+ cout<<"ZHECHE ING STATE+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ if(abs(realspeed)<0.2){
|
|
|
+ controlAng=0; //tju
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ hasZhuched = true; //1125
|
|
|
+ zhucheMode = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+ cout<<"ZHECHE STATE JIESHU +++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ //判断驻车标志位
|
|
|
+ if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
|
|
|
+ {
|
|
|
+ hasZhuched = false;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if ( vehState==changingRoad || vehState==chaocheBack)
|
|
|
+ {
|
|
|
+ double lastAng = 0.0 - lastAngle;
|
|
|
+ if (controlAng>40)
|
|
|
+ {
|
|
|
+ controlAng =40;
|
|
|
+ }
|
|
|
+ else if (controlAng<-40)
|
|
|
+ {
|
|
|
+ controlAng = - 40;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ //速度结合角度的限制
|
|
|
+ controlAng = limitAngle(realspeed, controlAng);
|
|
|
+
|
|
|
+
|
|
|
+ std::cout << "\n呼车指令状态:%d\n" << ServiceCarStatus.carState << std::endl;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //1220
|
|
|
+ if(PathPoint+80<gpsMapLine.size()-1){
|
|
|
+ if(gpsMapLine[PathPoint+80]->roadMode==4 && !ServiceCarStatus.daocheMode){
|
|
|
+ changingDangWei=true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(changingDangWei){
|
|
|
+ if(abs(realspeed)>0.1){
|
|
|
+ dSpeed=0;
|
|
|
+ }else{
|
|
|
+ dSpeed=0;
|
|
|
+ gps_decition->dangWei=2;
|
|
|
+ ServiceCarStatus.daocheMode=true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //1220 end
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ for(int i = 0;i<ServiceCarStatus.car_stations.size();i++)
|
|
|
+ {
|
|
|
+ GPS_INS gpsIns;
|
|
|
+ GaussProjCal(ServiceCarStatus.car_stations[i].longtitude, ServiceCarStatus.car_stations[i].latitude, &gpsIns.gps_x, &gpsIns.gps_y);
|
|
|
+
|
|
|
+ double dis = GetDistance(gpsIns, now_gps_ins);
|
|
|
+ if(dis<20)
|
|
|
+ ServiceCarStatus.currentstation = ServiceCarStatus.car_stations[i].ID;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ // 解析云平台数据
|
|
|
+ if(ServiceCarStatus.stationCmd.received==true)
|
|
|
+ {
|
|
|
+ std::vector<Station> station_received;
|
|
|
+ Station station_aa,station_nearest;
|
|
|
+
|
|
|
+ if(ServiceCarStatus.stationCmd.has_emergencyStop)
|
|
|
+ {
|
|
|
+ if(ServiceCarStatus.stationCmd.emergencyStop==0x01)
|
|
|
+ {
|
|
|
+ //ServiceCarStatus.carState = 0;
|
|
|
+ //busMode=true;
|
|
|
+ ServiceCarStatus.emergencyStop=1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //ServiceCarStatus.carState = 1;
|
|
|
+ //busMode=false;
|
|
|
+ ServiceCarStatus.emergencyStop=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ServiceCarStatus.stationCmd.has_stationStop)
|
|
|
+ {
|
|
|
+ //寻找最近站点
|
|
|
+ station_received.clear();
|
|
|
+ for(int i=0;i<ServiceCarStatus.stationCmd.stationTotalNum;i++)
|
|
|
+ {
|
|
|
+ station_aa.index=i;
|
|
|
+ station_aa.station_location.gps_lat=ServiceCarStatus.stationCmd.stationGps[i].gps_lat;
|
|
|
+ station_aa.station_location.gps_lng=ServiceCarStatus.stationCmd.stationGps[i].gps_lng;
|
|
|
+ GaussProjCal(station_aa.station_location.gps_lng,station_aa.station_location.gps_lat, &station_aa.station_location.gps_x, &station_aa.station_location.gps_y);
|
|
|
+ station_received.push_back(station_aa);
|
|
|
+ }
|
|
|
+ station_nearest=getNearestStation(now_gps_ins,station_received,gpsMapLine);
|
|
|
+
|
|
|
+ qDebug("station_nearest: %d, lat: %.7f, lon: %.7f", station_nearest.map_index, station_nearest.station_location.gps_lat, ServiceCarStatus.amilng=station_nearest.station_location.gps_lng);
|
|
|
+ givlog->debug("brain_v2x","station_nearest: %d, lat: %.7f, lon: %.7f",
|
|
|
+ station_nearest.map_index, station_nearest.station_location.gps_lat,
|
|
|
+ station_nearest.station_location.gps_lng);
|
|
|
+ //进入站点模式
|
|
|
+ if((ServiceCarStatus.stationCmd.stationStop==0x00))
|
|
|
+ {
|
|
|
+ ServiceCarStatus.carState = 2;
|
|
|
+ ServiceCarStatus.amilat=station_nearest.station_location.gps_lat;
|
|
|
+ ServiceCarStatus.amilng=station_nearest.station_location.gps_lng;
|
|
|
+ busMode=true;
|
|
|
+ }else
|
|
|
+ {
|
|
|
+ ServiceCarStatus.carState = 1;
|
|
|
+ busMode=false;
|
|
|
+ ServiceCarStatus.station_control_door=0;
|
|
|
+ ServiceCarStatus.station_control_door_option=false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(ServiceCarStatus.stationCmd.has_carMode)
|
|
|
+ {
|
|
|
+ if(ServiceCarStatus.stationCmd.carMode==0x00)
|
|
|
+ {
|
|
|
+ ServiceCarStatus.stationCmd.mode_manual_drive=true;
|
|
|
+ }else
|
|
|
+ {
|
|
|
+ ServiceCarStatus.stationCmd.mode_manual_drive=false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ ServiceCarStatus.stationCmd.received=false;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //carState == 0，紧急停车
|
|
|
+ if ((ServiceCarStatus.emergencyStop==1)) //||(adjuseultra()==true))
|
|
|
+ {
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (ServiceCarStatus.carState == 3 && busMode)
|
|
|
+ {
|
|
|
+
|
|
|
+ if(realspeed<0.1){
|
|
|
+ ServiceCarStatus.carState=0;
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ }else{
|
|
|
+ nearStation=true;
|
|
|
+ dSpeed=0;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ //carState==2,站点停车
|
|
|
+ if ((ServiceCarStatus.carState==2)&&busMode)
|
|
|
+ {
|
|
|
+
|
|
|
+ aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
|
|
|
+ aim_gps_ins.gps_lng=ServiceCarStatus.amilng;
|
|
|
+
|
|
|
+ GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
|
|
|
+ double dis = GetDistance(aim_gps_ins, now_gps_ins);
|
|
|
+ Point2D pt = Coordinate_Transfer((aim_gps_ins).gps_x, (aim_gps_ins).gps_y, now_gps_ins);
|
|
|
+
|
|
|
+ if (dis<20 && pt.y<5 && abs(pt.x)<3)
|
|
|
+ {
|
|
|
+ dSpeed = 0;
|
|
|
+ nearStation=true;
|
|
|
+ //is_arrivaled = true;
|
|
|
+ //ServiceCarStatus.status[0] = true;
|
|
|
+ ServiceCarStatus.istostation=1;
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ }
|
|
|
+ else if (dis<20 && pt.y<15 && abs(pt.x)<3)
|
|
|
+ {
|
|
|
+ nearStation=true;
|
|
|
+ dSpeed = min(8.0, dSpeed);
|
|
|
+ }
|
|
|
+ else if (dis<30 && pt.y<20 && abs(pt.x)<3)
|
|
|
+ {
|
|
|
+ dSpeed = min(15.0, dSpeed);
|
|
|
+ }
|
|
|
+ else if (dis<50 && abs(pt.x)<3)
|
|
|
+ {
|
|
|
+ dSpeed = min(20.0, dSpeed);
|
|
|
+ }
|
|
|
+
|
|
|
+ if((pt.y<5)&&(abs(realspeed)<0.1)&&(ServiceCarStatus.station_control_door==0))
|
|
|
+ {
|
|
|
+ ServiceCarStatus.station_control_door=1; //open door
|
|
|
+ }
|
|
|
+
|
|
|
+ //站点停车log
|
|
|
+ givlog->debug("brain_v2x","aim_gps_ins.gps_lat: %.7f, aim_gps_ins.gps_lng: %.7f, dis: %.2f, dSpeed: %.1f",
|
|
|
+ aim_gps_ins.gps_lat, aim_gps_ins.gps_lng,
|
|
|
+ dis,dSpeed);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (gpsMapLine[PathPoint]->runMode == 2)
|
|
|
+ {
|
|
|
+ obsDistance = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout<<"juecesudu0="<<dSpeed<<std::endl;
|
|
|
+
|
|
|
+ //-------------------------------traffic light----------------------------------------------------------------------------------------
|
|
|
+
|
|
|
+ if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
|
|
|
+ traffic_light_pathpoint = iv::decition::Compute00().getNoAngleNearestPointIndex(traffic_light_gps, gpsMapLine);
|
|
|
+ // traffic_light_pathpoint=130;
|
|
|
+ float traffic_speed=ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime,gpsMapLine,
|
|
|
+ traffic_light_pathpoint ,PathPoint,secSpeed,dSpeed);
|
|
|
+ dSpeed = min((double)traffic_speed,dSpeed);
|
|
|
+ if(traffic_speed==0){
|
|
|
+ minDecelerate=-2.0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //-------------------------------traffic light end-----------------------------------------------------------------------------------------------
|
|
|
+
|
|
|
+
|
|
|
+ //-------------------------------v2x traffic light-----------------------------------------------------------------------------------------
|
|
|
+
|
|
|
+
|
|
|
+ double v2xTrafficSpeed= ComputeV2XTrafficLightSpeed( trafficLight, gpsMapLine, v2xTrafficVector,
|
|
|
+ PathPoint, secSpeed, dSpeed, circleMode);
|
|
|
+
|
|
|
+
|
|
|
+ dSpeed = min(v2xTrafficSpeed,dSpeed);
|
|
|
+
|
|
|
+ //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
|
|
|
+
|
|
|
+
|
|
|
+ transferGpsMode2(gpsMapLine);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if((ServiceCarStatus.msysparam.mvehtype=="hapo")&&(abs(realspeed)<18)){
|
|
|
+ if(obsDistance>0 && obsDistance<6){
|
|
|
+ dSpeed=0;
|
|
|
+ }
|
|
|
+ }else if(obsDistance>0 && obsDistance<10){
|
|
|
+ dSpeed=0;
|
|
|
+ }
|
|
|
+
|
|
|
+ // obsDistance=-1; //1227
|
|
|
+
|
|
|
+ if(ServiceCarStatus.group_control){
|
|
|
+ dSpeed = ServiceCarStatus.group_comm_speed;
|
|
|
+ }
|
|
|
+ if(dSpeed==0){
|
|
|
+ minDecelerate=min(-1.0f,minDecelerate);
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout<<"dSpeed= "<<dSpeed<<std::endl;
|
|
|
+
|
|
|
+ // givlog->debug("SPEED","dSpeed is %f",dSpeed);
|
|
|
+ gps_decition->wheel_angle = 0.0 - controlAng;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ lastAngle=gps_decition->wheel_angle;
|
|
|
+
|
|
|
+ // qDebug("decide1 time is %d",xTime.elapsed());
|
|
|
+
|
|
|
+ for(int i=0;i<ServiceCarStatus.obsTraceLeft.size();i++){
|
|
|
+ givlog->debug("obs_distance","obsTraceLeftX: %f, obsTraceLeftY: %f",
|
|
|
+ ServiceCarStatus.obsTraceLeft.at(i).x, ServiceCarStatus.obsTraceLeft.at(i).y);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ return gps_decition;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+iv::Station ivdecision_brain::getNearestStation(iv::GPS_INS now_gps_ins,std::vector<Station> station_n,std::vector<GPSData> gpsMap){
|
|
|
+
|
|
|
+ int now_index=0,front_index=0;
|
|
|
+ int station_size=station_n.size();
|
|
|
+
|
|
|
+ for(int i=0;i<station_size;i++)
|
|
|
+ {
|
|
|
+ int minDistance=10;
|
|
|
+ for (int j = 0; j < gpsMap.size(); j++)
|
|
|
+ {
|
|
|
+ double tmpdis = GetDistance(station_n[i].station_location, (*gpsMap[j]));
|
|
|
+ if (tmpdis < minDistance )
|
|
|
+ {
|
|
|
+ minDistance = tmpdis;
|
|
|
+ station_n[i].map_index=j;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ givlog->debug("brain_v2x","stationi: %d, map_index: %d",
|
|
|
+ i,station_n[i].map_index);
|
|
|
+
|
|
|
+ }
|
|
|
+ int minDistance=10;
|
|
|
+ for (int j = 0; j < gpsMap.size(); j++)
|
|
|
+ {
|
|
|
+ double tmpdis = GetDistance(now_gps_ins, (*gpsMap[j]));
|
|
|
+ if (tmpdis < minDistance )
|
|
|
+ {
|
|
|
+ minDistance = tmpdis;
|
|
|
+ now_index=j;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ givlog->debug("brain_v2x","now_index: %d",
|
|
|
+ now_index);
|
|
|
+
|
|
|
+ int min_index=gpsMap.size()-1;
|
|
|
+ int station_index=0;
|
|
|
+ for(int i=0;i<station_size;i++)
|
|
|
+ {
|
|
|
+ if(station_n[i].map_index>now_index)
|
|
|
+ {
|
|
|
+ front_index=station_n[i].map_index;
|
|
|
+ if(front_index<min_index)
|
|
|
+ {
|
|
|
+ min_index=front_index;
|
|
|
+ station_index=i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ qDebug("station_index:%d",station_index);
|
|
|
+
|
|
|
+ return station_n[station_index];
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void ivdecision_brain::initMethods(){
|
|
|
+
|
|
|
+ pid_Controller= new PIDController();
|
|
|
+ ge3_Adapter = new Ge3Adapter();
|
|
|
+ qingyuan_Adapter = new QingYuanAdapter();
|
|
|
+ vv7_Adapter = new VV7Adapter();
|
|
|
+ zhongche_Adapter = new ZhongcheAdapter();
|
|
|
+ bus_Adapter = new BusAdapter();
|
|
|
+ hapo_Adapter = new HapoAdapter();
|
|
|
+ yuhesen_Adapter = new YuHeSenAdapter();
|
|
|
+
|
|
|
+
|
|
|
+ mpid_Controller.reset(pid_Controller);
|
|
|
+ mbus_Adapter.reset(bus_Adapter);
|
|
|
+ mhapo_Adapter.reset(hapo_Adapter);
|
|
|
+ myuhesen_Adapter.reset(yuhesen_Adapter);
|
|
|
+
|
|
|
+ frenetPlanner = new FrenetPlanner();
|
|
|
+ // laneChangePlanner = new LaneChangePlanner();
|
|
|
+
|
|
|
+ mfrenetPlanner.reset(frenetPlanner);
|
|
|
+ // mlaneChangePlanner.reset(laneChangePlanner);
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void ivdecision_brain::phaseSpeedDecition(iv::decition::Decition decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns ) {
|
|
|
+
|
|
|
+ pid_Controller->getControlDecition( gpsIns, gpsTraceNow, dSpeed, obsDis, obsSpeed, false, true, &decition);
|
|
|
+
|
|
|
+ if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
|
|
|
+ ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
|
|
|
+ qingyuan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }else if( ServiceCarStatus.msysparam.mvehtype=="vv7"){
|
|
|
+ vv7_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }else if( ServiceCarStatus.msysparam.mvehtype=="zhongche"){
|
|
|
+ zhongche_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }else if(ServiceCarStatus.msysparam.mvehtype=="bus"){
|
|
|
+ bus_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
|
|
|
+ hapo_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }
|
|
|
+ else if(ServiceCarStatus.msysparam.mvehtype=="yuhesen"){
|
|
|
+ yuhesen_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+std::vector<iv::Point2D> ivdecision_brain::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
|
|
|
+ vector<iv::Point2D> trace;
|
|
|
+ // int PathPoint = iv::decition::Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, maxAngle);
|
|
|
+ /*if (PathPoint != -1)
|
|
|
+ lastGpsIndex = PathPoint;*/
|
|
|
+
|
|
|
+ if (gpsMapLine.size() > 600 && PathPoint >= 0) {
|
|
|
+ int aimIndex;
|
|
|
+ if(circleMode){
|
|
|
+ aimIndex=PathPoint+600;
|
|
|
+ }else{
|
|
|
+ aimIndex=min((PathPoint+600),(int)gpsMapLine.size());
|
|
|
+ }
|
|
|
+
|
|
|
+ for (int i = PathPoint; i < aimIndex; i++)
|
|
|
+ {
|
|
|
+ int index = i % gpsMapLine.size();
|
|
|
+ Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
|
|
|
+ pt.x += offset_real * 0.032;
|
|
|
+ pt.v1 = (*gpsMapLine[index]).speed_mode;
|
|
|
+ pt.v2 = (*gpsMapLine[index]).mode2;
|
|
|
+ pt.roadMode=(*gpsMapLine[index]).roadMode;
|
|
|
+ if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
|
|
|
+ {
|
|
|
+ readyZhucheMode = true;
|
|
|
+ zhuchePointNum = index;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
|
|
|
+ {
|
|
|
+ readyZhucheMode = true;
|
|
|
+ zhuchePointNum = index;
|
|
|
+ }
|
|
|
+
|
|
|
+ //csvv7
|
|
|
+ if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
|
|
|
+ {
|
|
|
+ readyParkMode = true;
|
|
|
+ finishPointNum = index;
|
|
|
+ }
|
|
|
+
|
|
|
+ // if (pt.v1 == 22 || pt.v1 == 23)
|
|
|
+ // {
|
|
|
+ // readyParkMode = true;
|
|
|
+ // finishPointNum = i;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+ switch (pt.v1)
|
|
|
+ {
|
|
|
+ case 0:
|
|
|
+ pt.speed = 80;
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ pt.speed = 25;
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ pt.speed =25;
|
|
|
+ break;
|
|
|
+ case 3:
|
|
|
+ pt.speed = 20;
|
|
|
+ break;
|
|
|
+ case 4:
|
|
|
+ pt.speed =18;
|
|
|
+ break;
|
|
|
+ case 5:
|
|
|
+ pt.speed = 18;
|
|
|
+ break;
|
|
|
+ case 7:
|
|
|
+ pt.speed = 10;
|
|
|
+ break;
|
|
|
+ case 22:
|
|
|
+ pt.speed = 5;
|
|
|
+ break;
|
|
|
+ case 23:
|
|
|
+ pt.speed = 5;
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ trace.push_back(pt);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return trace;
|
|
|
+}
|
|
|
+
|
|
|
+std::vector<iv::Point2D> ivdecision_brain::getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset) {
|
|
|
+
|
|
|
+ if (offset==0)
|
|
|
+ {
|
|
|
+ return gpsTrace;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::vector<iv::Point2D> trace;
|
|
|
+ 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 (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);
|
|
|
+
|
|
|
+ double avoidLenth = abs(offset);
|
|
|
+ if (offset<0)
|
|
|
+ {
|
|
|
+ Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + M_PI / 2),
|
|
|
+ carFronty + avoidLenth * sin(anglevalue + M_PI / 2));
|
|
|
+ ptLeft.speed = gpsTrace[j].speed;
|
|
|
+ ptLeft.v1 = gpsTrace[j].v1;
|
|
|
+ ptLeft.v2 = gpsTrace[j].v2;
|
|
|
+ trace.push_back(ptLeft);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ Point2D ptRight(carFrontx + avoidLenth * cos(anglevalue - M_PI / 2),
|
|
|
+ carFronty + avoidLenth * sin(anglevalue - M_PI / 2));
|
|
|
+ ptRight.speed = gpsTrace[j].speed;
|
|
|
+ ptRight.v1 = gpsTrace[j].v1;
|
|
|
+ ptRight.v2 = gpsTrace[j].v2;
|
|
|
+
|
|
|
+
|
|
|
+ trace.push_back(ptRight);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return trace;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double ivdecision_brain::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
|
|
|
+ double angle=0;
|
|
|
+ if ( abs(minDis) < 20 && abs(maxAngle) < 100)
|
|
|
+ {
|
|
|
+ // angle = iv::decition::iv::decition::Compute00().getDecideAngle(gpsTrace, realspeed);
|
|
|
+ pid_Controller->getControlDecition(gpsIns, gpsTrace, -1, -1, -1, true, false, &decition);
|
|
|
+ angle= decition->wheel_angle;
|
|
|
+ }
|
|
|
+ return angle;
|
|
|
+}
|
|
|
+
|
|
|
+double ivdecision_brain::getSpeed(std::vector<Point2D> gpsTrace) {
|
|
|
+ double speed=0;
|
|
|
+ int speedPoint = iv::decition::Compute00().getSpeedPointIndex(gpsTrace, max(10.0, realspeed));
|
|
|
+ speed = gpsTrace[speedPoint].speed;
|
|
|
+ for (int i = 0; i < speedPoint; i++) {
|
|
|
+ speed = min(speed, gpsTrace[i].speed);
|
|
|
+ }
|
|
|
+ return speed;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//void ivdecision_brain::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
|
|
|
+//
|
|
|
+// if (!obsRadars.empty())
|
|
|
+// {
|
|
|
+// esrIndex = iv::decition::iv::decition::Compute00().getEsrIndex(gpsTrace, obsRadars);
|
|
|
+//
|
|
|
+// if (esrIndex != -1)
|
|
|
+// {
|
|
|
+// esrDistance = obsRadars[esrIndex].nomal_y;
|
|
|
+//
|
|
|
+//
|
|
|
+//
|
|
|
+// obsSpeed = obsRadars[esrIndex].speed_y;
|
|
|
+//
|
|
|
+// }
|
|
|
+// else {
|
|
|
+// esrDistance = -1;
|
|
|
+// }
|
|
|
+//
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// esrIndex = -1;
|
|
|
+// esrDistance = -1;
|
|
|
+// }
|
|
|
+// if (esrDistance < 0) {
|
|
|
+// ODS("\n------------------>ESR障碍物距离:%f\n", esrDistance);
|
|
|
+// }
|
|
|
+// else {
|
|
|
+// ODS("\n------------------>ESR障碍物距离:%f ( %.05f , %.05f )\n", esrDistance, obsRadars[esrIndex].nomal_x, obsRadars[esrIndex].nomal_y);
|
|
|
+// }
|
|
|
+//
|
|
|
+// ODS("\n------------------>ESR障碍物速度:%f\n", obsSpeed);
|
|
|
+//}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void ivdecision_brain::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
|
|
|
+
|
|
|
+
|
|
|
+ int esrPathpoint;
|
|
|
+
|
|
|
+ esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, roadNum, &esrPathpoint,xiuzhengCs);
|
|
|
+
|
|
|
+ if (esrIndex != -1)
|
|
|
+ {
|
|
|
+ //优化
|
|
|
+ // double distance = 0.0;
|
|
|
+ // for(int i=0; i<esrPathpoint; ++i){
|
|
|
+ // distance+=DirectDistance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
|
|
|
+ // }
|
|
|
+ // esrDistance = distance - Esr_Y_Offset;
|
|
|
+ // if(esrDistance<=0){
|
|
|
+ // esrDistance=1;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+ esrDistance = ServiceCarStatus.obs_radar[esrIndex].nomal_y;
|
|
|
+ obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ esrDistance = -1;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void ivdecision_brain::getEsrObsDistanceAvoid() {
|
|
|
+
|
|
|
+ esrIndexAvoid = iv::decition::Compute00().getEsrIndexAvoid(gpsTraceAvoid);
|
|
|
+
|
|
|
+ if (esrIndexAvoid != -1)
|
|
|
+ {
|
|
|
+ esrDistanceAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y;
|
|
|
+ obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ esrDistanceAvoid = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (esrDistanceAvoid < 0) {
|
|
|
+ std::cout << "\nESR障碍物距离Avoid:%f\n" << esrDistanceAvoid << std::endl;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ std::cout << "\nESR障碍物距离Avoid:%f %d:( %.05f , %.05f ,%.05f )\n" << esrDistanceAvoid << esrIndexAvoid << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_x << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y << std::endl;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout << "\nESR障碍物速度Avoid:%f\n" << obsSpeedAvoid <<std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double ivdecision_brain::limitAngle(double speed, double angle) {
|
|
|
+ double preAngle = angle;
|
|
|
+
|
|
|
+ if (speed > 15)
|
|
|
+ {
|
|
|
+ if (preAngle > 350)
|
|
|
+ {
|
|
|
+ preAngle = 350;
|
|
|
+ }
|
|
|
+ if (preAngle < -350)
|
|
|
+ {
|
|
|
+ preAngle = -350;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (speed > 22)
|
|
|
+ {
|
|
|
+ if (preAngle > 200)
|
|
|
+ {
|
|
|
+ preAngle = 200;
|
|
|
+ }
|
|
|
+ if (preAngle < -200)
|
|
|
+ {
|
|
|
+ preAngle = -200;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (speed > 25)
|
|
|
+ {
|
|
|
+ if (preAngle > 150)
|
|
|
+ {
|
|
|
+ preAngle = 150;
|
|
|
+ }
|
|
|
+ if (preAngle < -150)
|
|
|
+ {
|
|
|
+ preAngle = -150;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (speed > 30)
|
|
|
+ {
|
|
|
+ if (preAngle > 70)
|
|
|
+ {
|
|
|
+ preAngle = 70;
|
|
|
+ }
|
|
|
+ if (preAngle < -70)
|
|
|
+ {
|
|
|
+ preAngle = -70;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (speed > 45) //20
|
|
|
+ {
|
|
|
+ if (preAngle > 15)
|
|
|
+ {
|
|
|
+ preAngle = 15;
|
|
|
+ }
|
|
|
+ if (preAngle < -15)
|
|
|
+ {
|
|
|
+ preAngle = -15;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return preAngle;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double ivdecision_brain::limitSpeed(double angle, double speed) {
|
|
|
+
|
|
|
+ if (abs(angle) > 500 && speed > 8) speed = 8;
|
|
|
+ else if (abs(angle) > 350 && speed > 14) speed = 14;
|
|
|
+ else if (abs(angle) > 200 && speed > 21) speed = 21;
|
|
|
+ else if (abs(angle) > 150 && speed > 24) speed = 24;
|
|
|
+ else if (abs(angle) > 60 && speed > 29) speed = 29;
|
|
|
+ else if (abs(angle) > 20 && speed > 34) speed = 34;
|
|
|
+ return max(0.0, speed);
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+bool ivdecision_brain::checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr,int roadNum) {
|
|
|
+
|
|
|
+ if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
|
|
|
+ (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<15))
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (roadNum-roadNow>1)
|
|
|
+ {
|
|
|
+ for (int i = roadNow+1; i < roadNum; i++)
|
|
|
+ {
|
|
|
+ if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ else if (roadNow-roadNum>1)
|
|
|
+ {
|
|
|
+ for (int i = roadNow-1; i >roadNum; i--)
|
|
|
+ {
|
|
|
+ if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+bool ivdecision_brain::checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum) {
|
|
|
+ //lsn
|
|
|
+ if (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<20)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNum]< obsDisVector[roadNow]) ||
|
|
|
+ (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNow]<0))
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (roadNum - roadNow>1)
|
|
|
+ {
|
|
|
+ for (int i = roadNow + 1; i < roadNum; i++)
|
|
|
+ {
|
|
|
+ if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ else if (roadNow - roadNum>1)
|
|
|
+ {
|
|
|
+ for (int i = roadNow - 1; i >roadNum; i--)
|
|
|
+ {
|
|
|
+ if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void ivdecision_brain::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ lidarDistanceAvoid = lastLidarDisAvoid;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ obsPointAvoid = iv::decition::Compute00().getLidarObsPointAvoid(gpsTraceAvoid, lidarGridPtr,lidarDistanceAvoid);
|
|
|
+ lastLidarDisAvoid = lidarDistanceAvoid;
|
|
|
+ }
|
|
|
+ std::cout << "\nLidarAvoid距离:%f\n" << lidarDistanceAvoid << std::endl;
|
|
|
+ getEsrObsDistanceAvoid();
|
|
|
+
|
|
|
+ //lidarDistanceAvoid = -1; //20200103 apollo_fu
|
|
|
+
|
|
|
+ if (esrDistanceAvoid>0 && lidarDistanceAvoid > 0)
|
|
|
+ {
|
|
|
+ if (lidarDistanceAvoid >= esrDistanceAvoid)
|
|
|
+ {
|
|
|
+ obsDistanceAvoid = esrDistanceAvoid;
|
|
|
+ obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
|
|
|
+ }
|
|
|
+ else if (!ServiceCarStatus.obs_radar.empty())
|
|
|
+ {
|
|
|
+ obsDistanceAvoid = lidarDistanceAvoid;
|
|
|
+ obsSpeedAvoid = iv::decition::Compute00().getObsSpeed(obsPointAvoid, secSpeed);
|
|
|
+ std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obsDistanceAvoid = lidarDistanceAvoid;
|
|
|
+ obsSpeedAvoid = obsSpeedAvoid = 0 - secSpeed;;
|
|
|
+ std::cout << "\n毫米波无数据，计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (esrDistanceAvoid>0)
|
|
|
+ {
|
|
|
+ obsDistanceAvoid = esrDistanceAvoid;
|
|
|
+ obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
|
|
|
+ }
|
|
|
+ else if (lidarDistanceAvoid > 0)
|
|
|
+ {
|
|
|
+ obsDistanceAvoid = lidarDistanceAvoid;
|
|
|
+ obsSpeedAvoid = iv::decition::Compute00().getObsSpeed(obsPointAvoid, secSpeed);
|
|
|
+ std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ obsDistanceAvoid = esrDistanceAvoid;
|
|
|
+ obsSpeedAvoid = 0 - secSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout << "\n最终得出的obsDistanceAvoid:%f\n" << obsDistanceAvoid << std::endl;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (obsDistanceAvoid <0 && obsLostTimeAvoid<4)
|
|
|
+ {
|
|
|
+ obsDistanceAvoid = lastDistanceAvoid;
|
|
|
+ obsLostTimeAvoid++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obsLostTimeAvoid = 0;
|
|
|
+ lastDistanceAvoid = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (obsDistanceAvoid>0)
|
|
|
+ {
|
|
|
+ lastDistanceAvoid = obsDistanceAvoid;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout << "\nODSAvoid距离:%f\n" << obsDistanceAvoid << std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+void ivdecision_brain::init() {
|
|
|
+ for (int i = 0; i < roadSum; i++)
|
|
|
+ {
|
|
|
+ lastEsrIdVector.push_back(-1);
|
|
|
+ lastEsrCountVector.push_back(0);
|
|
|
+ GPS_INS gps_ins;
|
|
|
+ gps_ins.gps_x = 0;
|
|
|
+ gps_ins.gps_y = 0;
|
|
|
+ startAvoidGpsInsVector.push_back(gps_ins);
|
|
|
+ avoidMinDistanceVector.push_back(0);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+#include <QTime>
|
|
|
+
|
|
|
+void ivdecision_brain::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
|
|
|
+ const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
|
|
|
+
|
|
|
+ // QTime xTime;
|
|
|
+ // xTime.start();
|
|
|
+ // qDebug("time 0 is %d ",xTime.elapsed());
|
|
|
+ double obs,obsSd;
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ lidarDistance = lastLidarDis;
|
|
|
+ // lidarDistance = lastlidarDistance;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ obsPoint = iv::decition::Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
|
|
|
+ float lidarXiuZheng=0;
|
|
|
+ if(!ServiceCarStatus.useMobileEye){
|
|
|
+ lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
|
|
|
+ }
|
|
|
+ lidarDistance = obsPoint.y + lidarXiuZheng; //激光距离推到车头 gjw20191110 lidar
|
|
|
+ // lidarDistance=-1;
|
|
|
+ if (lidarDistance<0)
|
|
|
+ {
|
|
|
+ lidarDistance = -1;
|
|
|
+ }
|
|
|
+ lastLidarDis = lidarDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ // obsPredict start
|
|
|
+ if(ServiceCarStatus.useObsPredict){
|
|
|
+ float preObsDis=200;
|
|
|
+ if(!lidar_per.empty()){
|
|
|
+ preObsDis=PredictObsDistance( secSpeed, gpsTrace, lidar_per);
|
|
|
+ lastPreObsDistance=preObsDis;
|
|
|
+ }else{
|
|
|
+ preObsDis=lastPreObsDistance;
|
|
|
+ }
|
|
|
+ if(preObsDis<lidarDistance || lidarDistance==-1){
|
|
|
+ lidarDistance=preObsDis;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // obsPredict end
|
|
|
+
|
|
|
+ // qDebug("time 1 is %d ",xTime.elapsed());
|
|
|
+ // if(gpsMapLine[PathPoint]->roadMode==14||gpsMapLine[PathPoint]->roadMode==15){
|
|
|
+ // lidarDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+ getEsrObsDistance(gpsTrace, roadNum);
|
|
|
+
|
|
|
+ double mobieye_ttc,mobieye_obsid,mobieye_speed,mobieye_distance;
|
|
|
+
|
|
|
+ double fveh_width = 2.0;
|
|
|
+#ifdef USE_MOBIEYE_OBS
|
|
|
+ MobieyeInst.GetObsFromMobieye(&gpsTrace,mobieye_distance,mobieye_speed,mobieye_ttc,mobieye_obsid,fveh_width);
|
|
|
+#endif
|
|
|
+ // qDebug("time 2 is %d ",xTime.elapsed());
|
|
|
+ // esrDistance=-1;
|
|
|
+
|
|
|
+ // if(PathPoint>2992 && PathPoint<4134){
|
|
|
+ // lidarDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+ // if(PathPoint>10193 && PathPoint<10929){
|
|
|
+ // esrDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+ if(lidarDistance<0){
|
|
|
+ lidarDistance=500;
|
|
|
+ }
|
|
|
+
|
|
|
+#ifdef USE_MOBIEYE_OBS
|
|
|
+ esrDistance = mobieye_distance;
|
|
|
+
|
|
|
+ if(esrDistance>150){
|
|
|
+ esrDistance=500;
|
|
|
+ }
|
|
|
+#else
|
|
|
+ if(esrDistance>30){
|
|
|
+ esrDistance=500;
|
|
|
+ }
|
|
|
+#endif
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(esrDistance<0){
|
|
|
+ esrDistance=500;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
|
|
|
+
|
|
|
+
|
|
|
+ // if(esrDistance>30){
|
|
|
+ // esrDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // if(esrDistance>=15 && esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0 ){
|
|
|
+ // if(abs(lidarDistance-esrDistance)>5){
|
|
|
+
|
|
|
+ // esrDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // if(esrDistance>=1&& esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0
|
|
|
+ // && gpsMapLine[PathPoint]->runMode == 1 ){
|
|
|
+ // if(abs(lidarDistance-esrDistance)>5){
|
|
|
+
|
|
|
+ // esrDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
|
|
|
+ myesrDistance = esrDistance;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(lidarDistance==500){
|
|
|
+ lidarDistance=-1;
|
|
|
+ }
|
|
|
+ if(esrDistance==500){
|
|
|
+ esrDistance=-1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ ServiceCarStatus.mRadarObs = esrDistance;
|
|
|
+ ServiceCarStatus.mLidarObs = lidarDistance;
|
|
|
+
|
|
|
+
|
|
|
+ //zhuanwan pingbi haomibo
|
|
|
+ if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
|
|
|
+ esrDistance=-1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (esrDistance>0 && lidarDistance > 0)
|
|
|
+ {
|
|
|
+ if (lidarDistance >= esrDistance)
|
|
|
+ {
|
|
|
+#ifdef USE_MOBIEYE_OBS
|
|
|
+ obs = esrDistance;
|
|
|
+ obsSd = mobieye_speed;
|
|
|
+#else
|
|
|
+ // obsDistance = esrDistance;
|
|
|
+ obs = esrDistance;
|
|
|
+ // obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+ obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+#endif
|
|
|
+ }
|
|
|
+ else if (!ServiceCarStatus.obs_radar.empty())
|
|
|
+ {
|
|
|
+ // obsDistance = lidarDistance;
|
|
|
+ obs = lidarDistance;
|
|
|
+ // obsSpeed = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
|
|
|
+ obsSd = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ // obsDistance = lidarDistance;
|
|
|
+ obs=lidarDistance;
|
|
|
+ // obsSpeed = 0 - secSpeed;
|
|
|
+ obsSd = 0 -secSpeed;
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (esrDistance>0)
|
|
|
+ {
|
|
|
+ // obsDistance = esrDistance;
|
|
|
+ // obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+
|
|
|
+#ifdef USE_MOBIEYE_OBS
|
|
|
+ obs = esrDistance;
|
|
|
+ obsSd = mobieye_speed;
|
|
|
+#else
|
|
|
+ obs = esrDistance;
|
|
|
+ obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+#endif
|
|
|
+ }
|
|
|
+ else if (lidarDistance > 0)
|
|
|
+ {
|
|
|
+ // obsDistance = lidarDistance;
|
|
|
+ // obsSpeed = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
|
|
|
+ obs = lidarDistance;
|
|
|
+ obsSd = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ // obsDistance = esrDistance;
|
|
|
+ // obsSpeed = 0 - secSpeed;
|
|
|
+ obs = esrDistance;
|
|
|
+ obsSd = 0 - secSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(roadNum==roadNow){
|
|
|
+ obsDistance=obs;
|
|
|
+ obsSpeed=obsSd;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ // if (obsDistance <0 && obsLostTime<4)
|
|
|
+ // {
|
|
|
+ // obsDistance = lastDistance;
|
|
|
+ // obsLostTime++;
|
|
|
+ // }
|
|
|
+ // else
|
|
|
+ // {
|
|
|
+ // obsLostTime = 0;
|
|
|
+ // lastDistance = -1;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
|
|
|
+
|
|
|
+ ServiceCarStatus.mObs = obsDistance;
|
|
|
+ if(ServiceCarStatus.mObs>100){
|
|
|
+ ServiceCarStatus.mObs =-1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (obsDistance>0)
|
|
|
+ {
|
|
|
+ lastDistance = obsDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ //lsn
|
|
|
+ if(obs<0){
|
|
|
+ obsDisVector[roadNum]=500;
|
|
|
+ }else{
|
|
|
+ obsDisVector[roadNum]=obs;
|
|
|
+ }
|
|
|
+
|
|
|
+ givlog->debug("obs_distance","obs: %f, lidar_obs: %f, Radar_obs: %f, lidar_obs_x: %f, lidar_obs_y: %f",
|
|
|
+ ServiceCarStatus.mObs, ServiceCarStatus.mLidarObs,
|
|
|
+ ServiceCarStatus.mRadarObs,obsPoint.x,lidarDistance);
|
|
|
+
|
|
|
+ // qDebug("time 3 is %d ",xTime.elapsed());
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//1220
|
|
|
+void ivdecision_brain::computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
|
|
|
+ const std::vector<GPSData> gpsMapLine) {
|
|
|
+
|
|
|
+ double obs,obsSd;
|
|
|
+
|
|
|
+ if(!ServiceCarStatus.obs_rear_radar.empty()){
|
|
|
+ getRearEsrObsDistance(gpsTrace, roadNum);
|
|
|
+ }
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ lidarDistance = lastLidarDis;
|
|
|
+ // lidarDistance = lastlidarDistance;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ obsPoint = iv::decition::Compute00().getLidarRearObsPoint(gpsTrace, lidarGridPtr);
|
|
|
+ float lidarXiuZheng=0;
|
|
|
+ if(!ServiceCarStatus.useMobileEye){
|
|
|
+ lidarXiuZheng = ServiceCarStatus.msysparam.rearGpsXiuzheng;
|
|
|
+ }
|
|
|
+ if(abs(obsPoint.y)>lidarXiuZheng)
|
|
|
+ lidarDistance = abs(obsPoint.y)-lidarXiuZheng; //激光距离推到车头 1220
|
|
|
+
|
|
|
+ // lidarDistance=-1;
|
|
|
+ if (lidarDistance<0)
|
|
|
+ {
|
|
|
+ lidarDistance = -1;
|
|
|
+ }
|
|
|
+ lastLidarDis = lidarDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ // if(gpsMapLine[PathPoint]->roadMode==14||gpsMapLine[PathPoint]->roadMode==15){
|
|
|
+ // lidarDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+ // getEsrObsDistance(gpsTrace, roadNum);
|
|
|
+ esrDistance=-1;
|
|
|
+
|
|
|
+
|
|
|
+ // if(PathPoint>2992 && PathPoint<4134){
|
|
|
+ // lidarDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+ // if(PathPoint>10193 && PathPoint<10929){
|
|
|
+ // esrDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+ if(lidarDistance<0){
|
|
|
+ lidarDistance=500;
|
|
|
+ }
|
|
|
+ if(esrDistance<0){
|
|
|
+ esrDistance=500;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
|
|
|
+
|
|
|
+
|
|
|
+ // if(esrDistance>30){
|
|
|
+ // esrDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // if(esrDistance>=15 && esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0 ){
|
|
|
+ // if(abs(lidarDistance-esrDistance)>5){
|
|
|
+
|
|
|
+ // esrDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // if(esrDistance>=1&& esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0
|
|
|
+ // && gpsMapLine[PathPoint]->runMode == 1 ){
|
|
|
+ // if(abs(lidarDistance-esrDistance)>5){
|
|
|
+
|
|
|
+ // esrDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
|
|
|
+ myesrDistance = esrDistance;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(lidarDistance==500){
|
|
|
+ lidarDistance=-1;
|
|
|
+ }
|
|
|
+ if(esrDistance==500){
|
|
|
+ esrDistance=-1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ ServiceCarStatus.mRadarObs = esrDistance;
|
|
|
+ ServiceCarStatus.mLidarObs = lidarDistance;
|
|
|
+
|
|
|
+
|
|
|
+ //zhuanwan pingbi haomibo
|
|
|
+ if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
|
|
|
+ esrDistance=-1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (esrDistance>0 && lidarDistance > 0)
|
|
|
+ {
|
|
|
+ if (lidarDistance >= esrDistance)
|
|
|
+ {
|
|
|
+ // obsDistance = esrDistance;
|
|
|
+ obs = esrDistance;
|
|
|
+ // obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+ obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+ }
|
|
|
+ else if (!ServiceCarStatus.obs_radar.empty())
|
|
|
+ {
|
|
|
+ // obsDistance = lidarDistance;
|
|
|
+ obs = lidarDistance;
|
|
|
+ // obsSpeed = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
|
|
|
+ obsSd = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ // obsDistance = lidarDistance;
|
|
|
+ obs=lidarDistance;
|
|
|
+ // obsSpeed = 0 - secSpeed;
|
|
|
+ obsSd = 0 -secSpeed;
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (esrDistance>0)
|
|
|
+ {
|
|
|
+ // obsDistance = esrDistance;
|
|
|
+ // obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+ obs = esrDistance;
|
|
|
+ obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+ }
|
|
|
+ else if (lidarDistance > 0)
|
|
|
+ {
|
|
|
+ // obsDistance = lidarDistance;
|
|
|
+ // obsSpeed = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
|
|
|
+ obs = lidarDistance;
|
|
|
+ obsSd = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ // obsDistance = esrDistance;
|
|
|
+ // obsSpeed = 0 - secSpeed;
|
|
|
+ obs = esrDistance;
|
|
|
+ obsSd = 0 - secSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(roadNum==roadNow){
|
|
|
+ obsDistance=obs;
|
|
|
+ obsSpeed=obsSd;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (obsDistance <0 && obsLostTime<4)
|
|
|
+ {
|
|
|
+ obsDistance = lastDistance;
|
|
|
+ obsLostTime++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obsLostTime = 0;
|
|
|
+ lastDistance = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
|
|
|
+
|
|
|
+ ServiceCarStatus.mObs = obsDistance;
|
|
|
+ if(ServiceCarStatus.mObs>100){
|
|
|
+ ServiceCarStatus.mObs =-1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (obsDistance>0)
|
|
|
+ {
|
|
|
+ lastDistance = obsDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ //lsn
|
|
|
+ if(obs<0){
|
|
|
+ obsDisVector[roadNum]=500;
|
|
|
+ }else{
|
|
|
+ obsDisVector[roadNum]=obs;
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void ivdecision_brain::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
|
|
|
+ double preObsDis;
|
|
|
+
|
|
|
+ if(!lidar_per.empty()){
|
|
|
+ preObsDis=PredictObsDistance( realSpeed, gpsTrace, lidar_per);
|
|
|
+ lastPreObsDistance=preObsDis;
|
|
|
+
|
|
|
+ }else{
|
|
|
+ preObsDis=lastPreObsDistance;
|
|
|
+ }
|
|
|
+ if(preObsDis<obsDistance){
|
|
|
+ obsDistance=preObsDis;
|
|
|
+ lastDistance=obsDistance;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+int ivdecision_brain::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
|
|
|
+ roadPre = -1;
|
|
|
+
|
|
|
+ // if (roadNow<roadOri)
|
|
|
+ // {
|
|
|
+ // for (int i = 0; i < roadNow; i++)
|
|
|
+ // {
|
|
|
+ // gpsTraceAvoid.clear();
|
|
|
+ // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
|
|
|
+ // avoidX = (roadWidth)*(roadOri - i);
|
|
|
+ // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ // // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
|
|
|
+ // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+ // for (int i = roadOri+1; i < roadSum; i++)
|
|
|
+ // {
|
|
|
+ // gpsTraceAvoid.clear();
|
|
|
+ // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
|
|
|
+ // avoidX = (roadWidth)*(roadOri - i);
|
|
|
+ // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ // // bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
|
|
|
+ // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // else if (roadNow>roadOri)
|
|
|
+ // {
|
|
|
+ // for (int i = 0; i < roadOri; i++)
|
|
|
+ // {
|
|
|
+ // gpsTraceAvoid.clear();
|
|
|
+ // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
|
|
|
+ // avoidX = (roadWidth)*(roadOri - i);
|
|
|
+ // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ // // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
|
|
|
+ // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
|
|
|
+ // }
|
|
|
+ // for (int i = roadNow + 1; i < roadSum; i++)
|
|
|
+ // {
|
|
|
+ // gpsTraceAvoid.clear();
|
|
|
+ // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
|
|
|
+ // avoidX = (roadWidth)*(roadOri - i);
|
|
|
+ // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ // // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
|
|
|
+ // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
|
|
|
+ // }
|
|
|
+
|
|
|
+ // }
|
|
|
+
|
|
|
+ // else
|
|
|
+ // {
|
|
|
+ // for (int i = 0; i < roadOri; i++)
|
|
|
+ // {
|
|
|
+ // gpsTraceAvoid.clear();
|
|
|
+ // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
|
|
|
+ // avoidX = (roadWidth)*(roadOri - i);
|
|
|
+ // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ // // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
|
|
|
+ // computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
|
|
|
+ // }
|
|
|
+ // for (int i = roadOri + 1; i < roadSum; i++)
|
|
|
+ // {
|
|
|
+ // gpsTraceAvoid.clear();
|
|
|
+ // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
|
|
|
+ // avoidX = (roadWidth)*(roadOri - i);
|
|
|
+ // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ // // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
|
|
|
+ // computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
|
|
|
+ // }
|
|
|
+
|
|
|
+ // }
|
|
|
+
|
|
|
+ for (int i = 0; i < roadSum; i++)
|
|
|
+ {
|
|
|
+ gpsTraceAvoid.clear();
|
|
|
+ // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
|
|
|
+ // avoidX = (roadWidth)*(roadOri - i);
|
|
|
+ avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
|
|
|
+ gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
|
|
|
+ computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (lidarGridPtr!=NULL)
|
|
|
+ {
|
|
|
+ hasCheckedAvoidLidar = true;
|
|
|
+ }
|
|
|
+
|
|
|
+ for(int i=0; i<roadSum;i++){
|
|
|
+ std::cout<<"\odsjuli====================:\n"<<i<<"chedaojuli:"<<obsDisVector[i]<<endl;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ checkAvoidObsTimes++;
|
|
|
+ if (checkAvoidObsTimes<4 || hasCheckedAvoidLidar==false)
|
|
|
+ {
|
|
|
+ return - 1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ for (int i = 1; i < roadSum; i++)
|
|
|
+ {
|
|
|
+ if (roadNow + i < roadSum) {
|
|
|
+ // avoidX = (roadOri-roadNow-i)*roadWidth;
|
|
|
+ avoidX=computeAvoidX(roadNow+i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
|
|
|
+ if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow+i))
|
|
|
+ {
|
|
|
+ /*if (roadNow==roadOri)
|
|
|
+ {
|
|
|
+ avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
|
|
|
+ startAvoid_gps_ins = now_gps_ins;
|
|
|
+ } */
|
|
|
+ avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
|
|
|
+ startAvoidGpsInsVector[roadNow] = now_gps_ins;
|
|
|
+ roadPre = roadNow + i;
|
|
|
+ return roadPre;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (roadNow - i >= 0)
|
|
|
+ {
|
|
|
+ // avoidX = (roadOri - roadNow+i)*roadWidth;
|
|
|
+ avoidX=computeAvoidX(roadNow-i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
|
|
|
+ if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow - i))
|
|
|
+ {
|
|
|
+ /*if (roadNow == roadOri)
|
|
|
+ {
|
|
|
+ avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
|
|
|
+ startAvoid_gps_ins = now_gps_ins;
|
|
|
+ }*/
|
|
|
+ avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
|
|
|
+ startAvoidGpsInsVector[roadNow] = now_gps_ins;
|
|
|
+ roadPre = roadNow - i;
|
|
|
+ return roadPre;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ return roadPre;
|
|
|
+}
|
|
|
+
|
|
|
+int ivdecision_brain::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
|
|
|
+ roadPre = -1;
|
|
|
+
|
|
|
+ // computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // if (roadNow>=roadOri+1)
|
|
|
+ // {
|
|
|
+ // for (int i = roadOri+1; i < roadNow; i++)
|
|
|
+ // {
|
|
|
+ // gpsTraceBack.clear();
|
|
|
+ // // gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
|
|
|
+ // avoidX = (roadWidth)*(roadOri - i);
|
|
|
+ // gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ // computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // else if (roadNow <= roadOri - 1) {
|
|
|
+
|
|
|
+ // for (int i = roadOri - 1; i > roadNow; i--)
|
|
|
+ // {
|
|
|
+ // gpsTraceBack.clear();
|
|
|
+ // // gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
|
|
|
+ // avoidX = (roadWidth)*(roadOri - i);
|
|
|
+ // gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ // computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
|
|
|
+ // }
|
|
|
+
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ for (int i = 0; i <roadSum; i++)
|
|
|
+ {
|
|
|
+ gpsTraceBack.clear();
|
|
|
+ // gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
|
|
|
+ // avoidX = (roadWidth)*(roadOri - i);
|
|
|
+ avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
|
|
|
+ gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (lidarGridPtr != NULL)
|
|
|
+ {
|
|
|
+ hasCheckedBackLidar = true;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ checkBackObsTimes++;
|
|
|
+ if (checkBackObsTimes<4 || hasCheckedBackLidar == false)
|
|
|
+ {
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //if ((GetDistance(now_gps_ins, startAvoid_gps_ins)>avoidRunDistance)&&
|
|
|
+ //(checkReturnEnable(avoidX, lidarGridPtr,roadOri)))
|
|
|
+ if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)) &&
|
|
|
+ (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
|
|
|
+ {
|
|
|
+ roadPre = roadOri;
|
|
|
+ return roadPre;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (roadNow-roadOri>1)
|
|
|
+ {
|
|
|
+ for (int i = roadOri + 1;i < roadNow;i++) {
|
|
|
+ if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
|
|
|
+ (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> avoidMinDistanceVector[i])&&
|
|
|
+ (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
|
|
|
+ {
|
|
|
+ roadPre = i;
|
|
|
+ return roadPre;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ else if (roadNow <roadOri-1)
|
|
|
+ {
|
|
|
+ for (int i = roadOri - 1;i > roadNow;i--) {
|
|
|
+ if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
|
|
|
+ (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
|
|
|
+ (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
|
|
|
+ {
|
|
|
+ roadPre = i;
|
|
|
+ return roadPre;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return roadPre;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double ivdecision_brain::trumpet() {
|
|
|
+ if (trumpetFirstCount)
|
|
|
+ {
|
|
|
+ trumpetFirstCount = false;
|
|
|
+ trumpetLastTime= GetTickCount();
|
|
|
+ trumpetTimeSpan = 0.0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ trumpetStartTime= GetTickCount();
|
|
|
+ trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
|
|
|
+ trumpetLastTime = trumpetStartTime;
|
|
|
+ }
|
|
|
+
|
|
|
+ return trumpetTimeSpan;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double ivdecision_brain::transferP() {
|
|
|
+ if (transferFirstCount)
|
|
|
+ {
|
|
|
+ transferFirstCount = false;
|
|
|
+ transferLastTime= GetTickCount();
|
|
|
+ transferTimeSpan = 0.0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ transferStartTime= GetTickCount();
|
|
|
+ transferTimeSpan += transferStartTime - transferLastTime;
|
|
|
+ transferLastTime = transferStartTime;
|
|
|
+ }
|
|
|
+
|
|
|
+ return transferTimeSpan;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void ivdecision_brain::handBrakePark(iv::decition::Decition decition, long duringTime, GPS_INS now_gps_ins) {
|
|
|
+
|
|
|
+ if (abs(now_gps_ins.speed)>0.1)
|
|
|
+ {
|
|
|
+ decition->accelerator = 0;
|
|
|
+ decition->brake = 20;
|
|
|
+ decition->wheel_angle = 0;
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+
|
|
|
+ decition->accelerator = 0;
|
|
|
+ decition->brake = 20;
|
|
|
+ decition->wheel_angle = 0;
|
|
|
+ handPark = true;
|
|
|
+ handParkTime = duringTime;
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void ivdecision_brain::getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins) {
|
|
|
+ gmapsL.clear();
|
|
|
+ gmapsR.clear();
|
|
|
+ for (int i = 0; i < 31; i++)
|
|
|
+ {
|
|
|
+ std::vector<iv::GPSData> gpsMapLineBeside;
|
|
|
+ // gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, -0.5*i);
|
|
|
+ gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, -0.5*i);
|
|
|
+
|
|
|
+ gmapsL.push_back(gpsMapLineBeside);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ for (int i = 0; i < 31; i++)
|
|
|
+ {
|
|
|
+ std::vector<iv::GPSData> gpsMapLineBeside;
|
|
|
+ // gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, 0.5*i);
|
|
|
+ gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, 0.5*i);
|
|
|
+
|
|
|
+ gmapsR.push_back(gpsMapLineBeside);
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+bool ivdecision_brain::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr) {
|
|
|
+
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ // lidarDistance = lastlidarDistance;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ obsPoint = iv::decition::Compute00().getLidarObsPoint(gpsTraceOri, lidarGridPtr);
|
|
|
+ double lidarDistance = obsPoint.y - 2.5; //激光距离推到车头
|
|
|
+ // ODS("\n超车雷达距离:%f\n", lidarDistance);
|
|
|
+ if (lidarDistance >-20 && lidarDistance<35)
|
|
|
+ {
|
|
|
+ checkChaoCheBackCounts = 0;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ checkChaoCheBackCounts++;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (checkChaoCheBackCounts>2) {
|
|
|
+ checkChaoCheBackCounts = 0;
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return false;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void ivdecision_brain::updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s){
|
|
|
+ Point2D pt = Coordinate_Transfer( now_gps_ins.gps_x, now_gps_ins.gps_y, group_ori_gps);
|
|
|
+
|
|
|
+ ServiceCarStatus.group_x_local=pt.x;
|
|
|
+ // ServiceCarStatus.group_y_local=pt.y;
|
|
|
+ ServiceCarStatus.group_y_local=s;
|
|
|
+ if(realspeed<0.36){
|
|
|
+ ServiceCarStatus.group_velx_local=0;
|
|
|
+ ServiceCarStatus.group_vely_local=0;
|
|
|
+ }else{
|
|
|
+ ServiceCarStatus.group_velx_local=realspeed*sin(theta)/3.6;
|
|
|
+ ServiceCarStatus.group_vely_local=realspeed*cos(theta)/3.6;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ ServiceCarStatus.group_pathpoint=PathPoint;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+float ivdecision_brain::ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time, const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
|
|
|
+ int pathpoint,float secSpeed,float dSpeed){
|
|
|
+ float traffic_speed=200;
|
|
|
+ float traffic_dis=0;
|
|
|
+ float passTime;
|
|
|
+ float passSpeed;
|
|
|
+ bool passEnable=false;
|
|
|
+
|
|
|
+ if(abs(secSpeed)<0.1){
|
|
|
+ secSpeed=0;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(pathpoint <= traffic_light_pathpoint){
|
|
|
+ for(int i=pathpoint;i<traffic_light_pathpoint;i++){
|
|
|
+ traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
|
|
|
+ }
|
|
|
+ }else{
|
|
|
+ for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
|
|
|
+ traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
|
|
|
+ }
|
|
|
+ for(int i=0;i<traffic_light_pathpoint;i++){
|
|
|
+ traffic_dis+=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // if(traffic_light_color != 0)
|
|
|
+ // {
|
|
|
+ // int a = 3;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+ if(traffic_light_color==0 && traffic_dis<10){
|
|
|
+ traffic_speed=0;
|
|
|
+ }
|
|
|
+ // else //20200108
|
|
|
+ // {
|
|
|
+ // traffic_speed=10;
|
|
|
+ // }
|
|
|
+ return traffic_speed;
|
|
|
+
|
|
|
+ passSpeed = min((float)(dSpeed/3.6),secSpeed);
|
|
|
+ passTime = traffic_dis/(dSpeed/3.6);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ switch(traffic_light_color){
|
|
|
+ case 0:
|
|
|
+ if(passTime>traffic_light_time+1 && traffic_dis>10){
|
|
|
+ passEnable=true;
|
|
|
+ }else{
|
|
|
+ passEnable=false;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ if(passTime<traffic_light_time-1 && traffic_dis<10){
|
|
|
+ passEnable=true;
|
|
|
+ }else{
|
|
|
+ passEnable = false;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ if(passTime<traffic_light_time){
|
|
|
+ passEnable= true;
|
|
|
+ }else{
|
|
|
+ passEnable=false;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+
|
|
|
+
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(!passEnable){
|
|
|
+ if(traffic_dis<5){
|
|
|
+ traffic_speed=0;
|
|
|
+ }else if(traffic_dis<10){
|
|
|
+ traffic_speed=5;
|
|
|
+ }else if(traffic_dis<20){
|
|
|
+ traffic_speed=15;
|
|
|
+ }else if(traffic_dis<30){
|
|
|
+ traffic_speed=25;
|
|
|
+ }else if(traffic_dis<50){
|
|
|
+ traffic_speed=30;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return traffic_speed;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void ivdecision_brain::computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr, const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
|
|
|
+{
|
|
|
+ // Point2D obsCombinePoint = Point2D(-1,-1);
|
|
|
+ iv::decition::FrenetPoint car_now_frenet_point = iv::decition::FrenetPlanner::getFrenetfromXY(0,0,gpsTrace,gpsMap,pathpoint,nowGps);
|
|
|
+ double obsSd;
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ lidarDistance = lastLidarDis;
|
|
|
+ // lidarDistance = lastlidarDistance;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ obsPoint = iv::decition::Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
|
|
|
+ // lidarDistance = obsPoint.y-2.5; //激光距离推到车头
|
|
|
+ iv::decition::FrenetPoint lidarFPointTmp = iv::decition::FrenetPlanner::getFrenetfromXY(this->obsPoint.x,this->obsPoint.y,gpsTrace,gpsMap,pathpoint,nowGps);
|
|
|
+ lidarDistance = lidarFPointTmp.s - car_now_frenet_point.s - 2.5;
|
|
|
+
|
|
|
+ // lidarDistance=-1;
|
|
|
+ if (lidarDistance<0)
|
|
|
+ {
|
|
|
+ lidarDistance = -1;
|
|
|
+ }
|
|
|
+ lastLidarDis = lidarDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ FrenetPoint esr_obs_frenet_point;
|
|
|
+ getEsrObsDistanceByFrenet(gpsTrace, car_now_frenet_point,esr_obs_frenet_point,gpsMap,pathpoint,nowGps);
|
|
|
+
|
|
|
+ if(lidarDistance<0){
|
|
|
+ lidarDistance=500;
|
|
|
+ }
|
|
|
+ if(esrDistance<0){
|
|
|
+ esrDistance=500;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
|
|
|
+ myesrDistance = esrDistance;
|
|
|
+
|
|
|
+ if(lidarDistance==500){
|
|
|
+ lidarDistance=-1;
|
|
|
+ }
|
|
|
+ if(esrDistance==500){
|
|
|
+ esrDistance=-1;
|
|
|
+ }
|
|
|
+
|
|
|
+ ServiceCarStatus.mRadarObs = esrDistance;
|
|
|
+ ServiceCarStatus.mLidarObs = lidarDistance;
|
|
|
+
|
|
|
+ // //zhuanwan pingbi haomibo
|
|
|
+ // if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
|
|
|
+ // esrDistance=-1;
|
|
|
+ // }
|
|
|
+
|
|
|
+ if (esrDistance>0 && lidarDistance > 0)
|
|
|
+ {
|
|
|
+ if (lidarDistance >= esrDistance)
|
|
|
+ {
|
|
|
+ obs = esrDistance;
|
|
|
+ // obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+ obsSd = obsSpeed;
|
|
|
+ //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
|
|
|
+ // obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
|
|
|
+ }
|
|
|
+ else if (!ServiceCarStatus.obs_radar.empty())
|
|
|
+ {
|
|
|
+ obs = lidarDistance;
|
|
|
+ // obsCombinePoint = obsPoint;
|
|
|
+ // obsSd = iv::decition::Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
|
|
|
+ obsSd = iv::decition::Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obs=lidarDistance;
|
|
|
+ // obsCombinePoint = obsPoint;
|
|
|
+ obsSd = 0 -secSpeed*cos(car_now_frenet_point.tangent_Ang-M_PI/2);
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (esrDistance>0)
|
|
|
+ {
|
|
|
+ obs = esrDistance;
|
|
|
+ // obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+ obsSd = obsSpeed;
|
|
|
+ //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
|
|
|
+ // obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
|
|
|
+ }
|
|
|
+ else if (lidarDistance > 0)
|
|
|
+ {
|
|
|
+ obs = lidarDistance;
|
|
|
+ // obsCombinePoint = obsPoint;
|
|
|
+ obsSd = iv::decition::Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ obs = esrDistance;
|
|
|
+ // obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
|
|
|
+ obsSd = 0 - secSpeed*cos(car_now_frenet_point.tangent_Ang-M_PI/2);
|
|
|
+ }
|
|
|
+
|
|
|
+ obsDistance=obs;
|
|
|
+ obsSpeed=obsSd;
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
|
|
|
+
|
|
|
+ ServiceCarStatus.mObs = obsDistance;
|
|
|
+ if(ServiceCarStatus.mObs>100){
|
|
|
+ ServiceCarStatus.mObs =-1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (obsDistance>0)
|
|
|
+ {
|
|
|
+ lastDistance = obsDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(obs<0){
|
|
|
+ obsDistance=500;
|
|
|
+ }else{
|
|
|
+ obsDistance=obs;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void ivdecision_brain::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
|
|
|
+
|
|
|
+
|
|
|
+ esrIndex = iv::decition::Compute00().getRearEsrIndex(gpsTrace, roadNum,xiuzhengCs);
|
|
|
+
|
|
|
+ if (esrIndex != -1)
|
|
|
+ {
|
|
|
+ esrDistance = ServiceCarStatus.obs_rear_radar[esrIndex].nomal_y;
|
|
|
+ obsSpeed = ServiceCarStatus.obs_rear_radar[esrIndex].speed_y;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ esrDistance = -1;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void ivdecision_brain::getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, const FrenetPoint car_now_frenet_point, FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps) {
|
|
|
+
|
|
|
+
|
|
|
+ esrIndex = iv::decition::Compute00().getEsrIndexByFrenet(gpsTrace,esrObs_F_Point,gpsMap,pathpoint,nowGps,xiuzhengCs);
|
|
|
+
|
|
|
+ if (esrIndex != -1)
|
|
|
+ {
|
|
|
+ //障碍物的距离，是障碍物和车辆沿道路上的距离，即s。并不直接是它的x坐标或y坐标或两点直线坐标。
|
|
|
+ //严格来说应是 esrDistance=障碍物的s - 车辆的s - Esr_Y_Offset，但这是根据车辆位置实时搜索路径，故车辆的s极其小。
|
|
|
+ esrDistance = esrObs_F_Point.s - car_now_frenet_point.s - Esr_Y_Offset; //-Esr_Y_Offset是把距离推到车头，换算frenet坐标时，把它加上了的。故需减去。
|
|
|
+
|
|
|
+ double speedx=ServiceCarStatus.obs_radar[esrIndex].speed_x; //障碍物相对于车辆x轴的速度
|
|
|
+ double speedy=ServiceCarStatus.obs_radar[esrIndex].speed_y; //障碍物相对于车辆y轴的速度
|
|
|
+ double speed_combine = sqrt(speedx*speedx+speedy*speedy); //将x、y轴两个方向的速度求矢量和
|
|
|
+ //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
|
|
|
+ //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
|
|
|
+ double Etheta = esrObs_F_Point.tangent_Ang - atan2(speedy,speedx);
|
|
|
+
|
|
|
+ obsSpeed = speed_combine*cos(Etheta); //由speed_combine分解的s轴方向上的速度
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ esrDistance = -1;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void ivdecision_brain::getV2XTrafficPositionVector(const std::vector<GPSData> gpsMapLine){
|
|
|
+ v2xTrafficVector.clear();
|
|
|
+ for (int var = 0; var < gpsMapLine.size(); var++) {
|
|
|
+ if(gpsMapLine[var]->roadMode==6 || gpsMapLine[var]->mode2==1000001){
|
|
|
+ v2xTrafficVector.push_back(var);
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+float ivdecision_brain::ComputeV2XTrafficLightSpeed(iv::TrafficLight trafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
|
|
|
+ int pathpoint,float secSpeed,float dSpeed, bool circleMode){
|
|
|
+ float trafficSpeed=200;
|
|
|
+ int nearTraffixPoint=-1;
|
|
|
+ float nearTrafficDis=0;
|
|
|
+ int traffic_color=0;
|
|
|
+ int traffic_time=0;
|
|
|
+ bool passThrough=false;
|
|
|
+ float dSecSpeed=dSpeed/3.6;
|
|
|
+
|
|
|
+ if(v2xTrafficVector.empty()){
|
|
|
+ return trafficSpeed;
|
|
|
+ }
|
|
|
+ if(!circleMode){
|
|
|
+ if(pathpoint>v2xTrafficVector.back()){
|
|
|
+ return trafficSpeed;
|
|
|
+ }else {
|
|
|
+ for(int i=0; i< v2xTrafficVector.size();i++){
|
|
|
+ if (pathpoint<= v2xTrafficVector[i]){
|
|
|
+ nearTraffixPoint=v2xTrafficVector[i];
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }else if(circleMode){
|
|
|
+ if(pathpoint>v2xTrafficVector.back()){
|
|
|
+ nearTraffixPoint=v2xTrafficVector[0];
|
|
|
+ }else {
|
|
|
+ for(int i=0; i< v2xTrafficVector.size();i++){
|
|
|
+ if (pathpoint<= v2xTrafficVector[i]){
|
|
|
+ nearTraffixPoint=v2xTrafficVector[i];
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(nearTraffixPoint!=-1){
|
|
|
+ for(int i=pathpoint;i<nearTraffixPoint;i++){
|
|
|
+ nearTrafficDis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(nearTrafficDis>50){
|
|
|
+ return trafficSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ int roadMode = gpsMapLine[pathpoint]->roadMode;
|
|
|
+ if(roadMode==14 || roadMode==16){
|
|
|
+ traffic_color=trafficLight.leftColor;
|
|
|
+ traffic_time=trafficLight.leftTime;
|
|
|
+ }else if(roadMode==15 ||roadMode==17){
|
|
|
+ traffic_color=trafficLight.rightColor;
|
|
|
+ traffic_time=trafficLight.rightTime;
|
|
|
+ }else {
|
|
|
+ traffic_color=trafficLight.straightColor;
|
|
|
+ traffic_time=trafficLight.straightTime;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
|
|
|
+ if(passThrough){
|
|
|
+ return trafficSpeed;
|
|
|
+ }else{
|
|
|
+ trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
|
|
|
+ if(nearTrafficDis<6){
|
|
|
+ float decelerate =0-( secSpeed*secSpeed*0.5/nearTrafficDis);
|
|
|
+ minDecelerate=min(minDecelerate,decelerate);
|
|
|
+ }
|
|
|
+ return trafficSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+ return trafficSpeed;
|
|
|
+}
|
|
|
+
|
|
|
+bool ivdecision_brain::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed){
|
|
|
+ float passTime=0;
|
|
|
+ if (trafficColor==2 || trafficColor==3){
|
|
|
+ return false;
|
|
|
+ }else if(trafficColor==0){
|
|
|
+ return true;
|
|
|
+ }else{
|
|
|
+ passTime=trafficDis/dSecSpeed;
|
|
|
+ if(passTime+1< trafficTime){
|
|
|
+ return true;
|
|
|
+ }else{
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+float ivdecision_brain::computeTrafficSpeedLimt(float trafficDis){
|
|
|
+ float limit=200;
|
|
|
+ if(trafficDis<10){
|
|
|
+ limit = 0;
|
|
|
+ }else if(trafficDis<15){
|
|
|
+ limit = 5;
|
|
|
+ }else if(trafficDis<20){
|
|
|
+ limit=10;
|
|
|
+ }else if(trafficDis<30){
|
|
|
+ limit=15;
|
|
|
+ }
|
|
|
+ return limit;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+bool ivdecision_brain::adjuseultra(){
|
|
|
+ bool front=false,back=false,left=false,right=false;
|
|
|
+ for(int i=1;i<=13;i++)
|
|
|
+ {
|
|
|
+ if((i==2)||(i==3)||(i==4)||(i==5)) //front
|
|
|
+ {
|
|
|
+ if(ServiceCarStatus.ultraDistance[i]<100)
|
|
|
+ {
|
|
|
+ front=true;
|
|
|
+ }
|
|
|
+ }else if((i==1)||(i==12)||(i==6)||(i==7)) //left,right
|
|
|
+ {
|
|
|
+ if(ServiceCarStatus.ultraDistance[i]<30)
|
|
|
+ {
|
|
|
+ left=true;
|
|
|
+ }
|
|
|
+ }else if((i==8)||(i==9)||(i==10)||(i==11)) //back
|
|
|
+ {
|
|
|
+ if(ServiceCarStatus.ultraDistance[i]<10)
|
|
|
+ {
|
|
|
+ back=true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(front||left||back)
|
|
|
+ return true;
|
|
|
+ else
|
|
|
+ return false;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void ivdecision_brain::transferGpsMode2( const std::vector<GPSData> gpsMapLine){
|
|
|
+ if( gpsMapLine[PathPoint]->mode2==3000){
|
|
|
+ if(obsDistance>5){
|
|
|
+ obsDistance=200;
|
|
|
+ }
|
|
|
+ dSpeed=min(dSpeed,5.0);
|
|
|
+ }
|
|
|
+}
|
|
|
+float ivdecision_brain::computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth){
|
|
|
+ if(roadAim==roadOri){
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+ float x=0;
|
|
|
+ float veh_to_roadSide=(gps->mfRoadWidth- ServiceCarStatus.msysparam.vehWidth)*0.5;
|
|
|
+ float roadSide_to_roadSide=ServiceCarStatus.msysparam.vehWidth;
|
|
|
+ if(!ServiceCarStatus.inRoadAvoid){
|
|
|
+ x= (roadOri-roadAim)*gps->mfRoadWidth;
|
|
|
+ }else{
|
|
|
+ int num=roadOri-roadAim;
|
|
|
+ switch (abs(num%3)) {
|
|
|
+ case 0:
|
|
|
+ x=(num/3)*gps->mfRoadWidth;
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ if(num>0){
|
|
|
+ x=(num/3)*gps->mfRoadWidth +veh_to_roadSide;
|
|
|
+ }else{
|
|
|
+ x=(num/3)*gps->mfRoadWidth -veh_to_roadSide;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ if(num>0){
|
|
|
+ x=(num/3)*gps->mfRoadWidth +veh_to_roadSide+roadSide_to_roadSide;
|
|
|
+ }else{
|
|
|
+ x=(num/3)*gps->mfRoadWidth -veh_to_roadSide-roadSide_to_roadSide;
|
|
|
+ }
|
|
|
+
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return x;
|
|
|
+}
|
|
|
+
|
|
|
|
|
|
}
|
|
|
|
