collectconvert.cpp 25 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765
  1. #include "collectconvert.h"
  2. #include "gnss_coordinate_convert.h"
  3. #include "geofit.h"
  4. #include "autoconnect.h"
  5. int groadid = 10001;
  6. CollectConvert::CollectConvert(iv::map::collectveh & xcollectveh,OpenDrive * pxodr)
  7. {
  8. mcollectveh.CopyFrom(xcollectveh);
  9. mpxodr = pxodr;
  10. }
  11. void CollectConvert::StartConvert()
  12. {
  13. threadconvert();
  14. }
  15. void CollectConvert::threadconvert()
  16. {
  17. OpenDrive * pxodr = mpxodr;
  18. pxodr->SetHeader(1,1,"adc",1.1,"2022",0,0,0,0,mcollectveh.mlat0(),mcollectveh.mlon0(),360);
  19. int i;
  20. for(i=0;i<mcollectveh.mroads_size();i++)
  21. {
  22. iv::map::collectvehroad * proad = mcollectveh.mutable_mroads(i);
  23. ConvertRoad(proad,pxodr);
  24. }
  25. for(i=0;i<mcollectveh.mconnects_size();i++)
  26. {
  27. iv::map::roadconnect * pconnect = mcollectveh.mutable_mconnects(i);
  28. convertconnect(pconnect,pxodr);
  29. }
  30. AutoConnect pAC(pxodr);
  31. pAC.Connect();
  32. }
  33. void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxodr)
  34. {
  35. if(proad->mpoints_size() == 0)return;
  36. double flon0,flat0;
  37. pxodr->GetHeader()->GetLat0Lon0(flat0,flon0);
  38. // pxodr->GetHeader()->GetLat0Lon0(flon0,flat0);
  39. double x0,y0;
  40. GaussProjCal(flon0,flat0,&x0,&y0);
  41. int i;
  42. std::vector<iv::collectxy> xvectorcollectxy;
  43. for(i=0;i<proad->mpoints_size();i++)
  44. {
  45. iv::map::collectvehroadpoint * ppoint = proad->mutable_mpoints(i);
  46. double x,y;
  47. // double fl1 = ppoint->mflon();
  48. // double fl2 = ppoint->mflat();
  49. GaussProjCal(ppoint->mflon(),ppoint->mflat(),&x,&y);
  50. iv::collectxy xc;
  51. xc.mLaneColor_Left = ppoint->mlanecolor_left();
  52. xc.mLaneColor_Right = ppoint->mlanecolor_right();
  53. xc.mLaneType_Left = ppoint->mlanetype_left();
  54. xc.mLaneType_Right = ppoint->mlanetype_right();
  55. xc.height = 0;
  56. xc.width = ppoint->mflanewidth();
  57. xc.fhdg = (90 -ppoint->mfheading())*M_PI/180.0;
  58. xc.x = x-x0 + ppoint->mfdis_left()*cos(xc.fhdg +M_PI/2.0);
  59. xc.y = y-y0 + ppoint->mfdis_left()*sin(xc.fhdg +M_PI/2.0);
  60. xvectorcollectxy.push_back(xc);
  61. if(i>0)
  62. {
  63. double fdis =sqrt(pow(xvectorcollectxy[i].x - xvectorcollectxy[i-1].x,2)+pow(xvectorcollectxy[i].y-xvectorcollectxy[i-1].y,2));
  64. xvectorcollectxy[i].fdistolast = fdis;
  65. }
  66. }
  67. double LINE_ERROR = 0.20;
  68. std::vector<geobase> xvectorgeo;
  69. bool bComplete = false;
  70. int j;
  71. int ncurpos = 0;
  72. int nrange = xvectorcollectxy.size();
  73. bool bHaveLastxyhdg = false;
  74. double fLastX,fLastY,fLastHdg;
  75. double fEndX,fEndY,fEndHdg;
  76. double fStartX,fStartY,fStartHdg;
  77. while(bComplete == false)
  78. {
  79. VectorXd x_veh(nrange);
  80. VectorXd y_veh(nrange);
  81. for(j=0;j<nrange;j++)
  82. {
  83. x_veh[j] =xvectorcollectxy[j+ncurpos].x;// xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fx;//mvectorrtkdata.at(j+ncurpos).mfrelx;
  84. y_veh[j] = xvectorcollectxy[j+ncurpos].y;//xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fy;//mvectorrtkdata.at(j+ncurpos).mfrely;
  85. }
  86. bool bArcOk = false;
  87. bool bLineOk = false;
  88. double dismax = 0;
  89. QPointF sp,ep;
  90. double fR,flen;
  91. double fhdgstart,fhdgend;
  92. if(nrange >= 3)
  93. {
  94. geofit::Inst().arcfitanddis(x_veh,y_veh,dismax,fR,sp,ep,fhdgstart,fhdgend,flen);
  95. if(dismax < LINE_ERROR)
  96. {
  97. bArcOk = true;
  98. // std::cout<<" a arc is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
  99. }
  100. }
  101. auto coeffs = polyfit(x_veh, y_veh, 1);
  102. dismax = 0;
  103. for(j=0;j<nrange;j++)
  104. {
  105. double A = coeffs[1];
  106. double B = -1;
  107. double C = coeffs[0];
  108. double dis = fabs(A*x_veh[j] + B*y_veh[j] +C )/sqrt(pow(A,2)+pow(B,2));
  109. if(dis>dismax)dismax = dis;
  110. }
  111. if(dismax < LINE_ERROR)
  112. {
  113. bLineOk = true;
  114. // std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
  115. }
  116. if((nrange <= 2)||bLineOk||bArcOk)
  117. {
  118. if((bLineOk)||(nrange<=2))
  119. {
  120. // std::cout<<"use line"<<std::endl;
  121. geobase xgeo;
  122. xgeo.mnStartPoint = ncurpos;
  123. xgeo.mnEndPoint = ncurpos + nrange -1 ;
  124. double x0,y0,x1,y1;
  125. geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[0],
  126. y_veh[0],x0,y0);
  127. geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[nrange -1],
  128. y_veh[nrange - 1],x1,y1);
  129. xgeo.mfA = coeffs[1];
  130. xgeo.mfB = -1;
  131. xgeo.mfC = coeffs[0];
  132. xgeo.mfHdg = geofit::Inst().CalcHdg(x0,y0,x1,y1);
  133. xgeo.mfX = x0;
  134. xgeo.mfY = y0;
  135. xgeo.mfLen = sqrt(pow(x1-x0,2)+pow(y1-y0,2));
  136. xgeo.mnType = 0;
  137. fStartX = x0;
  138. fStartY = y0;
  139. fStartHdg = xgeo.mfHdg;
  140. fEndX = x1;
  141. fEndY = y1;
  142. fEndHdg = xgeo.mfHdg;
  143. xvectorgeo.push_back(xgeo);
  144. }
  145. else
  146. {
  147. if(bArcOk)
  148. {
  149. std::cout<<"use arc"<<std::endl;
  150. geobase xgeo;
  151. xgeo.mnStartPoint = ncurpos;
  152. xgeo.mnEndPoint = ncurpos + nrange -1;
  153. xgeo.mfHdg = fhdgstart;
  154. xgeo.mfHdgStart = fhdgstart;
  155. xgeo.mfHdgEnd = fhdgend;
  156. xgeo.mfX = sp.x();
  157. xgeo.mfY = sp.y();
  158. xgeo.mfLen = flen;
  159. xgeo.mnType = 1;
  160. xgeo.mfEndX = ep.x();
  161. xgeo.mfEndY = ep.y();
  162. xgeo.mR = fR;
  163. fStartX = xgeo.mfX;
  164. fStartY = xgeo.mfY;
  165. fStartHdg = xgeo.mfHdgStart;
  166. fEndX = xgeo.mfEndX;
  167. fEndY = xgeo.mfEndY;
  168. fEndHdg = xgeo.mfHdgEnd;
  169. xvectorgeo.push_back(xgeo);
  170. }
  171. }
  172. // std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
  173. if(bHaveLastxyhdg)
  174. {
  175. std::vector<geobase> xvectorgeobe = geofit::CreateBezierGeo(fLastX,fLastY,
  176. fLastHdg,fStartX,fStartY,fStartHdg);
  177. if(xvectorgeobe.size()>0)
  178. {
  179. xvectorgeobe[0].mnStartPoint = ncurpos-1;
  180. xvectorgeobe[0].mnEndPoint = ncurpos;
  181. xvectorgeo.insert(xvectorgeo.begin()+(xvectorgeo.size()-1),xvectorgeobe[0]);
  182. }
  183. }
  184. bHaveLastxyhdg = true;
  185. fLastX = fEndX;
  186. fLastY = fEndY;
  187. fLastHdg = fEndHdg;
  188. // ncurpos = ncurpos + nrange -1;
  189. ncurpos = ncurpos + nrange; //Skip 1 point, use bezier
  190. nrange = xvectorcollectxy.size() - ncurpos;
  191. }
  192. else
  193. {
  194. if(nrange > 30)
  195. nrange = nrange/2;
  196. else
  197. nrange = nrange -1;
  198. if(nrange<2)nrange = 2;
  199. }
  200. if(ncurpos>=(int)(xvectorcollectxy.size()-1))
  201. {
  202. if(ncurpos == (int)(xvectorcollectxy.size()-1))
  203. {
  204. // std::cout<<"Last section, 2 Points, use line."<<std::endl;
  205. VectorXd x_veh1(2);
  206. VectorXd y_veh1(2);
  207. x_veh1[0] = fLastX;
  208. y_veh1[0] = fLastY;
  209. x_veh1[1] = xvectorcollectxy[ncurpos].x;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].fx;
  210. y_veh1[1] = xvectorcollectxy[ncurpos].y;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].fy;
  211. auto coeffs = polyfit(x_veh1, y_veh1, 1);
  212. geobase xgeo;
  213. xgeo.mnStartPoint = ncurpos-1;
  214. xgeo.mnEndPoint = ncurpos;
  215. xgeo.mfA = coeffs[1];
  216. xgeo.mfB = -1;
  217. xgeo.mfC = coeffs[0];
  218. xgeo.mfHdg = geofit::Inst().CalcHdg(x_veh1[0],y_veh1[0],x_veh1[1],y_veh1[1]);
  219. xgeo.mfX = x_veh1[0];
  220. xgeo.mfY = y_veh1[0];
  221. xgeo.mfLen = sqrt(pow(x_veh1[1]-x_veh1[0],2)+pow(y_veh1[1]-y_veh1[0],2));
  222. xgeo.mnType = 0;
  223. xvectorgeo.push_back(xgeo);
  224. }
  225. bComplete = true;
  226. }
  227. }
  228. double snow = 0;
  229. double froadlen = 0;
  230. for(i=0;i<(int)xvectorgeo.size();i++)
  231. {
  232. geobase * pgeo = &xvectorgeo[i];
  233. froadlen = froadlen + xvectorgeo[i].mfLen;
  234. // std::cout<<"geo len: "<<pgeo->mfLen<<" type"<<pgeo->mnType<<std::endl;
  235. double tems = 0;
  236. xvectorcollectxy[pgeo->mnStartPoint].s = snow;
  237. for(j=(pgeo->mnStartPoint+1);j<=pgeo->mnEndPoint;j++)
  238. {
  239. xvectorcollectxy[j].s = snow + tems+xvectorcollectxy[j].fdistolast;
  240. // xvectorvehicle[xvectorvl[j].mvehindex].s = snow + tems + xvectorvehicle[xvectorvl[j].mvehindex].fdistolast;
  241. tems = tems + xvectorcollectxy[j].fdistolast;
  242. }
  243. snow = snow + pgeo->mfLen;
  244. }
  245. std::vector<iv::widthabcd> xvectorwidthabcd;
  246. bComplete = false;
  247. ncurpos = 0;
  248. nrange = xvectorcollectxy.size();
  249. double flanewidtherror = 0.1; //1cm
  250. while(bComplete == false)
  251. {
  252. double ele_coff[4];
  253. for(j=0;j<4;j++)ele_coff[j] = 0;
  254. if(nrange>0)ele_coff[0] = xvectorcollectxy[ncurpos].width;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].flanewidth;
  255. int M = nrange;
  256. VectorXd x_vehhg(M);
  257. VectorXd y_vehhg(M);
  258. for(j=0;j<M;j++)
  259. {
  260. x_vehhg[j] = xvectorcollectxy[ncurpos+j].s - xvectorcollectxy[ncurpos].s;// xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].s - xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
  261. y_vehhg[j] = xvectorcollectxy[ncurpos+j].width;// xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].flanewidth;
  262. }
  263. int MX = 3;
  264. if(M<4)MX = M -1;
  265. if(MX>3)MX = 3;
  266. if(MX>0)
  267. {
  268. VectorXd coeffs = polyfit(x_vehhg, y_vehhg, MX);
  269. for(j=0;j<=MX;j++)
  270. {
  271. ele_coff[j] = coeffs[j];
  272. }
  273. }
  274. double ferror = 0;
  275. for(j=0;j<M;j++)
  276. {
  277. double s = x_vehhg[j];
  278. double fvalue = fabs(ele_coff[0] + ele_coff[1]*s + ele_coff[2]*s*s+ele_coff[3]*s*s*s - y_vehhg[j]);
  279. if(fvalue>ferror)ferror = fvalue;
  280. }
  281. if(ferror>flanewidtherror)
  282. {
  283. if(nrange>10)nrange = nrange/2;
  284. else nrange = nrange -1;
  285. }
  286. else
  287. {
  288. iv::widthabcd xabcd;
  289. xabcd.s = xvectorcollectxy[ncurpos].s;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
  290. xabcd.A = ele_coff[0];
  291. xabcd.B = ele_coff[1];
  292. xabcd.C = ele_coff[2];
  293. xabcd.D = ele_coff[3];
  294. xvectorwidthabcd.push_back(xabcd);
  295. ncurpos = ncurpos + nrange-1;
  296. nrange = xvectorcollectxy.size() - ncurpos;
  297. }
  298. if(ncurpos>=(int)(xvectorcollectxy.size()-1))
  299. {
  300. bComplete = true;
  301. }
  302. }
  303. pxodr->AddRoad(proad->strroadname(),froadlen, QString::number(groadid).toStdString(),"-1");
  304. Road * p = pxodr->GetRoad(pxodr->GetRoadCount() - 1);
  305. groadid++;
  306. // p->AddElevation(0,xlaneheightcoff.A,xlaneheightcoff.B,xlaneheightcoff.C,xlaneheightcoff.D);
  307. double s = 0;
  308. j= 0;
  309. // for(j=0;j<4;j++)
  310. for(j=0;j<(int)xvectorgeo.size();j++)
  311. {
  312. p->AddGeometryBlock();
  313. GeometryBlock * pgb = p->GetGeometryBlock(j);
  314. geobase * pline;
  315. geobase * pbez;
  316. geobase * parc;
  317. switch(xvectorgeo[j].mnType)
  318. {
  319. case 0:
  320. pline = &xvectorgeo[j];
  321. pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
  322. break;
  323. case 1:
  324. parc = &xvectorgeo[j];
  325. pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
  326. break;
  327. case 2:
  328. pbez = &xvectorgeo[j];
  329. // std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
  330. pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
  331. pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
  332. pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
  333. pbez->mfv[1],pbez->mfv[2],pbez->mfv[3],false);
  334. break;
  335. }
  336. s = s + xvectorgeo[j].mfLen;
  337. }
  338. p->AddLaneSection(0);
  339. LaneSection * pLS = p->GetLaneSection(0);
  340. pLS->AddLane(0,0,"none",false);
  341. Lane * pCenterLane = pLS->GetLastAddedLane();
  342. iv::map::collectvehroadpoint_LaneType xLastType;
  343. if(xvectorcollectxy.size()>0)
  344. {
  345. xLastType = xvectorcollectxy[0].mLaneType_Left;
  346. std::string strlanecolor = "white";
  347. if(xvectorcollectxy[0].mLaneColor_Left == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
  348. pCenterLane->AddRoadMarkRecord(xvectorcollectxy[0].s,LaneTypeToStr(xLastType),
  349. "standard",strlanecolor,0.15,"none");
  350. }
  351. for(i=1;i<(int)xvectorcollectxy.size();i++)
  352. {
  353. if(xvectorcollectxy[i].mLaneType_Left != xLastType)
  354. {
  355. xLastType = xvectorcollectxy[i].mLaneType_Left;
  356. std::string strlanecolor = "white";
  357. if(xvectorcollectxy[i].mLaneColor_Left == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
  358. pCenterLane->AddRoadMarkRecord(xvectorcollectxy[i].s,LaneTypeToStr(xLastType),
  359. "standard",strlanecolor,0.15,"none");
  360. }
  361. }
  362. pLS->AddLane(-1,(-1)*(pLS->GetRightLaneCount()+1),"driving",false);
  363. Lane * pLane = pLS->GetLastRightLane();
  364. for(j=0;j<(int)xvectorwidthabcd.size();j++)
  365. {
  366. iv::widthabcd * pwa = &xvectorwidthabcd[j];
  367. pLane->AddWidthRecord(pwa->s,pwa->A,pwa->B,pwa->C,pwa->D);
  368. }
  369. if(xvectorcollectxy.size()>0)
  370. {
  371. xLastType = xvectorcollectxy[0].mLaneType_Right;
  372. std::string strlanecolor = "white";
  373. if(xvectorcollectxy[0].mLaneColor_Right == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
  374. pLane->AddRoadMarkRecord(xvectorcollectxy[0].s,LaneTypeToStr(xLastType),
  375. "standard",strlanecolor,0.15,"none");
  376. }
  377. for(i=1;i<(int)xvectorcollectxy.size();i++)
  378. {
  379. if(xvectorcollectxy[i].mLaneType_Right != xLastType)
  380. {
  381. xLastType = xvectorcollectxy[i].mLaneType_Right;
  382. std::string strlanecolor = "white";
  383. if(xvectorcollectxy[i].mLaneColor_Right == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
  384. pLane->AddRoadMarkRecord(xvectorcollectxy[i].s,LaneTypeToStr(xLastType),
  385. "standard",strlanecolor,0.15,"none");
  386. }
  387. }
  388. }
  389. std::string CollectConvert::LaneTypeToStr(iv::map::collectvehroadpoint_LaneType xType)
  390. {
  391. switch (xType) {
  392. case iv::map::collectvehroadpoint_LaneType_NONE:
  393. return "none";
  394. break;
  395. case iv::map::collectvehroadpoint_LaneType_SOLID:
  396. return "solid";
  397. break;
  398. case iv::map::collectvehroadpoint_LaneType_DASH:
  399. return "broken";
  400. break;
  401. case iv::map::collectvehroadpoint_LaneType_SOLID_SOLID:
  402. return "solid solid";
  403. break;
  404. case iv::map::collectvehroadpoint_LaneType_SOLID_DASH:
  405. return "solid broken";
  406. break;
  407. case iv::map::collectvehroadpoint_LaneType_DASH_SOLID:
  408. return "broken solid";
  409. break;
  410. case iv::map::collectvehroadpoint_LaneType_DASH_DASH:
  411. return "broken broken";
  412. break;
  413. default:
  414. return "none";
  415. break;
  416. }
  417. }
  418. void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *pxodr)
  419. {
  420. Road * p1, * p2;
  421. std::string strroad1name = pconnect->strroad1();
  422. std::string strroad2name = pconnect->strroad2();
  423. int i;
  424. p1 = NULL;
  425. p2 = NULL;
  426. double fwidth1,fwidth2;
  427. for(i=0;i<(int)pxodr->GetRoadCount();i++)
  428. {
  429. if(pxodr->GetRoad(i)->GetRoadName() == strroad1name)
  430. {
  431. p1 = pxodr->GetRoad(i);
  432. break;
  433. }
  434. }
  435. for(i=0;i<(int)pxodr->GetRoadCount();i++)
  436. {
  437. if(pxodr->GetRoad(i)->GetRoadName() == strroad2name)
  438. {
  439. p2 = pxodr->GetRoad(i);
  440. break;
  441. }
  442. }
  443. if((p1 == NULL)||(p2 == NULL))
  444. {
  445. std::cout<<"convertconnect can not find road. "<<std::endl;
  446. return;
  447. }
  448. double startx,starty,starthdg;
  449. double endx,endy,endhdg;
  450. p1->GetGeometryCoords(p1->GetRoadLength() - 0.001,startx,starty,starthdg);
  451. p2->GetGeometryCoords(0,endx,endy,endhdg);
  452. // std::cout<<"start x : "<<startx<<" start y: "<<starty<<std::endl;
  453. LaneSection * pLS1 = p1->GetLastLaneSection();
  454. if(pLS1 == NULL)
  455. {
  456. std::cout<<" road p1 no lanesection."<<std::endl;
  457. return;
  458. }
  459. Lane * pLane = NULL;
  460. for(i=0;i<(int)pLS1->GetLaneCount();i++)
  461. {
  462. Lane * pLaneX = pLS1->GetLane(i);
  463. if(pLaneX->GetId() == -1)
  464. {
  465. pLane = pLaneX;
  466. break;
  467. }
  468. }
  469. if(pLane == NULL)
  470. {
  471. std::cout<<"can't get road1 lane."<<std::endl;
  472. return;
  473. }
  474. fwidth1 = pLane->GetWidthValue(p1->GetRoadLength() - 0.001);
  475. pLS1 = p2->GetLaneSection(0);
  476. if(pLS1 == NULL)
  477. {
  478. std::cout<<" road p2 no lanesection."<<std::endl;
  479. return;
  480. }
  481. pLane = NULL;
  482. for(i=0;i<(int)pLS1->GetLaneCount();i++)
  483. {
  484. Lane * pLaneX = pLS1->GetLane(i);
  485. if(pLaneX->GetId() == -1)
  486. {
  487. pLane = pLaneX;
  488. break;
  489. }
  490. }
  491. if(pLane == NULL)
  492. {
  493. std::cout<<"can't get road2 lane."<<std::endl;
  494. return;
  495. }
  496. fwidth2 = pLane->GetWidthValue(0.001);
  497. //Create Geo
  498. double R = 6.0;
  499. std::vector<geobase> xvectorgeo;
  500. std::vector<geobase> xvectorgeo1,xvectorgeo2;
  501. switch(pconnect->mmode())
  502. {
  503. case iv::map::roadconnect_ConnectMode_TURN:
  504. xvectorgeo = geofit::CreateTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
  505. break;
  506. case iv::map::roadconnect_ConnectMode_STRAIGHT:
  507. {
  508. double fdis = sqrt(pow(startx - endx,2) +pow(starty -endy,2));
  509. if((fdis<3) || (starthdg == endhdg) )
  510. {
  511. xvectorgeo = geofit::CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
  512. }
  513. else
  514. {
  515. xvectorgeo = geofit::CreateBezierGeo(startx,starty,starthdg,endx,endy,endhdg);
  516. }
  517. //
  518. }
  519. break;
  520. case iv::map::roadconnect_ConnectMode_UTURN:
  521. xvectorgeo = geofit::CreateUTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
  522. break;
  523. default:
  524. break;
  525. }
  526. if(pconnect->mmode() == iv::map::roadconnect_ConnectMode_UTURN)
  527. {
  528. for(i=0;i<(int)xvectorgeo.size()/2;i++)
  529. {
  530. xvectorgeo1.push_back(xvectorgeo.at(i));
  531. }
  532. for(i=(int)xvectorgeo.size()/2;i<(int)xvectorgeo.size();i++)
  533. {
  534. xvectorgeo2.push_back(xvectorgeo.at(i));
  535. }
  536. }
  537. if(xvectorgeo.size() == 0)
  538. {
  539. std::cout<<" CollectConvert::convertconnect Create Road Fail."<<std::endl;
  540. // QMessageBox::warning(this,"warn","Create Road Fail.");
  541. return;
  542. }
  543. double xroadlen = 0;
  544. if(pconnect->mmode() != iv::map::roadconnect_ConnectMode_UTURN)
  545. {
  546. for(i=0;i<(int)xvectorgeo.size();i++)xroadlen = xroadlen + xvectorgeo[i].mfLen;
  547. pxodr->AddRoad("",xroadlen, QString::number(groadid).toStdString(),"-1");
  548. groadid++;
  549. Road * p = pxodr->GetRoad(pxodr->GetRoadCount() - 1);
  550. // p->AddElevation(0,startheight,(endheight-startheight)/xroadlen,0,0);
  551. double s = 0;
  552. int j;
  553. j= 0;
  554. for(j=0;j<(int)xvectorgeo.size();j++)
  555. {
  556. p->AddGeometryBlock();
  557. GeometryBlock * pgb = p->GetGeometryBlock(j);
  558. geobase * pline;
  559. geobase * pbez;
  560. geobase * parc;
  561. switch(xvectorgeo[j].mnType)
  562. {
  563. case 0:
  564. pline = &xvectorgeo[j];
  565. pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
  566. break;
  567. case 1:
  568. parc = &xvectorgeo[j];
  569. pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
  570. break;
  571. case 2:
  572. pbez = &xvectorgeo[j];
  573. pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
  574. pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
  575. pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
  576. pbez->mfv[1],pbez->mfv[2],pbez->mfv[3],false);
  577. break;
  578. }
  579. s = s + xvectorgeo[j].mfLen;
  580. }
  581. p->AddLaneSection(0);
  582. LaneSection * pLS = p->GetLaneSection(0);
  583. pLS->SetS(0);
  584. pLS->AddLane(0,0,"none",false);
  585. double b = (fwidth2 - fwidth1)/xroadlen;
  586. pLS->AddLane(-1,-1,"driving",false);
  587. pLane = pLS->GetLastAddedLane();
  588. pLane->AddWidthRecord(0,fwidth1,b,0,0);
  589. }
  590. else
  591. {
  592. double xroadlen1 = 0;
  593. double xroadlen2 = 0;
  594. for(i=0;i<(int)xvectorgeo1.size();i++)xroadlen1 = xroadlen1 + xvectorgeo1[i].mfLen;
  595. for(i=0;i<(int)xvectorgeo2.size();i++)xroadlen2 = xroadlen2 + xvectorgeo2[i].mfLen;
  596. int index1 = pxodr->AddRoad("",xroadlen1, QString::number(groadid).toStdString(),"-1");groadid++;
  597. int index2 = pxodr->AddRoad("",xroadlen2, QString::number(groadid).toStdString(),"-1");groadid++;
  598. Road * proad2 = pxodr->GetRoad(index2);
  599. Road * proad1 = pxodr->GetRoad(index1);
  600. // OpenDrive * px = &mxodr;
  601. double s = 0;
  602. int j;
  603. j= 0;
  604. for(j=0;j<(int)xvectorgeo1.size();j++)
  605. {
  606. proad1->AddGeometryBlock();
  607. GeometryBlock * pgb = proad1->GetGeometryBlock(j);
  608. geobase * pline;
  609. geobase * pbez;
  610. geobase * parc;
  611. switch(xvectorgeo1[j].mnType)
  612. {
  613. case 0:
  614. pline = &xvectorgeo1[j];
  615. pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
  616. break;
  617. case 1:
  618. parc = &xvectorgeo1[j];
  619. pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
  620. break;
  621. case 2:
  622. pbez = &xvectorgeo1[j];
  623. std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
  624. pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
  625. pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
  626. pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
  627. pbez->mfv[1],pbez->mfv[2],pbez->mfv[3],false);
  628. break;
  629. }
  630. s = s + xvectorgeo1[j].mfLen;
  631. }
  632. s=0.0;
  633. for(j=0;j<(int)xvectorgeo2.size();j++)
  634. {
  635. proad2->AddGeometryBlock();
  636. GeometryBlock * pgb = proad2->GetGeometryBlock(j);
  637. geobase * pline;
  638. geobase * pbez;
  639. geobase * parc;
  640. switch(xvectorgeo2[j].mnType)
  641. {
  642. case 0:
  643. pline = &xvectorgeo2[j];
  644. pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
  645. break;
  646. case 1:
  647. parc = &xvectorgeo2[j];
  648. pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
  649. break;
  650. case 2:
  651. pbez = &xvectorgeo2[j];
  652. std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
  653. pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
  654. pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
  655. pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
  656. pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
  657. break;
  658. }
  659. s = s + xvectorgeo2[j].mfLen;
  660. }
  661. proad1->AddLaneSection(0);
  662. LaneSection * pLS1 = proad1->GetLaneSection(0);
  663. pLS1->SetS(0);
  664. pLS1->AddLane(0,0,"none",false);
  665. proad2->AddLaneSection(0);
  666. LaneSection * pLS2 = proad2->GetLaneSection(0);
  667. pLS2->SetS(0);
  668. pLS2->AddLane(0,0,"none",false);
  669. double b1,b2;
  670. double fwidth3;
  671. fwidth3 = fwidth1 + (fwidth2 - fwidth1)*(xroadlen1)/(xroadlen1+xroadlen2);
  672. b1 = (fwidth3 - fwidth1)/xroadlen1;
  673. b2 = (fwidth2 - fwidth3)/xroadlen2;
  674. pLS1->AddLane(-1,-1,"driving",false);
  675. pLane = pLS1->GetLastAddedLane();
  676. pLane->AddWidthRecord(0,fwidth1,b1,0,0);
  677. pLS2->AddLane(-1,-1,"driving",false);
  678. pLane = pLS2->GetLastAddedLane();
  679. pLane->AddWidthRecord(0,fwidth3,b2,0,0);
  680. }
  681. }