omni_planner.cpp 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517
  1. #include "omni_planner/omni_planner.h"
  2. namespace omni {
  3. static common::Path __Comp(common::Point& s, common::Point& e) // 补全路径
  4. {
  5. common::Path comp;
  6. float x = e.x - s.x;
  7. float y = e.y - s.y;
  8. float step_x = fabs(x) / __rlu;
  9. float step_y = fabs(y) / __rlu;
  10. float n = std::max(step_x, step_y);
  11. if (n < 1.1) {
  12. comp.clear();
  13. return comp;
  14. }
  15. float dx = x / n;
  16. float dy = y / n;
  17. common::Point p = s;
  18. int nn = std::floor(n);
  19. comp.push_back(p);
  20. for (int i = 0;i <nn - 1;i++) {
  21. p.x += dx;
  22. p.y += dy;
  23. comp.push_back(p);
  24. }
  25. return comp;
  26. }
  27. void omniPlanner::setPath(common::Path& path)
  28. {
  29. reset();
  30. common::Path compent;
  31. // 稠密化路径
  32. for (int i = 0;i < path.size() - 1;i++) {
  33. if (common::dist(path[i], path[i + 1]) > 2.0 * __rlu) {
  34. common::Path tmp = __Comp(path[i], path[i + 1]);
  35. compent.insert(compent.end(), tmp.begin(), tmp.end());
  36. }
  37. else {
  38. compent.push_back(path[i]);
  39. }
  40. }
  41. compent.push_back(path.back());
  42. compent.back().head = compent.front().head;
  43. common::Path dis;
  44. // 离散路径
  45. for (int i = 0;i < compent.size();i++) {
  46. if (!dis.empty() && i != compent.size() - 1) {
  47. if (common::dist(dis.back(), compent[i]) < __rlu ||
  48. common::dist(compent[i], compent.back()) < __rlu
  49. ) {
  50. continue;
  51. }
  52. }
  53. dis.push_back(compent[i]);
  54. }
  55. path_ = std::move(dis);
  56. LOG(INFO)<<"size: "<<path.size()<<" path_ size: "<<path_.size();
  57. }
  58. void omniPlanner::reset()
  59. {
  60. i_ = 0;
  61. path_.clear();
  62. step_ = common::NavStep::STANDBY; // 待命状态
  63. }
  64. void omniPlanner::destroy()
  65. {
  66. return;
  67. }
  68. common::NavState omniPlanner::computeVelocity(common::Point& sta_pose, common::Twist& cmd)
  69. {
  70. if (!__pathCheck()) return common::NavState::DONE;
  71. float maxmal_next;
  72. Index min_nex = (Index)(config_.minmal_next / __rlu);
  73. Index back_i = path_.size();
  74. Index i = i_;
  75. maxmal_next = std::max(config_.maxmal_next, config_.max_linear);
  76. maxmal_next = std::max(maxmal_next, __rlu);
  77. common::Path path = __exten(path_, (Index)(maxmal_next / __rlu) + 1);
  78. conti: NULL;
  79. {
  80. i_ = __nib(sta_pose, i, path_, config_.max_linear); // 找路径上的最近点
  81. sta_pose.head = path_[i_].head; // * (M_PI / 180.0);
  82. Index max_nex = (Index)(maxmal_next / __rlu);
  83. Index set = i_ + max_nex; // 引导点
  84. bool closr =
  85. (common::dist(path_[i_], path_.back()) < 0.21) ||
  86. ((path_.size() - i_) < 10); // 接近终点
  87. if (closr) {
  88. if (__isArrv(sta_pose)) {
  89. return common::NavState::DONE;
  90. }
  91. }
  92. // 旋转调整处理
  93. {
  94. common::Point base = path[i_]; // i_一定小于path size
  95. common::Point tip = path[i_ + 1];
  96. base.theta = sta_pose.theta + sta_pose.head;
  97. base.theta = common::norm(base.theta);
  98. switch(step_) {
  99. case common::NavStep::STANDBY:
  100. step_ = common::NavStep::TANGENT;
  101. return common::NavState::ROTATE;
  102. break;
  103. case common::NavStep::TANGENT: // 切线调整
  104. /*需要改进*/
  105. if (__rotaTangent(base, tip, cmd) == common::NavState::DONE) {
  106. step_ = common::NavStep::TRACK;
  107. }
  108. return common::NavState::ROTATE;
  109. break;
  110. // case common::NavStep::ANGLE:
  111. // common::Point base = sta_pose;
  112. // common::Point tip = path_.back();
  113. // base.theta = sta_pose.theta + sta_pose.head;
  114. // base.theta = common::norm(base.theta);
  115. // if (__rotaAngle(base.theta, tip.theta, cmd) == common::NavState::DONE) {
  116. // step_ = common::NavStep::COMPLETE;
  117. // }
  118. // return common::NavState::ROTATE;
  119. // break;
  120. // case common::NavStep::COMPLETE:
  121. // reset();
  122. // return common::NavState::DONE;
  123. // break;
  124. case common::NavStep::TRACK:
  125. NULL;
  126. break;
  127. default:
  128. reset();
  129. return common::NavState::DONE;
  130. break;
  131. }
  132. }
  133. float goto_angle = 1e10;
  134. closr = (common::lineL(path_, (int)i_, (int)path_.size()) < config_.close_ad);
  135. // for (int i = (i_ + 1);i < set;i++) {
  136. for (int i = set;i > (i_ + min_nex - 1);i--) {
  137. float angle = __pathTangent(path, i) - __c(sta_pose, path[i]);
  138. angle = fabs(common::norm(angle));
  139. if (angle < goto_angle) {
  140. goto_angle = angle;
  141. set = i;
  142. }
  143. }
  144. if (set > path_.size() - 1 && (i_ + min_nex - 1) < path_.size()) { // 终点牵引
  145. set = path_.size() - 1;
  146. }
  147. float liner = common::dist(sta_pose, path_.back()) * config_.linear_slow_p;
  148. float comp = 0; // 角速度补偿量
  149. {
  150. common::Point goal = path[set];
  151. goal.theta = common::norm(__pathTangent(path, set) + M_PI);
  152. goal.head = 0;
  153. float boto = common::norm(__c(goal, sta_pose) + M_PI);
  154. float keep = common::norm(boto - sta_pose.head - sta_pose.theta);
  155. comp = fabs(keep) < config_.max_angular ? keep :
  156. (keep < 0 ? -config_.max_angular : config_.max_angular);
  157. if (closr) { // 接近终点
  158. if (fabs(keep) > config_.toler_theta) {
  159. comp = fabs(keep) < config_.max_angular ? keep :
  160. (keep < 0 ? -config_.max_angular : config_.max_angular);
  161. }
  162. float ld = common::PLD(path[i_], path[i_ + 1], sta_pose);
  163. if (ld > hypot(config_.toler_x, config_.toler_y)) {
  164. liner *= exp(-10.0 * ld);
  165. }
  166. liner = liner < 0.05 ? 0.05 : liner;
  167. }
  168. else {
  169. if (fabs(keep) > config_.toler_follow_theta ||
  170. fabs(keep) > 0.85) { // 跟随容忍角度误差
  171. cmd.x = 0;
  172. cmd.angular = fabs(keep) < config_.max_angular ? keep :
  173. (keep < 0 ? -config_.max_angular : config_.max_angular);
  174. return common::NavState::ROTATE;
  175. }
  176. }
  177. }
  178. liner = std::min(liner, maxmal_next);
  179. liner = std::min(liner, common::dist(path[i_], path[set]));
  180. // liner = std::min(liner, common::dist(path_[i_], path_.back()));
  181. liner = std::min(liner, common::dist(sta_pose, path[set]));
  182. liner = std::min(liner, config_.max_linear);
  183. liner = std::min(liner, path_[i_].max_linear);
  184. Index locli = set < ((int)path_.size() - 1) ? set : ((int)path_.size() - 1);
  185. local_path_ = __getPah(sta_pose, path[locli], __rlu); // 计算局部路径
  186. path[set].theta = common::norm(__pathTangent(path, set));
  187. common::NavState sd = __cal(sta_pose, path[set], cmd, liner);
  188. comp = fabs(comp) < 0.1 ? comp : (comp > 0 ? 0.1 : -0.1); // 最大补偿10°/s
  189. cmd.angular += comp * 0.1;
  190. // cmd.x *= exp(-10.0 * fabs(comp));
  191. return sd;
  192. }
  193. return common::NavState::ING;
  194. }
  195. common::NavState omniPlanner::computeVelocityCommands(common::Point& sta_pose, common::Twist& cmd)
  196. {
  197. return computeVelocity(sta_pose, cmd);
  198. }
  199. /**
  200. * 在path_上截取a长度,起始点i_
  201. */
  202. common::Path omniPlanner::interPath(float a)
  203. {
  204. common::Path path;
  205. for (int i = i_;i < (int)path_.size() - 1;i++) {
  206. a -= common::dist(path_[i + 1], path_[i]);
  207. if (a < 0) {
  208. break;
  209. }
  210. path.push_back(path_[i]);
  211. }
  212. return path;
  213. }
  214. common::Config omniPlanner::getConfig()
  215. {
  216. return config_;
  217. }
  218. void omniPlanner::setConfig(common::Config& conf)
  219. {
  220. config_ = conf;
  221. if (config_.angular_slow_p > 1.0) config_.angular_slow_p = 1.0;
  222. if (config_.linear_slow_p > 1.0) config_.linear_slow_p = 1.0;
  223. }
  224. common::Path omniPlanner::getLocalPath()
  225. {
  226. return local_path_;
  227. }
  228. common::Path omniPlanner::getGlobalPath()
  229. {
  230. return path_;
  231. }
  232. Index omniPlanner::__nib(common::Point& p, Index ci, common::Path& path, float range)
  233. {
  234. Index ni = ci;
  235. float ns = 1e10;
  236. for (Index i = ci;i < path.size();i++) {
  237. float ds = common::dist(p, path[i]);
  238. if (ds < ns) {
  239. ns = ds;
  240. ni = i;
  241. }
  242. if (i != 0) {
  243. range -= common::dist(path[i], path[i - 1]);
  244. }
  245. if (range < 0.0) {
  246. break;
  247. }
  248. }
  249. return ni;
  250. }
  251. common::Path omniPlanner::__exten(common::Path& path, Index i)
  252. {
  253. common::Path tp = path;
  254. if (path.size() < 2) return tp;
  255. Index i2 = path.size() - 2;
  256. float x = path.back().x - path[i2].x;
  257. float y = path.back().y - path[i2].y;
  258. float step_x = fabs(x) / __rlu;
  259. float step_y = fabs(y) / __rlu;
  260. float s = std::max(step_x, step_y);
  261. if (s < 0.01) {
  262. return tp;
  263. }
  264. float dx = x / s;
  265. float dy = y / s;
  266. common::Point p = tp.back();
  267. for (int m = i;m > -1;m--) {
  268. p.x += dx;
  269. p.y += dy;
  270. tp.push_back(p);
  271. }
  272. return tp;
  273. }
  274. bool omniPlanner::__isArrv(common::Point p)
  275. {
  276. if (path_.size() < 2) return true;
  277. Index i2 = path_.size() - 2;
  278. float x = path_.back().x - path_[i2].x;
  279. float y = path_.back().y - path_[i2].y;
  280. float a = atan2(y, x); // 点的切线方向为到达方向
  281. float cr_x = p.x - path_.back().x;
  282. float cr_y = p.y - path_.back().y;
  283. float cr_ex = cr_x * cos(-a) - cr_y * sin(-a);
  284. float cr_ey = cr_x * sin(-a) + cr_y * cos(-a);
  285. if (cr_ex > 0 ||
  286. (fabs(cr_ex) < config_.toler_x && fabs(cr_ey) < config_.toler_y)) {
  287. return true;
  288. }
  289. return false;
  290. }
  291. common::Path omniPlanner::__getPah(const common::Point& start, const common::Point& end, const float rlu)
  292. {
  293. common::Path path;
  294. if (rlu < 1e-3) {
  295. return path;
  296. }
  297. float angle = common::norm(start.theta + start.head); // config_.for_angle);
  298. const float x = (end.x - start.x) * cos(-angle) - (end.y - start.y) * sin(-angle);
  299. const float y = (end.x - start.x) * sin(-angle) + (end.y - start.y) * cos(-angle);
  300. const float kangle = atan2(y, x);
  301. const float l = hypot(x, y);
  302. const float r = 0.5 * l / sin(kangle);
  303. const float step_rad = rlu / r;
  304. const float step_cl = fabs(r * sin(step_rad * 0.5) * 2.f);
  305. float dx = 0.f, dy = 0.f, dangle = 0.f;
  306. int c = 0;
  307. common::Point path_element = start;
  308. while (1) {
  309. dangle += step_rad;
  310. dx += step_cl * cos(dangle);
  311. dy += step_cl * sin(dangle);
  312. path_element.x = start.x +
  313. dx * cos(-angle) + dy * sin(-angle); // trans footprint to map
  314. path_element.y = start.y +
  315. dy * cos(-angle) - dx * sin(-angle);
  316. path_element.theta = common::norm(angle + dangle);
  317. path.push_back(path_element);
  318. if (hypot(path_element.x - end.x, path_element.y - end.y) < rlu) {
  319. path.push_back(end);
  320. break;
  321. }
  322. c++;
  323. if (c > 1000) break;
  324. }
  325. return path;
  326. }
  327. common::NavState omniPlanner::__cal(common::Point& start, common::Point& goal, common::Twist& twist, float linear)
  328. {
  329. float a = start.theta; // + start.head;
  330. float x = (goal.x - start.x) * cos(-a) - (goal.y - start.y) * sin(-a);
  331. float y = (goal.x - start.x) * sin(-a) + (goal.y - start.y) * cos(-a);
  332. float theta = common::norm(goal.theta - start.theta - start.head);
  333. float lt = fabs(x) / std::min(config_.max_linear, linear);
  334. float at = fabs(theta) / config_.max_angular;
  335. float tt = fabs(y) / std::min(config_.max_transla, linear);
  336. float t = std::max(std::max(at, lt), tt);
  337. if (t < 1e-5) {
  338. twist = common::Twist();
  339. return common::NavState::TOO_CLOSE;
  340. }
  341. else {
  342. twist.x = x / t;
  343. twist.y = y / t;
  344. twist.angular = theta / t;
  345. }
  346. return common::NavState::ING;
  347. }
  348. /**
  349. * 从start到goal的圆弧角
  350. */
  351. float omniPlanner::__c(common::Point& start, common::Point& goal)
  352. {
  353. float a = start.theta + start.head;
  354. float x = (goal.x - start.x) * cos(-a) - (goal.y - start.y) * sin(-a);
  355. float y = (goal.x - start.x) * sin(-a) + (goal.y - start.y) * cos(-a);
  356. float l = x * x + y * y;
  357. // float theta = 2.0 * y;
  358. float theta = 2.0 * asin(y / sqrt(l));
  359. if (x < 0) {
  360. theta = (theta > 0) ? (theta - M_PI) : (theta + M_PI);
  361. }
  362. return common::norm(a + theta);
  363. }
  364. common::NavState omniPlanner::__rotaTangent(common::Point& base, common::Point& tip, common::Twist& twist)
  365. {
  366. float x = tip.x - base.x;
  367. float y = tip.y - base.y;
  368. float e = common::norm(atan2(y, x) - base.theta);
  369. float rotate = e * config_.linear_slow_p;
  370. if (fabs(e) < config_.toler_start_theta) {
  371. return common::NavState::DONE;
  372. }
  373. twist.x = 0;
  374. twist.y = 0;
  375. twist.angular = fabs(rotate) < config_.max_angular ? rotate :
  376. (rotate < 0 ? -config_.max_angular : config_.max_angular);
  377. return common::NavState::ROTATE;
  378. }
  379. common::NavState omniPlanner::__rotaAngle(float base, float tip, common::Twist& twist)
  380. {
  381. float e = tip - base;
  382. float rotate = e * config_.linear_slow_p;
  383. if (fabs(e) < config_.toler_theta) {
  384. return common::NavState::DONE;
  385. }
  386. twist.x = 0;
  387. twist.angular = fabs(rotate) < config_.max_angular ? rotate :
  388. (rotate < 0 ? -config_.max_angular : config_.max_angular);
  389. return common::NavState::ROTATE;
  390. }
  391. float omniPlanner::__maxmalDist(common::Path& pt1, common::Path& pt2)
  392. {
  393. float di = 0;
  394. for (common::Point& p1 : pt1) {
  395. float minmal = 1e10;
  396. for (common::Point& p2 : pt2) {
  397. minmal = std::min(minmal, common::dist(p2, p1));
  398. }
  399. di = std::max(minmal, di);
  400. }
  401. return di;
  402. }
  403. float omniPlanner::__pathTangent(common::Path& path, Index i)
  404. {
  405. float x = path[i + 1].x - path[i].x;
  406. float y = path[i + 1].y - path[i].y;
  407. return atan2(y, x);
  408. }
  409. bool omniPlanner::__pathCheck()
  410. {
  411. if (path_.size() < 2 ||
  412. i_ >= path_.size() ||
  413. false
  414. ) {
  415. return false;
  416. }
  417. return true;
  418. }
  419. }