#include #include #include #include #include "OpenDrive/OpenDrive.h" #include "OpenDrive/OpenDriveXmlParser.h" #include "globalplan.h" #include "gpsimu.pb.h" #include "v2x.pb.h" #include "modulecomm.h" #include "xmlparam.h" #include "gps_type.h" #include "xodrdijkstra.h" #include "gnss_coordinate_convert.h" #include "ivexit.h" #include "ivversion.h" OpenDrive mxodr; xodrdijkstra * gpxd; bool gmapload = false; double glat0,glon0,ghead0; double gvehiclewidth = 2.0; void * gpa; void * gpasrc; void * gpmap; void * gpagps; void * gpasimple; void * gpav2x; iv::Ivfault *gfault = nullptr; iv::Ivlog *givlog = nullptr; namespace iv { struct simpletrace { double gps_lat = 0;//纬度 double gps_lng = 0;//经度 double gps_x = 0; double gps_y = 0; double gps_z = 0; double ins_heading_angle = 0; //航向角 }; } /** * * * * * */ bool LoadXODR(std::string strpath) { OpenDriveXmlParser xp(&mxodr); xp.ReadFile(strpath); std::cout<<"road cout is "< xpath = pxd->getpath(10001,2,30012,2); // pxd->getgpspoint(10001,2,30012,2,xpath); int i; double nlenth = 0; int nroadsize = mxodr.GetRoadCount(); for(i=0;iGetRoadLength(); int bloksize = px->GetGeometryBlockCount(); if(px->GetGeometryBlock(0)->GetGeometryAt(0)->GetGeomType() == 4) { GeometryParamPoly3 * p = (GeometryParamPoly3 *) px->GetGeometryBlock(0)->GetGeometryAt(0); double a = p->GetuA(); a = p->GetuB(); a = p->GetuC(); a = p->GetuD(); a = p->GetvA(); } } // void Header::GetAllParams(unsigned short int &revMajor, unsigned short int &revMinor, string &name, float &version, string &date, // double &north, double &south, double &east,double &west,double &lat0,double &lon0, double & hdg0) unsigned short int revMajor,revMinor; std::string name,date; float version; double north,south,east,west,lat0,lon0,hdg0; if(mxodr.GetHeader() != 0) { mxodr.GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0); glat0 = lat0; glon0 = lon0; } Road * proad1 = mxodr.GetRoad(0); givlog->info("road name is %s",proad1->GetRoadName().data()); std::cout<<" road name is "<GetRoadName()< child; int nlen; }; #define EARTH_RADIUS 6370856.0 //从1到2的距离和方向 int CalcDisAngle(double lat1,double lon1,double lat2,double lon2,double * pLatDis,double * pLonDis,double * pDis,double * pangle) { double a,b; double LonDis,LatDis; double LonRadius; double Dis; double angle; if((lat1 == lat2)&&(lon1 == lon2))return -1; LonRadius = EARTH_RADIUS * cos(lat2/(180.0/M_PI)); a = (EARTH_RADIUS * M_PI*2.0/360.0)/(100000.0); b = lat2 - lat1; b = b*100000.0; LatDis = a*b; a = (LonRadius * M_PI*2.0/360.0)/100000.0; b = lon2 - lon1; b = b*100000.0; LonDis = a*b; Dis = sqrt(LatDis*LatDis + LonDis *LonDis); angle = acos(fabs(LonDis)/Dis); angle = angle * 180.0/M_PI; if(LonDis > 0) { if(LatDis > 0)angle = 90.0 - angle; else angle= 90.0+angle; } else { if(LatDis > 0)angle = 270.0+angle; else angle = 270.0-angle; } if(pLatDis != 0)*pLatDis = LatDis; if(pLonDis != 0)*pLonDis = LonDis; if(pDis != 0)*pDis = Dis; if(pangle != 0)*pangle = angle; } //========================================================== //高斯投影由经纬度(Unit:DD)反算大地坐标(含带号,Unit:Metres) //void GaussProjCal(double longitude, double latitude, double *X, double *Y) //{ // int ProjNo = 0; int ZoneWide; ////带宽 // double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval; // double a, f, e2, ee, NN, T, C, A, M, iPI; // iPI = 0.0174532925199433; ////3.1415926535898/180.0; // ZoneWide = 6; ////6度带宽 // a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数 // ////a=6378140.0; f=1/298.257; //80年西安坐标系参数 // ProjNo = (int)(longitude / ZoneWide); // longitude0 = ProjNo * ZoneWide + ZoneWide / 2; // longitude0 = longitude0 * iPI; // latitude0 = 0; // longitude1 = longitude * iPI; //经度转换为弧度 // latitude1 = latitude * iPI; //纬度转换为弧度 // e2 = 2 * f - f * f; // ee = e2 * (1.0 - e2); // NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1)); // T = tan(latitude1)*tan(latitude1); // C = ee * cos(latitude1)*cos(latitude1); // A = (longitude1 - longitude0)*cos(latitude1); // M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2 // *e2 / 1024)*sin(2 * latitude1) // + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1)); // xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120); // yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24 // + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720); // X0 = 1000000L * (ProjNo + 1) + 500000L; // Y0 = 0; // xval = xval + X0; yval = yval + Y0; // *X = xval; // *Y = yval; //} #include static void OffLngLat(double fLat0,double fLng0,double & fLat,double & fLng,double fHeading,double x,double y) { double fxdiff,fydiff; double xoff = y*(-1); double yoff = x; fxdiff = 0+xoff * cos(fHeading*M_PI/180.0)+ yoff*sin(fHeading*M_PI/180.0); //East fydiff = 0-xoff * sin(fHeading*M_PI/180.0)+ yoff*cos(fHeading*M_PI/180.0); //South double fEarthRadius = 6378245.0; double ns1d = fEarthRadius*2*M_PI/360.0; double fewRadius = fEarthRadius * cos(fLat0*M_PI/180.0); double ew1d = fewRadius * 2*M_PI/360.0; fLat = fLat0 + fydiff/ns1d; fLng = fLng0 + fxdiff/ew1d; } void CalcXY(const double lat0,const double lon0,const double hdg0, const double lat,const double lon, double & x,double & y) { double x0,y0; GaussProjCal(lon0,lat0,&x0,&y0); GaussProjCal(lon,lat,&x,&y); x = x - x0; y = y- y0; // double ang,dis; // CalcDisAngle(lat0,lon0,lat,lon,0,0,&dis,&ang); // double xang = hdg0 - ang; // while(xang<0)xang = xang + 360.0; // x = dis * cos(xang * M_PI/180.0); // y = dis * sin(xang * M_PI/180.0); } //void CalcLatLon(const double lat0,const double lon0,const double hdg0, // const double x,const double y,const double xyhdg, // double &lat,double & lon, double & hdg) //{ // OffLngLat(lat0,lon0,lat,lon,hdg0,x,y); // hdg = hdg0 - xyhdg * 180.0/M_PI; // while(hdg < 0)hdg = hdg + 360; // while(hdg >= 360)hdg = hdg - 360; //} void CalcLatLon(const double lat0,const double lon0, const double x,const double y, double &lat,double & lon) { double x0,y0; GaussProjCal(lon0,lat0,&x0,&y0); double x_gps,y_gps; x_gps = x0+x; y_gps = y0+y; GaussProjInvCal(x_gps,y_gps,&lon,&lat); } void CalcLatLon(const double lat0,const double lon0,const double hdg0, const double x,const double y,const double xyhdg, double &lat,double & lon, double & hdg) { double x0,y0; GaussProjCal(lon0,lat0,&x0,&y0); double x_gps,y_gps; x_gps = x0+x; y_gps = y0+y; GaussProjInvCal(x_gps,y_gps,&lon,&lat); // hdg = hdg0 -xyhdg * 270/M_PI; hdg = 90 - xyhdg* 180.0/M_PI; // OffLngLat(lat0,lon0,lat,lon,hdg0,x,y); // hdg = hdg0 - xyhdg * 180.0/M_PI; while(hdg < 0)hdg = hdg + 360; while(hdg >= 360)hdg = hdg - 360; } class xodrobj { public: double flatsrc; double flonsrc; double fhgdsrc; double flat; double flon; int lane; }; xodrobj gsrc; void ShareMap(std::vector navigation_data) { if(navigation_data.size()<1)return; iv::GPS_INS x; x = *(navigation_data.at(0)); char * data = new char[sizeof(iv::GPS_INS)*navigation_data.size()]; int gpssize = sizeof(iv::GPS_INS); int i; for(i=0;i100) { int nlast = 99; x = *(navigation_data.at(navigation_data.size()-1)); psim[nlast].gps_lat = x.gps_lat; psim[nlast].gps_lng = x.gps_lng; psim[nlast].gps_z = x.gps_z; psim[nlast].gps_x = x.gps_x; psim[nlast].gps_y = x.gps_y; psim[nlast].ins_heading_angle = x.ins_heading_angle; } iv::modulecomm::ModuleSendMsg(gpasimple,(char *)psim,nsize * sizeof(iv::simpletrace)); delete data; } static void ToGPSTrace(std::vector xPlan) { // double x_src,y_src,x_dst,y_dst; // x_src = -26;y_src = 10; // x_dst = -50;y_dst = -220; int i; int nSize = xPlan.size(); std::vector mapdata; QFile xfile; QString strpath; strpath = getenv("HOME"); strpath = strpath + "/map/maptrace.txt"; xfile.setFileName(strpath); xfile.open(QIODevice::ReadWrite); for(i=0;igps_lat, data->gps_lng,data->ins_heading_angle); data->index = i; data->speed = xPlan[i].speed; GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y); givlog->debug(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat, data->gps_lng,data->ins_heading_angle); data->roadSum = xPlan[i].mnLaneTotal; data->roadMode = 0; data->roadOri = xPlan[i].mnLaneori; data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft; data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft; data->mfLaneWidth = xPlan[i].mWidth; data->mfRoadWidth = xPlan[i].mfRoadWidth; data->mnLaneChangeMark = xPlan[i].lanmp; if(xPlan[i].lanmp == -1)data->roadMode = 15; if(xPlan[i].lanmp == 1)data->roadMode = 14; mapdata.push_back(data); char strline[255]; snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n", i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,0,0); xfile.write(strline); // fout << gps_index << "\t" << data->gps_lng << "\t" << data->gps_lat << "\t" << ServiceCarStatus.location->speed_mode << "\t" << ServiceCarStatus.location->mode2 << "\t" << data->ins_heading_angle << "\t" << obs_modes << "\t" << speed_modes << "\t" << lane_num << "\t" << lane_status <<"\t" < xPlan; double s; // x_src = -5;y_src = 6; // x_dst = -60;y_src = -150; int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - xo.fhgdsrc)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,xo.lane,xPlan); if(nRtn < 0) { qDebug("plan fail."); return; } int i; int nSize = xPlan.size(); std::vector mapdata; QFile xfile; QString strpath; strpath = getenv("HOME"); strpath = strpath + "/map/maptrace.txt"; xfile.setFileName(strpath); xfile.open(QIODevice::ReadWrite); for(i=0;igps_lat, data->gps_lng,data->ins_heading_angle); CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left, data->gps_lat_avoidleft,data->gps_lng_avoidleft); CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right, data->gps_lat_avoidright,data->gps_lng_avoidright); data->index = i; data->speed = xPlan[i].speed; // ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y); GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y); GaussProjCal(data->gps_lng_avoidleft,data->gps_lat_avoidleft,&data->gps_x_avoidleft,&data->gps_y_avoidleft); GaussProjCal(data->gps_lng_avoidright,data->gps_lat_avoidright,&data->gps_x_avoidright,&data->gps_y_avoidright); givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat, data->gps_lng,data->ins_heading_angle); data->roadOri = xPlan[i].mnLaneori; data->roadSum = xPlan[i].mnLaneTotal; data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft; data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft; data->mfLaneWidth = xPlan[i].mWidth; data->mfRoadWidth = xPlan[i].mfRoadWidth; data->mbInLaneAvoid = xPlan[i].bInlaneAvoid; data->mode2 = xPlan[i].nSignal; if(data->mode2 == 3000) { int k; for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--) { if(k<0)break; mapdata.at(k)->mode2 = 3000; } } // data->roadSum = 1; // data->roadMode = 0; // data->roadOri = 0; // if(xPlan[i].lanmp == -1)data->roadMode = 15; // if(xPlan[i].lanmp == 1)data->roadMode = 14; mapdata.push_back(data); char strline[255]; snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n", i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,0,0); xfile.write(strline); } xfile.close(); ShareMap(mapdata); s = 1; } void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double fsrcheading,int nlane) { std::vector xPlan; int i; double fLastHdg = 0; int ndeflane =nlane; for(i=0;istgps_size();i++) { double x_src,y_src,x_dst,y_dst; if(i==0) { CalcXY(glat0,glon0,ghead0,fsrclat,fsrclon,x_src,y_src); } else { CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i-1).lat(),pxv2x->stgps(i-1).lon(),x_src,y_src); } CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i).lat(),pxv2x->stgps(i).lon(),x_dst,y_dst); std::vector xPlanPart; double s; // x_src = -5;y_src = 6; // x_dst = -60;y_src = -150; int nRtn = -1; if(i==0) { nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - fsrcheading)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart); } else { nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,fLastHdg,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart); } if(nRtn < 0) { qDebug("plan fail."); return; } int j; for(j=0;j mapdata; QFile xfile; QString strpath; strpath = getenv("HOME"); strpath = strpath + "/map/maptrace.txt"; xfile.setFileName(strpath); xfile.open(QIODevice::ReadWrite); for(i=0;igps_lat, data->gps_lng,data->ins_heading_angle); data->index = i; data->speed = xPlan[i].speed; // ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y); GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y); givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat, data->gps_lng,data->ins_heading_angle); // data->roadSum = 1; // data->roadMode = 0; // data->roadOri = 0; // if(xPlan[i].lanmp == -1)data->roadMode = 15; // if(xPlan[i].lanmp == 1)data->roadMode = 14; data->roadOri = xPlan[i].mnLaneori; data->roadSum = xPlan[i].mnLaneTotal; data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft; data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft; data->mfLaneWidth = xPlan[i].mWidth; data->mfRoadWidth = xPlan[i].mfRoadWidth; data->mode2 = xPlan[i].nSignal; if(data->mode2 == 3000) { int k; for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--) { if(k<0)break; mapdata.at(k)->mode2 = 3000; } } mapdata.push_back(data); char strline[255]; snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n", i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,0,0); xfile.write(strline); } xfile.close(); ShareMap(mapdata); } void ListenCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { if(nSizedebug("lat is %f", xo.flat); xo.fhgdsrc = gsrc.fhgdsrc; xo.flatsrc = gsrc.flatsrc; xo.flonsrc = gsrc.flonsrc; SetPlan(xo); } void ListenV2X(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) { iv::v2x::v2x xv2x; if(!xv2x.ParseFromArray(strdata,nSize)) { givlog->warn("ListernV2X Parse Error."); std::cout<<"ListenV2X Parse Error."<debug("ListenV2X no gps station."); std::cout<<"ListenV2X no gps station."<warn("ListenSrc small"); std::cout<<"ListenSrc small."<debug("src hdg is %f", gsrc.fhgdsrc); std::cout<<"src hdg is "<warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize); return; } xodrobj xo; xo.fhgdsrc = xgpsimu.heading(); xo.flatsrc = xgpsimu.lat(); xo.flonsrc = xgpsimu.lon(); gsrc = xo; givlog->debug("src hdg is %f", gsrc.fhgdsrc); std::cout<<"src hdg is "< xPlan; double s; // int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,3.14,x_dst,y_dst,s,30.0,100.0,1,xPlan); // ToGPSTrace(xPlan); // double lat = 39.1443880; // double lon = 117.0812543; // xodrobj xo; // xo.fhgdsrc = 340; // xo.flatsrc = lat; xo.flonsrc = lon; // xo.flat = 39.1490196; // xo.flon = 117.0806979; // xo.lane = 1; // SetPlan(xo); void * pivexit = iv::ivexit::RegIVExitCall(0); return a.exec(); }