全局规划器-NavfnROS

本文链接地址: 全局规划器-NavfnROS

Content:

1. 调用路径
2. planner初始化
3. 规划路径
4. 核心算法：Dijkstra规划算法

全局规划器就如同平时生活中的地图导航一样。

调用路径

在Move Base中有全局规划器的如下调用，完成了全局规划器的实例化：

try { planner_ = bgp_loader_.createInstance(global_planner); planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_); } catch (const pluginlib::PluginlibException& ex) { ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what()); exit(1); }

planner初始化

planner_由类NavFn实现，plan_pub_用于发布规划路径，make_plan_srv_提供路径规划服务。

void NavfnROS::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame){ if(!initialized_){ costmap_ = costmap; global_frame_ = global_frame; planner_ = boost::shared_ptr(new NavFn(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()));   ros::NodeHandle private_nh("~/" + name);   plan_pub_ = private_nh.advertise("plan", 1);   private_nh.param("visualize_potential", visualize_potential_, false);   //if we're going to visualize the potential array we need to advertise if(visualize_potential_) potarr_pub_ = private_nh.advertise("potential", 1);   private_nh.param("allow_unknown", allow_unknown_, true); private_nh.param("planner_window_x", planner_window_x_, 0.0); private_nh.param("planner_window_y", planner_window_y_, 0.0); private_nh.param("default_tolerance", default_tolerance_, 0.0);   make_plan_srv_ = private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this);   initialized_ = true; } else ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing"); }

规划路径

NavfnROS::makePlan方法生成规划路径：

bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector& plan){ boost::mutex::scoped_lock lock(mutex_); if(!initialized_){ ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); return false; }   //clear the plan, just in case plan.clear();   ros::NodeHandle n;   //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame if(goal.header.frame_id != global_frame_){ ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame_.c_str(), goal.header.frame_id.c_str()); return false; }   if(start.header.frame_id != global_frame_){ ROS_ERROR("The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame_.c_str(), start.header.frame_id.c_str()); return false; } //获取开始位置，并确保开始位置在地图内 double wx = start.pose.position.x; double wy = start.pose.position.y;   unsigned int mx, my; if(!costmap_->worldToMap(wx, wy, mx, my)){ 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; } //清除小车位置的障碍物设定，否则就无法移动了 //clear the starting cell within the costmap because we know it can't be an obstacle clearRobotCell(start, mx, my); //确定导航生成的范围在地图内 //make sure to resize the underlying array that Navfn uses planner_->setNavArr(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);   int map_start[2]; map_start[0] = mx; map_start[1] = my; //同样，确认目标地点在地图内 wx = goal.pose.position.x; wy = goal.pose.position.y;   if(!costmap_->worldToMap(wx, wy, mx, my)){ if(tolerance <= 0.0){ ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal."); return false; } mx = 0; my = 0; }   int map_goal[2]; map_goal[0] = mx; map_goal[1] = my; //设置规划起点和目标点 planner_->setStart(map_goal); planner_->setGoal(map_start); //调用calcNavFnDijkstra算法进行路径规划 //bool success = planner_->calcNavFnAstar(); planner_->calcNavFnDijkstra(true);   double resolution = costmap_->getResolution(); geometry_msgs::PoseStamped p, best_pose; p = goal; //找到可能的合法点，只要其距离目标点在容忍的距离内，不是障碍物即可 bool found_legal = false; double best_sdist = DBL_MAX;   p.pose.position.y = goal.pose.position.y - tolerance;   while(p.pose.position.y <= goal.pose.position.y + tolerance){ p.pose.position.x = goal.pose.position.x - tolerance; while(p.pose.position.x <= goal.pose.position.x + tolerance){ double potential = getPointPotential(p.pose.position); double sdist = sq_distance(p, goal); if(potential < POT_HIGH && sdist < best_sdist){ best_sdist = sdist; best_pose = p; found_legal = true; } p.pose.position.x += resolution; } p.pose.position.y += resolution; } //找到合法点，调用getPlanFromPotential方法获取规划plan if(found_legal){ //extract the plan if(getPlanFromPotential(best_pose, plan)){ //make sure the goal we push on has the same timestamp as the rest of the plan geometry_msgs::PoseStamped goal_copy = best_pose; 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."); } } //如果打开可视化可能点显示，就发布这些点，rviz就可以显示它们 if (visualize_potential_) { // Publish the potentials as a PointCloud2 sensor_msgs::PointCloud2 cloud; cloud.width = 0; cloud.height = 0; cloud.header.stamp = ros::Time::now(); cloud.header.frame_id = global_frame_; sensor_msgs::PointCloud2Modifier cloud_mod(cloud); cloud_mod.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, "z", 1, sensor_msgs::PointField::FLOAT32, "pot", 1, sensor_msgs::PointField::FLOAT32); cloud_mod.resize(planner_->ny * planner_->nx); sensor_msgs::PointCloud2Iterator iter_x(cloud, "x");   PotarrPoint pt; float *pp = planner_->potarr; double pot_x, pot_y; for (unsigned int i = 0; i < (unsigned int)planner_->ny*planner_->nx ; i++) { if (pp[i] < 10e7) { mapToWorld(i%planner_->nx, i/planner_->nx, pot_x, pot_y); iter_x[0] = pot_x; iter_x[1] = pot_y; iter_x[2] = pp[i]/pp[planner_->start[1]*planner_->nx + planner_->start[0]]*20; iter_x[3] = pp[i]; ++iter_x; } } potarr_pub_.publish(cloud); }   //publish the plan for visualization purposes publishPlan(plan, 0.0, 1.0, 0.0, 0.0);   return !plan.empty(); }

核心算法：Dijkstra规划算法

下面就是主要的Dijkstra规划算法：
算法通过从目标点进行传播，直到起点。

bool NavFn::calcNavFnDijkstra(bool atStart) { //初始化数组准备下一轮传播 setupNavFn(true); //第一个参数为最大传播次数限制，atStart决定是否在起点停止传播 // calculate the nav fn and path propNavFnDijkstra(std::max(nx*ny/20,nx+ny),atStart); //计算路径，路径最大含有nx*ny/2个路径点 // path int len = calcPath(nx*ny/2);   if (len > 0) // found plan { ROS_DEBUG("[NavFn] Path found, %d steps\n", len); return true; } else { ROS_DEBUG("[NavFn] No path found\n"); return false; }   } // Set up navigation potential arrays for new propagation void NavFn::setupNavFn(bool keepit) { // reset values in propagation arrays for (int i=0; i nextP > overP // priority buffers curT = COST_OBS; curP = pb1; curPe = 0; nextP = pb2; nextPe = 0; overP = pb3; overPe = 0; memset(pending, 0, ns*sizeof(bool)); //设置目标点的cost为0，传播从目标点开始，k是目标点在点数组potarr里面的索引值 //通过计算目标点的邻接点的cost持续向外传播 // set goal int k = goal[0] + goal[1]*nx; initCost(k,0); //计算障碍物点的数量 // find # of obstacle cells pc = costarr; int ntot = 0; for (int i=0; i= COST_OBS) ntot++; // number of cells that are obstacles } nobs = ntot; }   //初始化目标点k的cost为0，并把其邻接点放入当前缓冲器curP中，以便下一次传播 // initialize a goal-type cost for starting propagation void NavFn::initCost(int k, float v) { potarr[k] = v; push_cur(k+1); push_cur(k-1); push_cur(k-nx); push_cur(k+nx); } #define push_cur(n) { if (n>=0 && n nwv) nwv = curPe; //重置当前缓冲器中的点的待计算状态 // reset pending flags on current priority buffer int *pb = curP; int i = curPe; while (i-- > 0) pending[*(pb++)] = false; //调用updateCell计算当前缓冲器中的所有点 // process current priority buffer pb = curP; i = curPe; while (i-- > 0) updateCell(*pb++);   if (displayInt > 0 && (cycle % displayInt) == 0) displayFn(this); //3个优先级，curP完了，接着nextP // swap priority blocks curP <=> nextP curPe = nextPe; nextPe = 0; pb = curP; // swap buffers curP = nextP; nextP = pb; //nextP完了，接着overP // see if we're done with this priority level if (curPe == 0) { curT += priInc; // increment priority threshold curPe = overPe; // set current to overflow block overPe = 0; pb = curP; // swap buffers curP = overP; overP = pb; } //如果覆盖率起点，结束此轮计算 // check if we've hit the Start cell if (atStart) if (potarr[startCell] < POT_HIGH) break; }   ROS_DEBUG("[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n", cycle,nc,(int)((nc*100.0)/(ns-nobs)),nwv);   if (cycle < cycles) return true; // finished up here else return false; }   //计算每个点cost的函数 inline void NavFn::updateCell(int n) { // get neighbors 得到左l 右r 上u 下d 4个点的 cost值 float u,d,l,r; l = potarr[n-1]; r = potarr[n+1]; u = potarr[n-nx]; d = potarr[n+nx]; // ROS_INFO("[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n", // potarr[n], l, r, u, d); // ROS_INFO("[Update] cost: %d\n", costarr[n]);   // find lowest, and its lowest neighbor找到列方向的低cost tc 和交叉方向的低cost ta float ta, tc; if (l= hf) // if too large, use ta-only update pot = 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; pot = ta + hf*v; }   // ROS_INFO("[Update] new pot: %d\n", costarr[n]); //如果计算出的pot值小于当期点的pot值，则表明当前传播路线更优，就更新邻接点 // now add affected neighbors to priority blocks if (pot < potarr[n]) { float le = INVSQRT2*(float)costarr[n-1]; float re = INVSQRT2*(float)costarr[n+1]; float ue = INVSQRT2*(float)costarr[n-nx]; float de = INVSQRT2*(float)costarr[n+nx]; potarr[n] = pot; if (pot < curT) // low-cost buffer block 如果pot值小于当前阈值curT {//邻接点预期计算后的cost会小于当前的cost，则放入nextP进行下一次（curP计算完后）计算 if (l > pot+le) push_next(n-1); if (r > pot+re) push_next(n+1); if (u > pot+ue) push_next(n-nx); if (d > pot+de) push_next(n+nx); } else // overflow block { if (l > pot+le) push_over(n-1); if (r > pot+re) push_over(n+1); if (u > pot+ue) push_over(n-nx); if (d > pot+de) push_over(n+nx); } }   }   }     //根据上面计算出的pot值，算出每个方向的梯度，求出路径 // Path construction // Find gradient at array points, interpolate path // Use step size of pathStep, usually 0.5 pixel // // Some sanity checks: // 1. Stuck at same index position // 2. Doesn't get near goal // 3. Surrounded by high potentials //   int NavFn::calcPath(int n, int *st) { // test write //savemap("test"); //path buf数组不够大时，重新初始化pathx, pathy // check path arrays if (npathbuf < n) { if (pathx) delete [] pathx; if (pathy) delete [] pathy; pathx = new float[n]; pathy = new float[n]; npathbuf = n; } //设置路径起始点，这儿为传入的起点start // set up start position at cell // st is always upper left corner for 4-point bilinear interpolation if (st == NULL) st = start; int stc = st[1]*nx + st[0];   // set up offset float dx=0; float dy=0; npath = 0;   // go for cycles at most for (int i=0; i ns-nx) // would be out of bounds { ROS_DEBUG("[PathCalc] Out of bounds"); return 0; }   // add to path加入当前点到路径 pathx[npath] = stc%nx + dx; pathy[npath] = stc/nx + dy; npath++; //当前点的前后为同一个点，表示有冲突，无法继续向前 bool oscillation_detected = false; if( npath > 2 && pathx[npath-1] == pathx[npath-3] && pathy[npath-1] == pathy[npath-3] ) { ROS_DEBUG("[PathCalc] oscillation detected, attempting fix."); oscillation_detected = true; }   int stcnx = stc+nx; int stcpx = stc-nx;   // check for potentials at eight positions near cell if (potarr[stc] >= POT_HIGH || potarr[stc+1] >= POT_HIGH || potarr[stc-1] >= POT_HIGH || potarr[stcnx] >= POT_HIGH || potarr[stcnx+1] >= POT_HIGH || potarr[stcnx-1] >= POT_HIGH || potarr[stcpx] >= POT_HIGH || potarr[stcpx+1] >= POT_HIGH || potarr[stcpx-1] >= POT_HIGH || oscillation_detected) { ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potarr[stc], npath); // check eight neighbors to find the lowest int minc = stc; int minp = potarr[stc]; int st = stcpx - 1; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st++; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st++; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st = stc-1; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st = stc+1; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st = stcnx-1; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st++; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st++; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } stc = minc; dx = 0; dy = 0;   ROS_DEBUG("[Path] Pot: %0.1f pos: %0.1f,%0.1f", potarr[stc], pathx[npath-1], pathy[npath-1]);   if (potarr[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(stc); gradCell(stc+1); gradCell(stcnx); gradCell(stcnx+1);     // get interpolated gradient float x1 = (1.0-dx)*gradx[stc] + dx*gradx[stc+1];//计算第一行x的梯度 float x2 = (1.0-dx)*gradx[stcnx] + dx*gradx[stcnx+1];//计算第二行x的梯度 float x = (1.0-dy)*x1 + dy*x2; // interpolated x//计算x的梯度 float y1 = (1.0-dx)*grady[stc] + dx*grady[stc+1];//计算第一列y的梯度 float y2 = (1.0-dx)*grady[stcnx] + dx*grady[stcnx+1];//计算第二列y的梯度 float y = (1.0-dy)*y1 + dy*y2; // interpolated y//计算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 0梯度，无法继续，失败 if (x == 0.0 && y == 0.0) { ROS_DEBUG("[PathCalc] Zero gradient"); return 0; }   // move in the right direction 增加对应方向的dx, dy 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+=nx; dy -= 1.0; } if (dy < -1.0) { stc-=nx; dy += 1.0; }   }   // ROS_INFO("[Path] Pot: %0.1f grad: %0.1f,%0.1f pos: %0.1f,%0.1f\n", // potarr[stc], x, y, pathx[npath-1], pathy[npath-1]); }   // return npath; // out of cycles, return failure ROS_DEBUG("[PathCalc] No path found, path too long"); //savemap("navfn_pathlong"); return 0; // out of cycles, return failure }

bool NavfnROS::getPlanFromPotential(const geometry_msgs::PoseStamped& goal, std::vector& plan){ if(!initialized_){ ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); return false; }   //clear the plan, just in case plan.clear();   //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame if(goal.header.frame_id != global_frame_){ ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame_.c_str(), goal.header.frame_id.c_str()); return false; }   double wx = goal.pose.position.x; double wy = goal.pose.position.y;   //the potential has already been computed, so we won't update our copy of the costmap unsigned int mx, my; if(!costmap_->worldToMap(wx, wy, mx, my)){ ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal."); return false; }   int map_goal[2]; map_goal[0] = mx; map_goal[1] = my;   planner_->setStart(map_goal);   planner_->calcPath(costmap_->getSizeInCellsX() * 4);   //extract the plan 从上面plan中获取x,y的坐标 float *x = planner_->getPathX(); float *y = planner_->getPathY(); int len = planner_->getPathLen(); ros::Time plan_time = ros::Time::now();   for(int i = len - 1; i >= 0; --i){ //convert the plan to world coordinates 转换成世界坐标 double world_x, world_y; mapToWorld(x[i], y[i], world_x, world_y);   geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = global_frame_; pose.pose.position.x = world_x; pose.pose.position.y = world_y; pose.pose.position.z = 0.0; pose.pose.orientation.x = 0.0; pose.pose.orientation.y = 0.0; pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; plan.push_back(pose); //把坐标点加入到plan中 }   //publish the plan for visualization purposes publishPlan(plan, 0.0, 1.0, 0.0, 0.0); return !plan.empty(); }