123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517 |
- #include "omni_planner/omni_planner.h"
- namespace omni {
- static common::Path __Comp(common::Point& s, common::Point& e) // 补全路径
- {
- common::Path comp;
- float x = e.x - s.x;
- float y = e.y - s.y;
-
- float step_x = fabs(x) / __rlu;
- float step_y = fabs(y) / __rlu;
- float n = std::max(step_x, step_y);
- if (n < 1.1) {
- comp.clear();
- return comp;
- }
- float dx = x / n;
- float dy = y / n;
- common::Point p = s;
- int nn = std::floor(n);
- comp.push_back(p);
- for (int i = 0;i <nn - 1;i++) {
- p.x += dx;
- p.y += dy;
- comp.push_back(p);
- }
- return comp;
- }
- void omniPlanner::setPath(common::Path& path)
- {
- reset();
- common::Path compent;
- // 稠密化路径
- for (int i = 0;i < path.size() - 1;i++) {
- if (common::dist(path[i], path[i + 1]) > 2.0 * __rlu) {
- common::Path tmp = __Comp(path[i], path[i + 1]);
- compent.insert(compent.end(), tmp.begin(), tmp.end());
- }
- else {
- compent.push_back(path[i]);
- }
- }
- compent.push_back(path.back());
- compent.back().head = compent.front().head;
- common::Path dis;
- // 离散路径
- for (int i = 0;i < compent.size();i++) {
- if (!dis.empty() && i != compent.size() - 1) {
- if (common::dist(dis.back(), compent[i]) < __rlu ||
- common::dist(compent[i], compent.back()) < __rlu
- ) {
- continue;
- }
- }
- dis.push_back(compent[i]);
- }
- path_ = std::move(dis);
- LOG(INFO)<<"size: "<<path.size()<<" path_ size: "<<path_.size();
- }
- void omniPlanner::reset()
- {
- i_ = 0;
- path_.clear();
- step_ = common::NavStep::STANDBY; // 待命状态
- }
- void omniPlanner::destroy()
- {
- return;
- }
- common::NavState omniPlanner::computeVelocity(common::Point& sta_pose, common::Twist& cmd)
- {
- if (!__pathCheck()) return common::NavState::DONE;
- float maxmal_next;
-
- Index min_nex = (Index)(config_.minmal_next / __rlu);
- Index back_i = path_.size();
- Index i = i_;
- maxmal_next = std::max(config_.maxmal_next, config_.max_linear);
- maxmal_next = std::max(maxmal_next, __rlu);
-
- common::Path path = __exten(path_, (Index)(maxmal_next / __rlu) + 1);
- conti: NULL;
- {
- i_ = __nib(sta_pose, i, path_, config_.max_linear); // 找路径上的最近点
- sta_pose.head = path_[i_].head; // * (M_PI / 180.0);
- Index max_nex = (Index)(maxmal_next / __rlu);
- Index set = i_ + max_nex; // 引导点
- bool closr =
- (common::dist(path_[i_], path_.back()) < 0.21) ||
- ((path_.size() - i_) < 10); // 接近终点
- if (closr) {
- if (__isArrv(sta_pose)) {
- return common::NavState::DONE;
- }
- }
- // 旋转调整处理
- {
- common::Point base = path[i_]; // i_一定小于path size
- common::Point tip = path[i_ + 1];
- base.theta = sta_pose.theta + sta_pose.head;
- base.theta = common::norm(base.theta);
- switch(step_) {
- case common::NavStep::STANDBY:
- step_ = common::NavStep::TANGENT;
- return common::NavState::ROTATE;
- break;
- case common::NavStep::TANGENT: // 切线调整
- /*需要改进*/
- if (__rotaTangent(base, tip, cmd) == common::NavState::DONE) {
- step_ = common::NavStep::TRACK;
- }
- return common::NavState::ROTATE;
- break;
- // case common::NavStep::ANGLE:
- // common::Point base = sta_pose;
- // common::Point tip = path_.back();
-
- // base.theta = sta_pose.theta + sta_pose.head;
- // base.theta = common::norm(base.theta);
- // if (__rotaAngle(base.theta, tip.theta, cmd) == common::NavState::DONE) {
- // step_ = common::NavStep::COMPLETE;
- // }
- // return common::NavState::ROTATE;
- // break;
- // case common::NavStep::COMPLETE:
- // reset();
- // return common::NavState::DONE;
- // break;
- case common::NavStep::TRACK:
- NULL;
- break;
- default:
- reset();
- return common::NavState::DONE;
- break;
- }
- }
- float goto_angle = 1e10;
- closr = (common::lineL(path_, (int)i_, (int)path_.size()) < config_.close_ad);
- // for (int i = (i_ + 1);i < set;i++) {
- for (int i = set;i > (i_ + min_nex - 1);i--) {
- float angle = __pathTangent(path, i) - __c(sta_pose, path[i]);
- angle = fabs(common::norm(angle));
- if (angle < goto_angle) {
- goto_angle = angle;
- set = i;
- }
- }
- if (set > path_.size() - 1 && (i_ + min_nex - 1) < path_.size()) { // 终点牵引
- set = path_.size() - 1;
- }
- float liner = common::dist(sta_pose, path_.back()) * config_.linear_slow_p;
- float comp = 0; // 角速度补偿量
- {
- common::Point goal = path[set];
- goal.theta = common::norm(__pathTangent(path, set) + M_PI);
- goal.head = 0;
- float boto = common::norm(__c(goal, sta_pose) + M_PI);
- float keep = common::norm(boto - sta_pose.head - sta_pose.theta);
- comp = fabs(keep) < config_.max_angular ? keep :
- (keep < 0 ? -config_.max_angular : config_.max_angular);
- if (closr) { // 接近终点
- if (fabs(keep) > config_.toler_theta) {
- comp = fabs(keep) < config_.max_angular ? keep :
- (keep < 0 ? -config_.max_angular : config_.max_angular);
- }
- float ld = common::PLD(path[i_], path[i_ + 1], sta_pose);
- if (ld > hypot(config_.toler_x, config_.toler_y)) {
- liner *= exp(-10.0 * ld);
- }
- liner = liner < 0.05 ? 0.05 : liner;
- }
- else {
- if (fabs(keep) > config_.toler_follow_theta ||
- fabs(keep) > 0.85) { // 跟随容忍角度误差
- cmd.x = 0;
- cmd.angular = fabs(keep) < config_.max_angular ? keep :
- (keep < 0 ? -config_.max_angular : config_.max_angular);
- return common::NavState::ROTATE;
- }
- }
- }
- liner = std::min(liner, maxmal_next);
- liner = std::min(liner, common::dist(path[i_], path[set]));
- // liner = std::min(liner, common::dist(path_[i_], path_.back()));
- liner = std::min(liner, common::dist(sta_pose, path[set]));
- liner = std::min(liner, config_.max_linear);
- liner = std::min(liner, path_[i_].max_linear);
- Index locli = set < ((int)path_.size() - 1) ? set : ((int)path_.size() - 1);
- local_path_ = __getPah(sta_pose, path[locli], __rlu); // 计算局部路径
- path[set].theta = common::norm(__pathTangent(path, set));
- common::NavState sd = __cal(sta_pose, path[set], cmd, liner);
- comp = fabs(comp) < 0.1 ? comp : (comp > 0 ? 0.1 : -0.1); // 最大补偿10°/s
- cmd.angular += comp * 0.1;
- // cmd.x *= exp(-10.0 * fabs(comp));
- return sd;
- }
- return common::NavState::ING;
- }
- common::NavState omniPlanner::computeVelocityCommands(common::Point& sta_pose, common::Twist& cmd)
- {
- return computeVelocity(sta_pose, cmd);
- }
- /**
- * 在path_上截取a长度,起始点i_
- */
- common::Path omniPlanner::interPath(float a)
- {
- common::Path path;
- for (int i = i_;i < (int)path_.size() - 1;i++) {
- a -= common::dist(path_[i + 1], path_[i]);
- if (a < 0) {
- break;
- }
- path.push_back(path_[i]);
- }
- return path;
- }
- common::Config omniPlanner::getConfig()
- {
- return config_;
- }
- void omniPlanner::setConfig(common::Config& conf)
- {
- config_ = conf;
- if (config_.angular_slow_p > 1.0) config_.angular_slow_p = 1.0;
- if (config_.linear_slow_p > 1.0) config_.linear_slow_p = 1.0;
- }
- common::Path omniPlanner::getLocalPath()
- {
- return local_path_;
- }
- common::Path omniPlanner::getGlobalPath()
- {
- return path_;
- }
- Index omniPlanner::__nib(common::Point& p, Index ci, common::Path& path, float range)
- {
- Index ni = ci;
- float ns = 1e10;
- for (Index i = ci;i < path.size();i++) {
- float ds = common::dist(p, path[i]);
- if (ds < ns) {
- ns = ds;
- ni = i;
- }
- if (i != 0) {
- range -= common::dist(path[i], path[i - 1]);
- }
- if (range < 0.0) {
- break;
- }
- }
- return ni;
- }
- common::Path omniPlanner::__exten(common::Path& path, Index i)
- {
- common::Path tp = path;
- if (path.size() < 2) return tp;
- Index i2 = path.size() - 2;
- float x = path.back().x - path[i2].x;
- float y = path.back().y - path[i2].y;
-
- float step_x = fabs(x) / __rlu;
- float step_y = fabs(y) / __rlu;
- float s = std::max(step_x, step_y);
- if (s < 0.01) {
- return tp;
- }
- float dx = x / s;
- float dy = y / s;
-
- common::Point p = tp.back();
- for (int m = i;m > -1;m--) {
- p.x += dx;
- p.y += dy;
- tp.push_back(p);
- }
- return tp;
- }
- bool omniPlanner::__isArrv(common::Point p)
- {
- if (path_.size() < 2) return true;
- Index i2 = path_.size() - 2;
- float x = path_.back().x - path_[i2].x;
- float y = path_.back().y - path_[i2].y;
- float a = atan2(y, x); // 点的切线方向为到达方向
- float cr_x = p.x - path_.back().x;
- float cr_y = p.y - path_.back().y;
- float cr_ex = cr_x * cos(-a) - cr_y * sin(-a);
- float cr_ey = cr_x * sin(-a) + cr_y * cos(-a);
- if (cr_ex > 0 ||
- (fabs(cr_ex) < config_.toler_x && fabs(cr_ey) < config_.toler_y)) {
- return true;
- }
- return false;
- }
- common::Path omniPlanner::__getPah(const common::Point& start, const common::Point& end, const float rlu)
- {
- common::Path path;
- if (rlu < 1e-3) {
- return path;
- }
- float angle = common::norm(start.theta + start.head); // config_.for_angle);
- const float x = (end.x - start.x) * cos(-angle) - (end.y - start.y) * sin(-angle);
- const float y = (end.x - start.x) * sin(-angle) + (end.y - start.y) * cos(-angle);
- const float kangle = atan2(y, x);
- const float l = hypot(x, y);
- const float r = 0.5 * l / sin(kangle);
- const float step_rad = rlu / r;
- const float step_cl = fabs(r * sin(step_rad * 0.5) * 2.f);
- float dx = 0.f, dy = 0.f, dangle = 0.f;
- int c = 0;
-
- common::Point path_element = start;
-
- while (1) {
- dangle += step_rad;
-
- dx += step_cl * cos(dangle);
- dy += step_cl * sin(dangle);
- path_element.x = start.x +
- dx * cos(-angle) + dy * sin(-angle); // trans footprint to map
- path_element.y = start.y +
- dy * cos(-angle) - dx * sin(-angle);
- path_element.theta = common::norm(angle + dangle);
- path.push_back(path_element);
-
- if (hypot(path_element.x - end.x, path_element.y - end.y) < rlu) {
- path.push_back(end);
- break;
- }
- c++;
- if (c > 1000) break;
- }
- return path;
- }
- common::NavState omniPlanner::__cal(common::Point& start, common::Point& goal, common::Twist& twist, float linear)
- {
- float a = start.theta; // + start.head;
- float x = (goal.x - start.x) * cos(-a) - (goal.y - start.y) * sin(-a);
- float y = (goal.x - start.x) * sin(-a) + (goal.y - start.y) * cos(-a);
- float theta = common::norm(goal.theta - start.theta - start.head);
- float lt = fabs(x) / std::min(config_.max_linear, linear);
- float at = fabs(theta) / config_.max_angular;
- float tt = fabs(y) / std::min(config_.max_transla, linear);
- float t = std::max(std::max(at, lt), tt);
- if (t < 1e-5) {
- twist = common::Twist();
- return common::NavState::TOO_CLOSE;
- }
- else {
- twist.x = x / t;
- twist.y = y / t;
- twist.angular = theta / t;
- }
- return common::NavState::ING;
- }
- /**
- * 从start到goal的圆弧角
- */
- float omniPlanner::__c(common::Point& start, common::Point& goal)
- {
- float a = start.theta + start.head;
- float x = (goal.x - start.x) * cos(-a) - (goal.y - start.y) * sin(-a);
- float y = (goal.x - start.x) * sin(-a) + (goal.y - start.y) * cos(-a);
- float l = x * x + y * y;
- // float theta = 2.0 * y;
- float theta = 2.0 * asin(y / sqrt(l));
- if (x < 0) {
- theta = (theta > 0) ? (theta - M_PI) : (theta + M_PI);
- }
- return common::norm(a + theta);
- }
- common::NavState omniPlanner::__rotaTangent(common::Point& base, common::Point& tip, common::Twist& twist)
- {
- float x = tip.x - base.x;
- float y = tip.y - base.y;
- float e = common::norm(atan2(y, x) - base.theta);
- float rotate = e * config_.linear_slow_p;
- if (fabs(e) < config_.toler_start_theta) {
- return common::NavState::DONE;
- }
- twist.x = 0;
- twist.y = 0;
- twist.angular = fabs(rotate) < config_.max_angular ? rotate :
- (rotate < 0 ? -config_.max_angular : config_.max_angular);
- return common::NavState::ROTATE;
- }
- common::NavState omniPlanner::__rotaAngle(float base, float tip, common::Twist& twist)
- {
- float e = tip - base;
- float rotate = e * config_.linear_slow_p;
- if (fabs(e) < config_.toler_theta) {
- return common::NavState::DONE;
- }
- twist.x = 0;
- twist.angular = fabs(rotate) < config_.max_angular ? rotate :
- (rotate < 0 ? -config_.max_angular : config_.max_angular);
- return common::NavState::ROTATE;
- }
- float omniPlanner::__maxmalDist(common::Path& pt1, common::Path& pt2)
- {
- float di = 0;
- for (common::Point& p1 : pt1) {
- float minmal = 1e10;
- for (common::Point& p2 : pt2) {
- minmal = std::min(minmal, common::dist(p2, p1));
- }
- di = std::max(minmal, di);
- }
- return di;
- }
- float omniPlanner::__pathTangent(common::Path& path, Index i)
- {
- float x = path[i + 1].x - path[i].x;
- float y = path[i + 1].y - path[i].y;
- return atan2(y, x);
- }
- bool omniPlanner::__pathCheck()
- {
- if (path_.size() < 2 ||
- i_ >= path_.size() ||
- false
- ) {
- return false;
- }
- return true;
- }
- }
|