sideparkcalc.cpp 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566
  1. #include "sideparkcalc.h"
  2. #include "math.h"
  3. #include <iostream>
  4. SideParkCalc::SideParkCalc(double x,double y,double hdg)
  5. {
  6. mx = x;
  7. my = y;
  8. mhdg = hdg;
  9. normalhdg(mhdg);
  10. mSideParkType = SideParkType::NoSolution;
  11. }
  12. void SideParkCalc::CalcPark()
  13. {
  14. if(mx< 0.5)
  15. {
  16. return;
  17. }
  18. if(fabs(mhdg) > M_PI/6.0)
  19. {
  20. return;
  21. }
  22. SideParkMode parkmode = CalcParkMode();
  23. if(parkmode == SideParkMode::ParkAtRight)
  24. {
  25. ParkAtRightCalc();
  26. }
  27. else
  28. ParkAtLeftCalc();
  29. }
  30. SideParkMode SideParkCalc::CalcParkMode()
  31. {
  32. if(my>=0)return SideParkMode::ParkAtRight;
  33. return SideParkMode::ParkAtLeft;
  34. }
  35. void SideParkCalc::ParkAtRightCalc()
  36. {
  37. if(mhdg<=0.0)
  38. {
  39. ParkAtRightCalc_Model1();
  40. }
  41. else
  42. {
  43. ParkAtRightCalc_Model2();
  44. }
  45. }
  46. void SideParkCalc::ParkAtRightCalc_Model1()
  47. {
  48. double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5,dx6,dy6;
  49. dy6 = 0;
  50. dx6 = mfLastDirectDis;
  51. // double fMaxdx5 = mfRaidus * sin(mfMaxAngle );
  52. double fMaxdy5 = mfRaidus *(1.0 - cos(mfMaxAngle));
  53. // double fMaxdx3 = fMaxdx5;
  54. // double fMaxdy3 = fMaxdy5;
  55. dx2 = mfRaidus * sin(fabs(mhdg));
  56. dy2 = mfRaidus * (1 - cos(fabs(mhdg)));
  57. double fang ;//= mfMaxAngle;
  58. if((my+dy2) >= (2*fMaxdy5) )
  59. {
  60. fang = mfMaxAngle;
  61. }
  62. else
  63. {
  64. fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
  65. }
  66. dx5 = mfRaidus * sin(fang);
  67. dy5 = mfRaidus * (1.0 - cos(fang));
  68. dx3 = dx5;
  69. dy3 = dy5;
  70. double thetax = mx - dx6 - dx3 - dx5 - dx2;
  71. if(fabs(tan(fabs(mhdg )) + tan(fang ))<0.0001)
  72. {
  73. std::cout<<" divide error."<<std::endl;
  74. return;
  75. }
  76. dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
  77. dx1 = thetax - dx4;
  78. dy1 = dx1 * tan(fabs(mhdg));
  79. dy4 = my + dy1 +dy2 -dy3-dy5;
  80. std::cout<<" compute. "<<std::endl;
  81. if(dx1>(-0.1)&&(dx4>(-0.1)))
  82. {
  83. mSideParkType = SideParkType::FiveStep;
  84. mvectorWheel.push_back(0.0);
  85. mvectorWheel.push_back(mfMaxWheel * (-1.0));
  86. mvectorWheel.push_back(0.0);
  87. mvectorWheel.push_back(mfMaxWheel);
  88. mvectorWheel.push_back(0.0);
  89. double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
  90. x4 = dx6;y4 = dy6;hdg4 = 0;flen5 = x4;
  91. x3 = x4 + dx5;y3 = y4 + dy5;hdg3 = fang;flen4 = fabs(fang) *mfRaidus;
  92. hdg2 = hdg3;
  93. if(dx4>0.1)
  94. {
  95. x2 = x3 + dx4;y2 = y3 + dy4;
  96. flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
  97. }
  98. else
  99. {
  100. x2 = x3;y2 = y3;flen3 = 0;
  101. }
  102. flen2 = mfRaidus * (fang - mhdg);
  103. x1 = mx - dx1;
  104. y1 = my - dy1;
  105. hdg1 = mhdg;
  106. flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
  107. mvectorlen.push_back(flen1);
  108. mvectorlen.push_back(flen2);
  109. mvectorlen.push_back(flen3);
  110. mvectorlen.push_back(flen4);
  111. mvectorlen.push_back(flen5);
  112. mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
  113. mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
  114. mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
  115. mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
  116. mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
  117. }
  118. // if((my+dy2) > (2*fMaxdy3) )
  119. // {
  120. // dy3 = fMaxdy3;
  121. // dx3 = fMaxdx3;
  122. // dx5 = fMaxdx5;
  123. // dy5 = fMaxdy5;
  124. // double thetax = mx - dx6 - fMaxdx3 - fMaxdx5 - dx2;
  125. // dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - fMaxdy3 - fMaxdy5)/(tan(fabs(mhdg ) + tan(mfMaxAngle )));
  126. // dx1 = thetax - dx4;
  127. // dy1 = dx1 * tan(fabs(mhdg));
  128. // dy4 = my + dy1 + dy2 -dy3-dy5;
  129. // std::cout<<" compute. "<<std::endl;
  130. // }
  131. // else
  132. // {
  133. // double fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
  134. // dx5 = mfRaidus * sin(fang);
  135. // dy5 = mfRaidus * (1.0 - cos(fang));
  136. // dx3 = dx5;
  137. // dy3 = dy5;
  138. // double thetax = mx - dx6 - dx3 - dx5 - dx2;
  139. // dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
  140. // dx1 = thetax - dx4;
  141. // dy1 = dx1 * tan(fabs(mhdg));
  142. // dy4 = my + dy1 +dy2 -dy3-dy5;
  143. // std::cout<<" compute. "<<std::endl;
  144. // }
  145. }
  146. void SideParkCalc::ParkAtRightCalc_Model2()
  147. {
  148. double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5;
  149. dy5 = 0;
  150. dx5 = mfLastDirectDis;
  151. double dx2fix = mfRaidus * sin(mhdg);
  152. double dy2fix = mfRaidus * (1.0 - cos(mhdg));
  153. double fMaxdy4 = mfRaidus *(1.0 - cos(mfMaxAngle));
  154. double fang ;
  155. if((my+dy2fix) >= (2*fMaxdy4) )
  156. {
  157. fang = mfMaxAngle;
  158. }
  159. else
  160. {
  161. fang = acos(1.0 - (my+dy2fix)/(2.0*mfRaidus)) ;
  162. }
  163. dx4 = mfRaidus * sin(fang);
  164. dy4 = mfRaidus * (1.0 - cos(fang));
  165. dx2 = dx4 - dx2fix;
  166. dy2 = dy4 - dy2fix;
  167. double thetax = mx - dx2 -dx4 -dx5;
  168. double k1 = tan(mhdg);
  169. double k3 = tan(fang);
  170. if(fabs(k1-k3)<0.001)
  171. {
  172. dy1 = 0;
  173. dx1 = 0;
  174. dy3 = my - dy2 - dy4;
  175. if(k3 > 0.001)
  176. {
  177. dx3 = dy3/k3;
  178. }
  179. else
  180. {
  181. dx3 = 0;
  182. }
  183. if(fabs(mx - dx1 - dx2 -dx3 -dx4 - dx5) > 0.1)
  184. {
  185. std::cout<<" no solve"<<std::endl;
  186. return;
  187. }
  188. }
  189. else
  190. {
  191. dx3 = (my - dy2 - dy4 - k1 * thetax)/(k3 - k1);
  192. dy3 = k3 * dx3;
  193. dx1 = thetax - dx3;
  194. dy1 = k1 * dx1;
  195. }
  196. if((dx3<-0.1) || (dx1<-0.1))
  197. {
  198. if(cos(mhdg)> 0.001)
  199. {
  200. double fR = my/(1 - cos(mhdg));
  201. double Rx = mx - fR*sin(mhdg);
  202. if((Rx>0.3) && (fR >= mfRaidus))
  203. {
  204. mSideParkType = SideParkType::TwoStep;
  205. mvectorWheel.push_back(mfMaxWheel * mfRaidus/fR);
  206. mvectorWheel.push_back(0.0);
  207. double flen1,flen2;
  208. flen2 = Rx;
  209. flen1 = fR*fabs(mhdg);
  210. mvectorlen.push_back(flen1);
  211. mvectorlen.push_back(flen2);
  212. mfTotalLen = flen1 + flen2;
  213. mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
  214. std::cout<<" Have One Step soluton."<<std::endl;
  215. }
  216. }
  217. }
  218. else
  219. {
  220. mSideParkType = SideParkType::FiveStep;
  221. mvectorWheel.push_back(0.0);
  222. mvectorWheel.push_back(mfMaxWheel * (-1.0));
  223. mvectorWheel.push_back(0.0);
  224. mvectorWheel.push_back(mfMaxWheel);
  225. mvectorWheel.push_back(0.0);
  226. double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
  227. x4 = dx5;y4 = dy5;hdg4 = 0;flen5 = x4;
  228. x3 = x4 + dx4;y3 = y4 + dy4;hdg3 = fang;flen4 = fabs(fang) *mfRaidus;
  229. hdg2 = hdg3;
  230. if(dx3>0.1)
  231. {
  232. x2 = x3 + dx3;y2 = y3 + dy3;
  233. flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
  234. }
  235. else
  236. {
  237. x2 = x3;y2 = y3;flen3 = 0;
  238. }
  239. flen2 = mfRaidus * (fang - mhdg);
  240. x1 = mx - dx1;
  241. y1 = my - dy1;
  242. hdg1 = mhdg;
  243. flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
  244. mvectorlen.push_back(flen1);
  245. mvectorlen.push_back(flen2);
  246. mvectorlen.push_back(flen3);
  247. mvectorlen.push_back(flen4);
  248. mvectorlen.push_back(flen5);
  249. mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
  250. mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
  251. mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
  252. mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
  253. mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
  254. std::cout<<" have compute."<<std::endl;
  255. }
  256. std::cout<<" compute. "<<std::endl;
  257. }
  258. void SideParkCalc::ParkAtLeftCalc()
  259. {
  260. if(mhdg>=0.0)
  261. {
  262. ParkAtLeftCalc_Model1();
  263. }
  264. else
  265. {
  266. ParkAtLeftCalc_Model2();
  267. }
  268. }
  269. void SideParkCalc::ParkAtLeftCalc_Model1()
  270. {
  271. double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5,dx6,dy6;
  272. dy6 = 0;
  273. dx6 = mfLastDirectDis;
  274. double fMaxdy5 = mfRaidus *(1.0 - cos(mfMaxAngle));
  275. dx2 = mfRaidus * sin(fabs(mhdg));
  276. dy2 = mfRaidus * (1 - cos(fabs(mhdg)));
  277. // double x = mx;
  278. double y = fabs(my);
  279. double fang ;//= mfMaxAngle;
  280. if((y+dy2) >= (2*fMaxdy5) )
  281. {
  282. fang = mfMaxAngle;
  283. }
  284. else
  285. {
  286. fang = acos(1.0 - (y+dy2)/(2.0*mfRaidus)) ;
  287. }
  288. dx5 = mfRaidus * sin(fang);
  289. dy5 = mfRaidus * (1.0 - cos(fang));
  290. dx3 = dx5;
  291. dy3 = dy5;
  292. double thetax = mx - dx6 - dx3 - dx5 - dx2;
  293. if(fabs(tan(fabs(mhdg )) + tan(fang ))<0.0001)
  294. {
  295. std::cout<<" divide error."<<std::endl;
  296. return;
  297. }
  298. dx4 = (y + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
  299. dx1 = thetax - dx4;
  300. dy1 = dx1 * tan(fabs(mhdg));
  301. dy4 = y + dy1 +dy2 -dy3-dy5;
  302. std::cout<<" compute. "<<std::endl;
  303. if(dx1>(-0.1)&&(dx4>(-0.1)))
  304. {
  305. mSideParkType = SideParkType::FiveStep;
  306. mvectorWheel.push_back(0.0);
  307. mvectorWheel.push_back(mfMaxWheel * (1.0));
  308. mvectorWheel.push_back(0.0);
  309. mvectorWheel.push_back(mfMaxWheel * (-1.0));
  310. mvectorWheel.push_back(0.0);
  311. double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
  312. x4 = dx6;y4 = dy6;hdg4 = 0;flen5 = x4;
  313. x3 = x4 + dx5;y3 = y4 - dy5;hdg3 = fang * (-1);flen4 = fabs(fang) *mfRaidus;
  314. hdg2 = hdg3;
  315. if(dx4>0.1)
  316. {
  317. x2 = x3 + dx4;y2 = y3 - dy4;
  318. flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
  319. }
  320. else
  321. {
  322. x2 = x3;y2 = y3;flen3 = 0;
  323. }
  324. flen2 = mfRaidus * (mhdg + fang);
  325. x1 = mx - dx1;
  326. y1 = my + dy1;
  327. hdg1 = mhdg;
  328. flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
  329. mvectorlen.push_back(flen1);
  330. mvectorlen.push_back(flen2);
  331. mvectorlen.push_back(flen3);
  332. mvectorlen.push_back(flen4);
  333. mvectorlen.push_back(flen5);
  334. mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
  335. mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
  336. mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
  337. mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
  338. mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
  339. }
  340. }
  341. void SideParkCalc::ParkAtLeftCalc_Model2()
  342. {
  343. double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5;
  344. dy5 = 0;
  345. dx5 = mfLastDirectDis;
  346. double dx2fix = mfRaidus * sin(fabs(mhdg));
  347. double dy2fix = mfRaidus * (1.0 - cos(fabs(mhdg)));
  348. double fMaxdy4 = mfRaidus *(1.0 - cos(mfMaxAngle));
  349. double fang ;
  350. double y = fabs(my);
  351. if((y+dy2fix) >= (2*fMaxdy4) )
  352. {
  353. fang = mfMaxAngle;
  354. }
  355. else
  356. {
  357. fang = acos(1.0 - (y+dy2fix)/(2.0*mfRaidus)) ;
  358. }
  359. dx4 = mfRaidus * sin(fang);
  360. dy4 = mfRaidus * (1.0 - cos(fang));
  361. dx2 = dx4 - dx2fix;
  362. dy2 = dy4 - dy2fix;
  363. double thetax = mx - dx2 -dx4 -dx5;
  364. double k1 = tan(mhdg);
  365. double k3 = tan(fang);
  366. if(fabs(k1-k3)<0.001)
  367. {
  368. dy1 = 0;
  369. dx1 = 0;
  370. dy3 = y - dy2 - dy4;
  371. if(k3 > 0.001)
  372. {
  373. dx3 = dy3/k3;
  374. }
  375. else
  376. {
  377. dx3 = 0;
  378. }
  379. if(fabs(mx - dx1 - dx2 - dx3 -dx4 - dx5) > 0.1)
  380. {
  381. std::cout<<" no solve"<<std::endl;
  382. return;
  383. }
  384. }
  385. else
  386. {
  387. dx3 = (y - dy2 - dy4 - k1 * thetax)/(k3 - k1);
  388. dy3 = k3 * dx3;
  389. dx1 = thetax - dx3;
  390. dy1 = k1 * dx1;
  391. }
  392. if((dx3<-0.1) || (dx1<-0.1))
  393. {
  394. if(cos(fabs(mhdg))> 0.001)
  395. {
  396. double fR = y/(1 - cos(fabs(mhdg)));
  397. double Rx = mx - fR*sin(fabs(mhdg));
  398. if((Rx>0.3) && (fR >= mfRaidus))
  399. {
  400. std::cout<<" Have One Step soluton."<<std::endl;
  401. mSideParkType = SideParkType::TwoStep;
  402. mvectorWheel.push_back(mfMaxWheel * mfRaidus/fR);
  403. mvectorWheel.push_back(0.0);
  404. double flen1,flen2;
  405. flen2 = Rx;
  406. flen1 = fR*fabs(mhdg);
  407. mvectorlen.push_back(flen1);
  408. mvectorlen.push_back(flen2);
  409. mfTotalLen = flen1 + flen2;
  410. mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
  411. std::cout<<" Have One Step soluton."<<std::endl;
  412. }
  413. }
  414. }
  415. else
  416. {
  417. mSideParkType = SideParkType::FiveStep;
  418. mvectorWheel.push_back(0.0);
  419. mvectorWheel.push_back(mfMaxWheel * (1.0));
  420. mvectorWheel.push_back(0.0);
  421. mvectorWheel.push_back(mfMaxWheel *(-1.0));
  422. mvectorWheel.push_back(0.0);
  423. double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
  424. x4 = dx5;y4 = dy5 * (-1.0);hdg4 = 0;flen5 = x4;
  425. x3 = x4 + dx4;y3 = y4 - dy4;hdg3 = fang * (-1.0);flen4 = fabs(fang) *mfRaidus;
  426. hdg2 = hdg3;
  427. if(dx3>0.1)
  428. {
  429. x2 = x3 + dx3;y2 = y3 - dy3;
  430. flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
  431. }
  432. else
  433. {
  434. x2 = x3;y2 = y3;flen3 = 0;
  435. }
  436. flen2 = mfRaidus * (fang + mhdg);
  437. x1 = mx - dx1;
  438. y1 = my + dy1;
  439. hdg1 = mhdg;
  440. flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
  441. mvectorlen.push_back(flen1);
  442. mvectorlen.push_back(flen2);
  443. mvectorlen.push_back(flen3);
  444. mvectorlen.push_back(flen4);
  445. mvectorlen.push_back(flen5);
  446. mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
  447. mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
  448. mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
  449. mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
  450. mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
  451. std::cout<<" have compute."<<std::endl;
  452. }
  453. std::cout<<" compute. "<<std::endl;
  454. }
  455. void SideParkCalc::normalhdg(double & fhdg)
  456. {
  457. while(fhdg > M_PI)fhdg = fhdg - 2.0*M_PI;
  458. while(fhdg <= (-M_PI))fhdg = fhdg + 2.0*M_PI;
  459. }
  460. SideParkType SideParkCalc::GetSolution(std::vector<iv::SideParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen)
  461. {
  462. if(mSideParkType == SideParkType::NoSolution)
  463. {
  464. return SideParkType::NoSolution;
  465. }
  466. xvectorpoint = mvectorpoint;
  467. xvectorWheel = mvectorWheel;
  468. xvectorlen = mvectorlen;
  469. fTotalLen = mfTotalLen;
  470. return mSideParkType;
  471. }