move_base的全局路径规划代码研究
2017-01-21 14:32
585 查看
algorithmn
parameter
code
主要是以下三个函数
计算所有的可行点
怎么计算一个点的可行点
从可行点中计算路径path
todo
Dijkstra
其实就是A star或者Dijkstra(基于priority queue实现的)的路径规划算法,关键是相邻点之间的cost怎么计算,怎么从可行点找到path
Navfn's optimal path is based on a path's "potential"(可能可以行走的目标). The potential is the relative cost of a
path based on the distance from the goal and from the existing path itself.(怎么计算两个点之间的距离cost) It must be noted that Navfn update's each cell's potential in the potential map, or potarr(定义的potential array变量) as it's called in navfn, as it checks that cell. This way,it can step back through the potential array to find the best possible path. The potential is determined by the cost of traversing a cell (traversability factor, hf)
and the distance away that the next cell is from the previous cell.
global planner
上面两个链接一个是navfn的配置,一个是具体哪个global planner的配置,具体的global planner 是可以覆盖navfn中的配置(要是大家有相同的参数)
比如下面这个参数global planner中的可以覆盖navfn中的配置:
这个参数可以让你看见potential array的图像,看计算出的cost是怎么样子(颜色深浅代表距离起始点的远近)
parameter
code
主要是以下三个函数
计算所有的可行点
怎么计算一个点的可行点
从可行点中计算路径path
todo
algorithmn
算法的解释Dijkstra
其实就是A star或者Dijkstra(基于priority queue实现的)的路径规划算法,关键是相邻点之间的cost怎么计算,怎么从可行点找到path
Navfn's optimal path is based on a path's "potential"(可能可以行走的目标). The potential is the relative cost of a
path based on the distance from the goal and from the existing path itself.(怎么计算两个点之间的距离cost) It must be noted that Navfn update's each cell's potential in the potential map, or potarr(定义的potential array变量) as it's called in navfn, as it checks that cell. This way,it can step back through the potential array to find the best possible path. The potential is determined by the cost of traversing a cell (traversability factor, hf)
and the distance away that the next cell is from the previous cell.
parameter
navfn 参数global planner
上面两个链接一个是navfn的配置,一个是具体哪个global planner的配置,具体的global planner 是可以覆盖navfn中的配置(要是大家有相同的参数)
比如下面这个参数global planner中的可以覆盖navfn中的配置:
~<name>/allow_unknown (bool, default: true)
这个参数可以让你看见potential array的图像,看计算出的cost是怎么样子(颜色深浅代表距离起始点的远近)
~<name>/visualize_potential (bool, default: false)
code
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) { if(!old_navfn_behavior_) convert_offset_ = 0.5; else convert_offset_ = 0.0; if (use_quadratic) p_calc_ = new QuadraticCalculator(cx, cy); else p_calc_ = new PotentialCalculator(cx, cy); if (use_dijkstra) { DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy); if(!old_navfn_behavior_) de->setPreciseStart(true); planner_ = de; } else planner_ = new AStarExpansion(p_calc_, cx, cy); if (use_grid_path) path_maker_ = new GridPath(p_calc_); else path_maker_ = new GradientPath(p_calc_); //发布一个make_plan的service make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this); } bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) { makePlan(req.start, req.goal, resp.plan.poses); } bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) { return makePlan(start, goal, default_tolerance_, plan); } bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) { double wx = start.pose.position.x; double wy = start.pose.position.y; if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) { ROS_WARN("The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?"); return false; } if(old_navfn_behavior_){ start_x = start_x_i; start_y = start_y_i; }else{ worldToMap(wx, wy, start_x, start_y); } wx = goal.pose.position.x; wy = goal.pose.position.y; if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) { ROS_WARN_THROTTLE(1.0,"The goal sent to the global planner is off the global costmap. Planning will always fail to this goal."); return false; } if(old_navfn_behavior_){ goal_x = goal_x_i; goal_y = goal_y_i; }else{ worldToMap(wx, wy, goal_x, goal_y); } //clear the starting cell within the costmap because we know it can't be an obstacle //设置起点为FREE_SPACE clearRobotCell(start_pose, start_x_i, start_y_i); int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY(); //make sure to resize the underlying array that Navfn uses p_calc_->setSize(nx, ny); planner_->setSize(nx, ny); path_maker_->setSize(nx, ny); potential_array_ = new float[nx * ny]; //将costmap的四周(边界)变为LETHAL_OBSTACLE outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE); // 寻找potential array bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y, nx * ny * 2, potential_array_); //对终点的处理 if(!old_navfn_behavior_) planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2); if(publish_potential_) publishPotential(potential_array_); if (found_legal) { //extract the plan,提取路径 if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) { //make sure the goal we push on has the same timestamp as the rest of the plan geometry_msgs::PoseStamped goal_copy = goal; goal_copy.header.stamp = ros::Time::now(); plan.push_back(goal_copy); } else { ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen."); } }else{ ROS_ERROR("Failed to get a plan."); } // add orientations if needed,对方向的处理 orientation_filter_->processPath(start, plan); //publish the plan for visualization purposes publishPlan(plan); delete potential_array_; return !plan.empty(); nx * ny * 2, potential_array_); }
主要是以下三个函数
可能不同的配置有不同的实现,但是每个函数的实现功能是一样的。
计算所有的可行点
namespace global_planner { bool DijkstraExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles, float* potential) { cells_visited_ = 0; // priority buffers threshold_ = lethal_cost_; currentBuffer_ = buffer1_; currentEnd_ = 0; nextBuffer_ = buffer2_; nextEnd_ = 0; overBuffer_ = buffer3_; overEnd_ = 0; memset(pending_, 0, ns_ * sizeof(bool)); std::fill(potential, potential + ns_, POT_HIGH); // set goal int k = toIndex(start_x, start_y); if(precise_) { double dx = start_x - (int)start_x, dy = start_y - (int)start_y; dx = floorf(dx * 100 + 0.5) / 100; dy = floorf(dy * 100 + 0.5) / 100; potential[k] = neutral_cost_ * 2 * dx * dy; potential[k+1] = neutral_cost_ * 2 * (1-dx)*dy; potential[k+nx_] = neutral_cost_*2*dx*(1-dy); potential[k+nx_+1] = neutral_cost_*2*(1-dx)*(1-dy);//*/ push_cur(k+2); push_cur(k-1); push_cur(k+nx_-1); push_cur(k+nx_+2); push_cur(k-nx_); push_cur(k-nx_+1); push_cur(k+nx_*2); push_cur(k+nx_*2+1); }else{ potential[k] = 0; push_cur(k+1); push_cur(k-1); push_cur(k-nx_); push_cur(k+nx_); } int nwv = 0; // max priority block size int nc = 0; // number of cells put into priority blocks int cycle = 0; // which cycle we're on // set up start cell int startCell = toIndex(end_x, end_y); for (; cycle < cycles; cycle++) // go for this many cycles, unless interrupted { // if (currentEnd_ == 0 && nextEnd_ == 0) // priority blocks empty return false; // stats nc += currentEnd_; if (currentEnd_ > nwv) nwv = currentEnd_; // reset pending_ flags on current priority buffer int *pb = currentBuffer_; int i = currentEnd_; while (i-- > 0) pending_[*(pb++)] = false; // process current priority buffer pb = currentBuffer_; i = currentEnd_; while (i-- > 0) updateCell(costs, potential, *pb++); // swap priority blocks currentBuffer_ <=> nextBuffer_ currentEnd_ = nextEnd_; nextEnd_ = 0; pb = currentBuffer_; // swap buffers currentBuffer_ = nextBuffer_; nextBuffer_ = pb; // see if we're done with this priority level if (currentEnd_ == 0) { threshold_ += priorityIncrement_; // increment priority threshold currentEnd_ = overEnd_; // set current to overflow block overEnd_ = 0; pb = currentBuffer_; // swap buffers currentBuffer_ = overBuffer_; overBuffer_ = pb; } // check if we've hit the Start cell if (potential[startCell] < POT_HIGH) break; } //ROS_INFO("CYCLES %d/%d ", cycle, cycles); if (cycle < cycles) return true; // finished up here else return false;计算路径path } }
怎么计算一个点的可行点
namespace global_planner { float QuadraticCalculator::calculatePotential(float* potential, unsigned char cost, int n, float prev_potential) { // get neighbors float u, d, l, r;namespace l = potential[n - 1]; r = potential[n + 1]; u = potential[n - nx_]; d = potential[n + nx_]; // ROS_INFO("[Update] c: %f l: %f r: %f u: %f d: %f\n", // potential , l, r, u, d); // ROS_INFO("[Update] cost: %d\n", costs ); // find lowest, and its lowest neighbor float ta, tc; if (l < r) tc = l; else tc = r; if (u < d) ta = u; else ta = d; float hf = cost; // traversability factor float dc = tc - ta; // relative cost between ta,tc if (dc < 0) // tc is lowest { dc = -dc; ta = tc; } // calculate new potential if (dc >= hf) // if too large, use ta-only update return ta + hf; else // two-neighbor interpolation update { // use quadratic approximation // might speed this up through table lookup, but still have to // do the divide float d = dc / hf; float v = -0.2301 * d * d + 0.5307 * d + 0.7040; return ta + hf * v; } } };
从可行点中计算路径path
bool GradientPath::getPath(float* potential, double start_x, double start_y, double goal_x, double goal_y, std::vector<std::pair<float, float> >& path) { std::pair<float, float> current; int stc = getIndex(goal_x, goal_y); // set up offset float dx = goal_x - (int)goal_x; float dy = goal_y - (int)goal_y; int ns = xs_ * ys_; memset(gradx_, 0, ns * sizeof(float)); memset(grady_, 0, ns * sizeof(float)); int c = 0; while (c++<ns*4) { // check if near goal double nx = stc % xs_ + dx, ny = stc / xs_ + dy; if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .5) { current.first = start_x; current.second = start_y; path.push_back(current); return true; } if (stc < xs_ || stc > xs_ * ys_ - xs_) // would be out of bounds { printf("[PathCalc] Out of bounds\n"); return false; } current.first = nx; current.second = ny; //ROS_INFO("%d %d | %f %f ", stc%xs_, stc/xs_, dx, dy); path.push_back(current); bool oscillation_detected = false; int npath = path.size(); if (npath > 2 && path[npath - 1].first == path[npath - 3].first && path[npath - 1].second == path[npath - 3].second) { ROS_DEBUG("[PathCalc] oscillation detected, attempting fix."); oscillation_detected = true; } int stcnx = stc + xs_; int stcpx = stc - xs_; // check for potentials at eight positions near cell if (potential[stc] >= POT_HIGH || potential[stc + 1] >= POT_HIGH || potential[stc - 1] >= POT_HIGH || potential[stcnx] >= POT_HIGH || potential[stcnx + 1] >= POT_HIGH || potential[stcnx - 1] >= POT_HIGH || potential[stcpx] >= POT_HIGH || potential[stcpx + 1] >= POT_HIGH || potential[stcpx - 1] >= POT_HIGH || oscillation_detected) { ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potential[stc], (int) path.size()); // check eight neighbors to find the lowest int minc = stc; int minp = potential[stc]; int st = stcpx - 1; if (potential[st] < minp) { minp = potential[st]; minc = st; } st++; if (potential[st] < minp) { minp = potential[st]; minc = st; } st++; if (potential[st] < minp) { minp = potential[st]; minc = st; } st = stc - 1; if (potential[st] < minp) { minp = potential[st]; minc = st; } st = stc + 1; if (potential[st] < minp) { minp = potential[st]; minc = st; } st = stcnx - 1; if (potential[st] < minp) { minp = potential[st]; minc = st; } st++; if (potential[st] < minp) { minp = potential[st]; minc = st; } st++; if (potential[st] < minp) { minp = potential[st]; minc = st; } stc = minc; dx = 0; dy = 0; //ROS_DEBUG("[Path] Pot: %0.1f pos: %0.1f,%0.1f", // potential[stc], path[npath-1].first, path[npath-1].second); if (potential[stc] >= POT_HIGH) { ROS_DEBUG("[PathCalc] No path found, high potential"); //savemap("navfn_highpot"); return 0; } } // have a good gradient here else { // get grad at four positions near cell gradCell(potential, stc); gradCell(potential, stc + 1); gradCell(potential, stcnx); gradCell(potential, stcnx + 1); // get interpolated gradient float x1 = (1.0 - dx) * gradx_[stc] + dx * gradx_[stc + 1]; float x2 = (1.0 - dx) * gradx_[stcnx] + dx * gradx_[stcnx + 1]; float x = (1.0 - dy) * x1 + dy * x2; // interpolated x float y1 = (1.0 - dx) * grady_[stc] + dx * grady_[stc + 1]; float y2 = (1.0 - dx) * grady_[stcnx] + dx * grady_[stcnx + 1]; float y = (1.0 - dy) * y1 + dy * y2; // interpolated y // show gradients ROS_DEBUG( "[Path] %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f; final x=%.3f, y=%.3f\n", gradx_[stc], grady_[stc], gradx_[stc+1], grady_[stc+1], gradx_[stcnx], grady_[stcnx], gradx_[stcnx+1], grady_[stcnx+1], x, y); // check for zero gradient, failed if (x == 0.0 && y == 0.0) { ROS_DEBUG("[PathCalc] Zero gradient"); return 0; } // move in the right direction float ss = pathStep_ / hypot(x, y); dx += x * ss; dy += y * ss; // check for overflow if (dx > 1.0) { stc++; dx -= 1.0; } if (dx < -1.0) { stc--; dx += 1.0; } if (dy > 1.0) { stc += xs_; dy -= 1.0; } if (dy < -1.0) { stc -= xs_; dy += 1.0; } } //printf("[Path] Pot: %0.1f grad: %0.1f,%0.1f pos: %0.1f,%0.1f\n", // potential[stc], dx, dy, path[npath-1].first, path[npath-1].second); } return false; }
todo
sbpl,这也是个global planner,没有试过相关文章推荐
- move_base的 局部路径规划代码研究
- ROS探索总结(十三)(十四)(十五)——导航与定位框架 move_base(路径规划) amcl(导航与定位)
- agv 路径规划move_base
- ROS探索总结(十四)——move_base(路径规划)
- ROS探索总结(十四)——move_base(路径规划)
- 【ROS】移动机器人导航仿真(3)——定位(amcl)和路径规划(move_base)
- ROS探索总结(十四)——move_base(路径规划)
- ROS探索总结(十四)——move_base(路径规划)
- ROS探索总结(十四)——move_base(路径规划)
- Navigation中的的move_base路径规划调用
- 使用move_base进行路径规划
- ros全局路径规划分享
- 在jsp中默认写上的一段java代码表示basePath 的路径的具体的意思是什么?
- 激光SLAM导航系列(四)全局路径规划
- 基于问句实体扩展和全局规划的答案摘要方法研究相关论文
- 起伏地形环境多机器人编队运动控制与路径规划研究_2016年中小结
- 接收导航包全局规划路径话题
- 为ROS navigation功能包添加自定义的全局路径规划器(Global Path Planner)
- 起伏地形环境多机器人编队运动控制与路径规划研究_2016年中小结
- 起伏地形环境多机器人编队运动控制与路径规划研究_2016年中小结