#include "ivdecision_brain.h"
#include <QCoreApplication>
#include "common/obs_predict.h"
#include "ivlog.h"
#include "xmlparam.h"
extern iv::Ivlog * givlog;
bool handPark;
long handParkTime;
bool rapidStop;
int gpsMissCount;
bool changeRoad;
double avoidX;
bool parkBesideRoad;
double steerSpeed;
bool transferPieriod;
bool transferPieriod2;
double traceDevi;
using namespace iv::decition;
namespace iv {
ivdecision_brain::ivdecision_brain()
{
mvehState = VehState::normalRun;
loadxml();
}
int ivdecision_brain::getdecision(brain::decition &xdecition,iv::brain::brainstate & xbs)
{
static qint64 nlastdecisiontime = 0;
static qint64 nLastMapUpdate = 0;
iv::GPSData now_gps_ins;
std::shared_ptr<iv::hmi::hmimsg> hmi_ptr;
if(GetHMImsg(hmi_ptr) == true)
{
ServiceCarStatus.mbRunPause = hmi_ptr->mbpause();
if(hmi_ptr->mbbochemode())
{
ServiceCarStatus.bocheMode = true;
}
ServiceCarStatus.busmode = hmi_ptr->mbbusmode();
}
if(ServiceCarStatus.mbRunPause)
{
return 0;
}
if(GetGPS(now_gps_ins) == false)
{
return -1; //No GPS Position
}
if(IsMAPUpdate(nLastMapUpdate))
{
GetMAP(mgpsMapLine,nLastMapUpdate);
mbisFirstRun = true;
}
iv::LidarGridPtr lidarptr;
GetLIDARGrid(lidarptr);
std::vector<iv::Perception::PerceptionOutput> xvectorper;
iv::TrafficLight xtrafficlight;
std::shared_ptr<iv::vbox::vbox> xvbox_ptr;
if(Getvboxmsg(xvbox_ptr))
{
xtrafficlight.leftColor=xvbox_ptr->st_left();
xtrafficlight.rightColor=xvbox_ptr->st_right();
xtrafficlight.straightColor=xvbox_ptr->st_straight();
xtrafficlight.uturnColor=xvbox_ptr->st_turn();
xtrafficlight.leftTime=xvbox_ptr->time_left();
xtrafficlight.rightTime=xvbox_ptr->time_right();
xtrafficlight.straightTime=xvbox_ptr->time_straight();
xtrafficlight.uturnTime=xvbox_ptr->time_turn();
}
updatev2x();
updateultra();
updateradar();
updategroupinfo();
if(mgpsMapLine.size()<10)
{
return 0;
}
iv::decition::Decition decition= getDecideFromGPS(*now_gps_ins,mgpsMapLine,lidarptr,xvectorper,xtrafficlight);
xdecition.set_accelerator(decition->accelerator);
xdecition.set_brake(decition->brake);
xdecition.set_leftlamp(decition->leftlamp);
xdecition.set_rightlamp(decition->rightlamp);
xdecition.set_speed(decition->speed);
xdecition.set_wheelangle(decition->wheel_angle);
xdecition.set_wheelspeed(decition->angSpeed);
xdecition.set_torque(decition->torque);
xdecition.set_mode(decition->mode);
xdecition.set_gear(decition->dangWei);
xdecition.set_handbrake(decition->handBrake);
xdecition.set_grade(decition->grade);
xdecition.set_engine(decition->engine);
xdecition.set_brakelamp(decition->brakeLight);
xdecition.set_ttc(ServiceCarStatus.mfttc);
xdecition.set_air_enable(decition->air_enable);
xdecition.set_air_temp(decition->air_temp);
xdecition.set_air_mode(decition->air_mode);
xdecition.set_wind_level(decition->wind_level);
xdecition.set_roof_light(decition->roof_light);
xdecition.set_home_light(decition->home_light);
xdecition.set_air_worktime(decition->air_worktime);
xdecition.set_air_offtime(decition->air_offtime);
xdecition.set_air_on(decition->air_on);
xdecition.set_door(decition->door);
xbs.set_mbbocheenable(ServiceCarStatus.bocheEnable);
xbs.set_mbbrainrunning(true); //apollo_fu debug ui 20200417
xbs.set_mflidarobs(ServiceCarStatus.mLidarObs);
xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
xbs.set_mfobs(ServiceCarStatus.mObs);
xbs.set_decision_period((QDateTime::currentMSecsSinceEpoch() - nlastdecisiontime));
nlastdecisiontime = QDateTime::currentMSecsSinceEpoch();
return 1;
}
void ivdecision_brain::updategroupinfo()
{
std::shared_ptr<iv::radio::radio_send> groupmsg_ptr;
if(false == Getp900(groupmsg_ptr))
{
return;
}
ServiceCarStatus.group_server_status = groupmsg_ptr->server_status();
ServiceCarStatus.group_comm_speed= (float)groupmsg_ptr->car_control_speed();
ServiceCarStatus.group_state= groupmsg_ptr->cmd_mode();
if(ServiceCarStatus.group_state!=2){
ServiceCarStatus.group_comm_speed=0;
}
}
void ivdecision_brain::updatechassistocarstatus()
{
std::shared_ptr<iv::chassis> xchassis_ptr;
if(false == Getchassis(xchassis_ptr))
{
return;
}
ServiceCarStatus.epb = xchassis_ptr->epbfault();
ServiceCarStatus.grade = xchassis_ptr->shift();
ServiceCarStatus.driverMode = xchassis_ptr->drivemode();
if(xchassis_ptr->has_epsmode()){
ServiceCarStatus.epsMode = xchassis_ptr->epsmode();
ServiceCarStatus.receiveEps=true;
if(ServiceCarStatus.epsMode == 0)
{
ServiceCarStatus.mbRunPause = true;
}
}
if(xchassis_ptr->has_torque())
{
ServiceCarStatus.torque_st = xchassis_ptr->torque();
if(ServiceCarStatus.msysparam.mvehtype=="bus"){
ServiceCarStatus.torque_st = xchassis_ptr->torque()*0.4;
}
std::cout<<"******* xchassis.torque:"<< xchassis_ptr->torque()<<std::endl;
std::cout<<"******* ServiceCarStatus.torque_st:"<< ServiceCarStatus.torque_st<<std::endl;
givlog->warn(" ServiceCarStatus.torque_st: is %f",ServiceCarStatus.torque_st);
}
if(xchassis_ptr->has_engine_speed())
{
ServiceCarStatus.engine_speed = xchassis_ptr->engine_speed();
std::cout<<"******* xchassis.engine_speed:"<< xchassis_ptr->engine_speed()<<std::endl;
}
}
void ivdecision_brain::loadxml()
{
QString strpath = QCoreApplication::applicationDirPath();
strpath = strpath + "/ADS_decision.xml";
iv::xmlparam::Xmlparam xp(strpath.toStdString());
ServiceCarStatus.msysparam.mstrvin = xp.GetParam("vin","10000000000000001");
ServiceCarStatus.msysparam.mstrid = xp.GetParam("id","1");
ServiceCarStatus.msysparam.mstriccid = xp.GetParam("iccid","11111111111111111111");
ServiceCarStatus.msysparam.mvehtype= xp.GetParam("vehType","yuhesen");
ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());
/*============= 20200113 apollo_fu add ===========*/
std::string str_zhuchetime = xp.GetParam("zhuchetime","16");
ServiceCarStatus.msysparam.mzhuchetime =atoi(str_zhuchetime.data());
/*============= end ================================== */
std::string strepsoff = xp.GetParam("epsoff","0.0");
ServiceCarStatus.mfEPSOff = atof(strepsoff.data());
std::string strroadmode_vel = xp.GetParam("roadmode0","10");
ServiceCarStatus.mroadmode_vel.mfmode0 = atof(strroadmode_vel.data());
strroadmode_vel = xp.GetParam("roadmode11","30");
ServiceCarStatus.mroadmode_vel.mfmode11 = atof(strroadmode_vel.data());
strroadmode_vel = xp.GetParam("roadmode14","15");
ServiceCarStatus.mroadmode_vel.mfmode14 = atof(strroadmode_vel.data());
//float a = ServiceCarStatus.mroadmode_vel.mfmode14;
//cout<<"........"<<a<<endl;
strroadmode_vel = xp.GetParam("roadmode15","15");
ServiceCarStatus.mroadmode_vel.mfmode15 = atof(strroadmode_vel.data());
/*==================== 20200113 apollo_fu add =================*/
strroadmode_vel = xp.GetParam("roadmode5","10");
ServiceCarStatus.mroadmode_vel.mfmode5 = atof(strroadmode_vel.data());
strroadmode_vel = xp.GetParam("roadmode13","5");
ServiceCarStatus.mroadmode_vel.mfmode13 = atof(strroadmode_vel.data());
strroadmode_vel = xp.GetParam("roadmode16","8");
ServiceCarStatus.mroadmode_vel.mfmode16 = atof(strroadmode_vel.data());
strroadmode_vel = xp.GetParam("roadmode17","8");
ServiceCarStatus.mroadmode_vel.mfmode17 = atof(strroadmode_vel.data());
strroadmode_vel = xp.GetParam("roadmode18","5");
ServiceCarStatus.mroadmode_vel.mfmode18 = atof(strroadmode_vel.data());
/*========================= end =============================*/
std::string group_cotrol = xp.GetParam("group","false");
if(group_cotrol=="true"){
ServiceCarStatus.group_control=true;
}else{
ServiceCarStatus.group_control=false;
}
std::string speed_control = xp.GetParam("speed","false");
if(speed_control=="true"){
ServiceCarStatus.speed_control=true;
}else{
ServiceCarStatus.speed_control=false;
}
std::string strparklat = xp.GetParam("parklat","39.0624557");
std::string strparklng = xp.GetParam("parklng","117.3575493");
std::string strparkheading = xp.GetParam("parkheading","112.5");
std::string strparktype = xp.GetParam("parktype","1");
ServiceCarStatus.mfParkLat = atof(strparklat.data());
ServiceCarStatus.mfParkLng = atof(strparklng.data());
ServiceCarStatus.mfParkHeading = atof(strparkheading.data());
ServiceCarStatus.mnParkType = atoi(strparktype.data());
ServiceCarStatus.mLidarRotation = atof(xp.GetParam("LidarRotation","90.0").data());
ServiceCarStatus.mLidarRangeUnit = atof(xp.GetParam("LidarRangeUnit","5.0").data());
std::string lightlon = xp.GetParam("lightlon","0");
std::string lightlat = xp.GetParam("lightlat","0");
ServiceCarStatus.iTrafficeLightLat=atof(lightlat.data());
ServiceCarStatus.iTrafficeLightLon=atof(lightlon.data());
//mobileEye
std::string timeWidth =xp.GetParam("waitGpsTimeWidth","5000");
std::string garageLong =xp.GetParam("outGarageLong","10");
ServiceCarStatus.waitGpsTimeWidth=atof(timeWidth.data());
ServiceCarStatus.outGarageLong=atof(garageLong.data());
//mobileEye end
//lidar start
std::string lidarGpsXiuzheng = xp.GetParam("lidarGpsXiuzheng","1.0");
std::string radarGpsXiuzheng = xp.GetParam("radarGpsXiuzheng","3.0");
std::string rearRadarGpsXiuzheng = xp.GetParam("rearRadarGpsXiuzheng","0.5");
std::string rearLidarGpsXiuzheng = xp.GetParam("rearLidarGpsXiuzheng","1.0");
std::string frontGpsXiuzheng = xp.GetParam("frontGpsXiuzheng","3.0");
std::string rearGpsXiuzheng = xp.GetParam("rearGpsXiuzheng","1.0");
std::string gpsOffset_x = xp.GetParam("gpsOffset_X","0");
std::string gpsOffset_y = xp.GetParam("gpsOffset_Y","0");
std::string strexternmpc = xp.GetParam("ExternMPC","false"); //If Use MPC set true
adjuseGpsLidarPosition();
if(strexternmpc == "true")
{
// mbUseExternMPC = true;
}
ServiceCarStatus.msysparam.lidarGpsXiuzheng = atof(lidarGpsXiuzheng.data());
ServiceCarStatus.msysparam.radarGpsXiuzheng = atof(radarGpsXiuzheng.data());
ServiceCarStatus.msysparam.rearRadarGpsXiuzheng = atof(rearRadarGpsXiuzheng.data());
ServiceCarStatus.msysparam.rearLidarGpsXiuzheng = atof(rearLidarGpsXiuzheng.data());
ServiceCarStatus.msysparam.frontGpsXiuzheng = atof(frontGpsXiuzheng.data());
ServiceCarStatus.msysparam.rearGpsXiuzheng = atof(rearGpsXiuzheng.data());
ServiceCarStatus.msysparam.gpsOffset_x = atof(gpsOffset_x.data());
ServiceCarStatus.msysparam.gpsOffset_y = atof(gpsOffset_y.data());
// lidar end
std::string strObsPredict = xp.GetParam("obsPredict","false"); //If Use MPC set true
if(strObsPredict == "true")
{
ServiceCarStatus.useObsPredict = true;
}
std::string inRoadAvoid = xp.GetParam("inRoadAvoid","false"); //If Use MPC set true
if(inRoadAvoid == "true")
{
ServiceCarStatus.inRoadAvoid = true;
}else{
ServiceCarStatus.inRoadAvoid = false;
}
std::string avoidObs = xp.GetParam("avoidObs","false"); //If Use MPC set true
if(avoidObs == "true")
{
ServiceCarStatus.avoidObs = true;
}else{
ServiceCarStatus.avoidObs = false;
}
//map
// mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
}
void ivdecision_brain::updatev2x()
{
std::shared_ptr<iv::v2x::v2x> xv2x_ptr;
if(false == Getv2xmsg(xv2x_ptr))
{
return;
}
ServiceCarStatus.stationCmd.received=true;
ServiceCarStatus.stationCmd.has_carID=xv2x_ptr->has_carid();
if(ServiceCarStatus.stationCmd.has_carID)
{
ServiceCarStatus.stationCmd.carID=xv2x_ptr->carid();
}
ServiceCarStatus.stationCmd.has_carMode=xv2x_ptr->has_carmode();
if(ServiceCarStatus.stationCmd.has_carMode)
{
ServiceCarStatus.stationCmd.carMode=xv2x_ptr->carmode();
qDebug("ServiceCarStatus.stationCmd.carMode:",ServiceCarStatus.stationCmd.carMode);
givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.carMode:%d",ServiceCarStatus.stationCmd.carMode);
}
ServiceCarStatus.stationCmd.has_emergencyStop=xv2x_ptr->has_emergencystop();
if(ServiceCarStatus.stationCmd.has_emergencyStop)
{
ServiceCarStatus.stationCmd.emergencyStop=xv2x_ptr->emergencystop();
qDebug("ServiceCarStatus.stationCmd.emergencyStop:",ServiceCarStatus.stationCmd.emergencyStop);
givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.emergencyStop:%d",ServiceCarStatus.stationCmd.emergencyStop);
}
ServiceCarStatus.stationCmd.has_stationStop=xv2x_ptr->has_stationstop();
if(ServiceCarStatus.stationCmd.has_stationStop)
{
ServiceCarStatus.stationCmd.stationStop=xv2x_ptr->stationstop();
qDebug("ServiceCarStatus.stationCmd.stationStop:",ServiceCarStatus.stationCmd.stationStop);
givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.stationStop:%d",ServiceCarStatus.stationCmd.stationStop);
}
int num=xv2x_ptr->stationid_size();
if(num>0)
{
ServiceCarStatus.stationCmd.stationTotalNum=num;
for(int i=0;i<ServiceCarStatus.stationCmd.stationTotalNum;i++)
{
ServiceCarStatus.stationCmd.stationGps[i].gps_lat=xv2x_ptr->stgps(i).lat();
ServiceCarStatus.stationCmd.stationGps[i].gps_lng=xv2x_ptr->stgps(i).lon();
qDebug("stationGps: %d, lat: %.7f, lon: %.7f", xv2x_ptr->stationid(i), ServiceCarStatus.stationCmd.stationGps[i].gps_lat, ServiceCarStatus.stationCmd.stationGps[i].gps_lng);
givlog->debug("brain_v2x","stationGps: %d, lat: %.7f, lon: %.7f",
xv2x_ptr->stationid(i),ServiceCarStatus.stationCmd.stationGps[i].gps_lat,
ServiceCarStatus.stationCmd.stationGps[i].gps_lng);
}
ServiceCarStatus.mbRunPause=false;
}
}
void ivdecision_brain::adjuseGpsLidarPosition()
{
ServiceCarStatus.msysparam.lidarGpsXiuzheng -= ServiceCarStatus.msysparam.gpsOffset_y;
ServiceCarStatus.msysparam.radarGpsXiuzheng -= ServiceCarStatus.msysparam.gpsOffset_y;
ServiceCarStatus.msysparam.rearRadarGpsXiuzheng += ServiceCarStatus.msysparam.gpsOffset_y;
ServiceCarStatus.msysparam.rearLidarGpsXiuzheng += ServiceCarStatus.msysparam.gpsOffset_y;
ServiceCarStatus.msysparam.frontGpsXiuzheng -= ServiceCarStatus.msysparam.gpsOffset_y;
ServiceCarStatus.msysparam.rearGpsXiuzheng += ServiceCarStatus.msysparam.gpsOffset_y;
}
void ivdecision_brain::updateultra()
{
std::shared_ptr<iv::ultrasonic::ultrasonic> xultra_ptr;
if(false == Getultrasonic(xultra_ptr))
{
return;
}
ServiceCarStatus.ultraDistance[1]=xultra_ptr->sigobjdist_flcorner();
ServiceCarStatus.ultraDistance[2]=xultra_ptr->sigobjdist_flmiddle();
ServiceCarStatus.ultraDistance[3]=xultra_ptr->sigobjdist_flside();
ServiceCarStatus.ultraDistance[4]=xultra_ptr->sigobjdist_frcorner();
ServiceCarStatus.ultraDistance[5]=xultra_ptr->sigobjdist_frmiddle();
ServiceCarStatus.ultraDistance[6]=xultra_ptr->sigobjdist_frside();
ServiceCarStatus.ultraDistance[7]=xultra_ptr->sigobjdist_rlcorner();
ServiceCarStatus.ultraDistance[8]=xultra_ptr->sigobjdist_rlmiddle();
ServiceCarStatus.ultraDistance[9]=xultra_ptr->sigobjdist_rlside();
ServiceCarStatus.ultraDistance[10]=xultra_ptr->sigobjdist_rrcorner();
ServiceCarStatus.ultraDistance[11]=xultra_ptr->sigobjdist_rrmiddle();
ServiceCarStatus.ultraDistance[12]=xultra_ptr->sigobjdist_rrside();
}
void ivdecision_brain::updateradar()
{
std::shared_ptr<iv::radar::radarobjectarray> xradar_ptr
= std::shared_ptr<iv::radar::radarobjectarray>(new iv::radar::radarobjectarray);
unsigned int i;
if(false == GetRADAR(xradar_ptr))
{
for(i=0;i<64;i++)
{
ServiceCarStatus.obs_radar[i].valid = false;
}
return;
}
for(i=0;i<64;i++)
{
iv::radar::radarobject * pradarobj = xradar_ptr->mutable_obj(i);
ServiceCarStatus.obs_radar[i].valid = pradarobj->bvalid();
ServiceCarStatus.obs_radar[i].nomal_x = pradarobj->x();
ServiceCarStatus.obs_radar[i].nomal_y = pradarobj->y();
ServiceCarStatus.obs_radar[i].speed_relative = pradarobj->vel();
ServiceCarStatus.obs_radar[i].speed_x = pradarobj->vx();
ServiceCarStatus.obs_radar[i].speed_y = pradarobj->vy();
}
}
//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,
iv::decition::Decition ivdecision_brain::decision_reverseCar(GPS_INS now_gps_ins)
{
iv::decition::Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
vehState = reversing;
qiedianCount=true;
return decision_reversing(now_gps_ins);
}
iv::decition::Decition ivdecision_brain::decision_reversing(GPS_INS now_gps_ins)
{
iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
double dis = GetDistance(now_gps_ins, iv::decition::Compute00().nearTpoint);
if (dis<2.0)//0.5
{
vehState = reverseCircle;
qiedianCount = true;
cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
return decision_reverseCircle(now_gps_ins);
}
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;
}
}
iv::decition::Decition ivdecision_brain::decision_reverseCircle(GPS_INS now_gps_ins)
{
iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
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;
return decision_reverseDirect(now_gps_ins);
}
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;
}
}
iv::decition::Decition ivdecision_brain::decision_reverseDirect(GPS_INS now_gps_ins)
{
iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
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;
}
}
iv::decition::Decition ivdecision_brain::decision_reverseArr(iv::GPS_INS now_gps_ins)
{
iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
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;
}
iv::decition::Decition ivdecision_brain::decision_dRever(GPS_INS now_gps_ins)
{
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;
return decision_dRever0(now_gps_ins);
}
iv::decition::Decition ivdecision_brain::decision_dRever0(GPS_INS now_gps_ins)
{
iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint0);
if (dis<2.0)
{
vehState = dRever1;
qiedianCount = true;
cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
return decision_dRever1(now_gps_ins);
}
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;
}
}
iv::decition::Decition ivdecision_brain::decision_dRever1(GPS_INS now_gps_ins)
{
iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint1);
if(dis<2.0)
{
vehState = dRever2;
qiedianCount = true;
cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
return decision_dRever2(now_gps_ins);
}
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;
}
}
iv::decition::Decition ivdecision_brain::decision_dRever2(GPS_INS now_gps_ins)
{
iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
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;
return decision_dRever3(now_gps_ins);
}
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;
}
}
iv::decition::Decition ivdecision_brain::decision_dRever3(GPS_INS now_gps_ins)
{
iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
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;
return decision_dRever4(now_gps_ins);
}
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;
}
}
iv::decition::Decition ivdecision_brain::decision_dRever4(GPS_INS now_gps_ins)
{
iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
// 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;
}
}
void ivdecision_brain::decision_firstRun(GPS_INS now_gps_ins,
const std::vector<GPSData> & gpsMapLine)
{
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;
}
}
int ivdecision_brain::decision_ParkCalc(GPS_INS now_gps_ins,iv::decition::Decition & gps_decition)
{
//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 1;
}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 1;
}else{
if(trumpet()>3000){
trumpetFirstCount=true;
vehState=reverseCar;
}
}
// ServiceCarStatus.bocheMode=false;
}
}
return 0;
}
void ivdecision_brain::decision_readyZhucheMode(GPS_INS now_gps_ins, const std::vector<GPSData> &gpsMapLine)
{
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;
}
}
}
void ivdecision_brain::decision_readyParkMode(GPS_INS now_gps_ins, const std::vector<GPSData> &gpsMapLine)
{
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;
}
}
}
void ivdecision_brain::decision_speedctrl(Decition &gps_decition,const std::vector<GPSData> & gpsMapLine)
{
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;
}
//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)
{
iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
//如果useFrenet、useOldAvoid两者不互为相异，则采用原来的方法。“^”为异或运算符。
if(!(useFrenet^useOldAvoid)){
useFrenet = false;
useOldAvoid = true;
}
if (isFirstRun)
{
decision_firstRun(now_gps_ins,gpsMapLine);
}
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);
}
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;
}
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;
}
//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;
realspeed = now_gps_ins.speed;
secSpeed = realspeed / 3.6;
if(decision_ParkCalc(now_gps_ins,gps_decition) == 1)
{
return gps_decition;
}
switch (vehState) {
// ChuiZhiTingChe
case reverseCar:
return decision_reverseCar(now_gps_ins);
break;
case reversing:
return decision_reversing(now_gps_ins);
break;
case reverseCircle:
return decision_reverseCircle(now_gps_ins);
break;
case reverseDirect:
return decision_reverseDirect(now_gps_ins);
break;
case reverseArr:
return decision_reverseArr(now_gps_ins);
break;
//ceFangWeiBoChe
case dRever:
return decision_dRever(now_gps_ins);
break;
case dRever0:
return decision_dRever0(now_gps_ins);
break;
case dRever1:
return decision_dRever1(now_gps_ins);
break;
case dRever2:
return decision_dRever2(now_gps_ins);
break;
case dRever3:
return decision_dRever3(now_gps_ins);
break;
case dRever4:
return decision_dRever4(now_gps_ins);
break;
default:
break;
}
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;
}
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)
{
decision_readyZhucheMode(now_gps_ins,gpsMapLine);
}
if (readyParkMode)
{
decision_readyParkMode(now_gps_ins,gpsMapLine);
}
decision_speedctrl(gps_decition,gpsMapLine);
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
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;
}
}