博客轉載自：https://blog.csdn.net/Neo11111/article/details/104643134算法
Movebase使用的全局規劃器默認爲NavFn，默認使用Dijkstra算法，在地圖上的起始點和目標點間規劃出一條最優路徑，供局部規劃器具體導航使用。在理解這部分的過程當中也參考了不少博客，NavFn的源碼中其實是有A*規劃算法的函數的，但關於爲何不在NavFn中使用A*，ROS Wiki上對提問者的回答是，早期NavFn包中的A*有bug，沒有處理，後來發佈了global_planner，修改好了A*的部分。global_planner封裝性更強，它和NavFn都是用於全局路徑規劃的包。數組
【相關文件】
- navfn/src/navfn_ros.cpp
- navfn/src/navfn.cpp
navfn_ros.cpp中定義了NavfnROS類，navfn.cpp中定義了NavFn類，ROS Navigation整個包的一個命名規則是，帶有ROS後綴的類完成的是該子過程與總體和其餘過程的銜接框架和數據流通，不帶ROS後綴的類中完成該部分的實際工做，並做爲帶有ROS後綴的類的成員。服務器
本篇記錄對navfn_ros.cpp中定義的NavfnROS類的閱讀和理解。併發
【代碼分析】
navfn_ros.cpp
–目錄–
準備工做 NavfnROS::initialize | 初始化
核心函數 NavfnROS::makePlan | 調用成員類NavFn的規劃函數
獲取單點Potential值 NavfnROS::getPointPotential | 在NavfnROS::makePlan中被調用
獲取規劃結果 NavfnROS::getPlanFromPotential | 在NavfnROS::makePlan中被調用
app
<1> NavfnROS::initialize
這裏主要對參數進行初始化，在MoveBase中首先被調用。這裏先用參數傳入的costmap對地圖進行初始化。框架
void NavfnROS::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame){ if(!initialized_){ costmap_ = costmap;//全局代價地圖 global_frame_ = global_frame;
並對成員類NavFn初始化，這個類將完成全局規劃實際計算。函數
//指向NavFn類實例，傳入參數爲地圖大小 planner_ = boost::shared_ptr<NavFn>(new NavFn(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY())); //建立全局規劃器名稱下的句柄 ros::NodeHandle private_nh("~/" + name); //發佈全局規劃器名稱/plan話題 plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1); //獲取參數服務器上的參數，若是沒有，就使用默認值 private_nh.param("visualize_potential", visualize_potential_, false); //若是要將potential array可視化，則發佈節點名稱下的/potential話題，須要的用戶能夠訂閱 if(visualize_potential_) potarr_pub_.advertise(private_nh, "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); //獲取tf前綴 ros::NodeHandle prefix_nh; tf_prefix_ = tf::getPrefixParam(prefix_nh); //發佈make_plan的服務 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"); }
<2> NavfnROS::makePlan
makePlan是在Movebase中對全局規劃器調用的函數，它是NavfnROS類的重點函數，負責調用包括Navfn類成員在內的函數完成實際計算，控制着全局規劃的整個流程。它的輸入中最重要的是當前和目標的位置。ui
bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& 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; }
準備工做：規劃前先清理plan，等待tf，存儲當前起點位置並轉換到地圖座標系，並將全局costmap上起點的cell設置爲FREE_SPACE。this
//清理plan plan.clear(); ros::NodeHandle n; //確保收到的目標和當前位姿都是基於當前的global frame //注：tf::resolve或者TransformListener::resolve方法的做用就是使用tf_prefix參數將frame_name解析爲frame_id if(tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame_)){ ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame_).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str()); return false; } if(tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame_)){ ROS_ERROR("The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame_).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str()); return false; } //起始位姿wx、wy double wx = start.pose.position.x; double wy = start.pose.position.y; //全局代價地圖座標系上的起始位姿mx、my 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; } //清理起始位置cell（必不是障礙物） tf::Stamped<tf::Pose> start_pose; tf::poseStampedMsgToTF(start, start_pose); clearRobotCell(start_pose, mx, my);
planner指向的是NavFn類，這裏調用它的setNavArr函數，主要做用是給定地圖的大小，建立NavFn類中使用的costarr數組（記錄全局costmap信息）、potarr數組（儲存各cell的Potential值）、以及x和y向的梯度數組（用於生成路徑），這三個數組構成NavFn類用Dijkstra計算的主幹，在NavFn類中詳述。
調用setCostmap函數給全局costmap賦值。
spa
#if 0 { static int n = 0; static char filename[1000]; snprintf( filename, 1000, "navfnros-makeplan-costmapB-%04d.pgm", n++ ); costmap->saveRawMap( std::string( filename )); } #endif //從新設置Navfn使用的underlying array的大小，並將傳入的代價地圖設置爲將要使用的全局代價地圖 planner_->setNavArr(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
這裏和上邊有兩部分用於保存不一樣格式的地圖，與主體關係不大。
#if 0 { static int n = 0; static char filename[1000]; snprintf( filename, 1000, "navfnros-makeplan-costmapC-%04d", n++ ); planner_->savemap( filename ); } #endif //起始位姿存入map_start[2] int map_start[2]; map_start[0] = mx; map_start[1] = my; //獲取global系下的目標位置 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; } //目標位置存入map_goal[2] int map_goal[2]; map_goal[0] = mx; map_goal[1] = my;
接下來將設置NavFn類的起點和目標位置，並調用該類的calcNavFnDijkstra函數，這個函數能夠完成全局路徑的計算。
//傳入Navfn實例中 planner_->setStart(map_goal); planner_->setGoal(map_start); planner_->calcNavFnDijkstra(true);
接下來，在目標位置附近2*tolerance的矩形範圍內，尋找與目標位置最近的、且不是障礙物的cell，做爲全局路徑實際的終點，這裏調用了類內getPointPotential函數，目的是獲取單點Potential值，與DBL_MAX比較，肯定是不是障礙物。
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; }
若成功找到實際終點best_pose，調用類內getPlanFromPotential函數，將best_pose傳遞給NavFn，得到最終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."); } }
potarr數組的發佈，與主體關係不大。
if (visualize_potential_){
//發佈Potential數組
pcl::PointCloud<PotarrPoint> pot_area;
pot_area.header.frame_id = global_frame_;
pot_area.points.clear();
std_msgs::Header header;
pcl_conversions::fromPCL(pot_area.header, header);
header.stamp = ros::Time::now();
pot_area.header = pcl_conversions::toPCL(header);
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);
pt.x = pot_x;
pt.y = pot_y;
pt.z = pp[i]/pp[planner_->start[1]*planner_->nx + planner_->start[0]]*20;
pt.pot_value = pp[i];
pot_area.push_back(pt);
}
}
potarr_pub_.publish(pot_area);
}
//plan發佈
publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
return !plan.empty();
}
<3> NavfnROS::getPointPotential
它在makePlan中被調用，主要工做是獲取NavFn類成員-potarr數組記錄的對應cell的Potential值。
double NavfnROS::getPointPotential(const geometry_msgs::Point& world_point){ //檢查是否初始化 if(!initialized_){ ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); return -1.0; } //將點轉換到地圖座標系下 unsigned int mx, my; if(!costmap_->worldToMap(world_point.x, world_point.y, mx, my)) return DBL_MAX; //nx、ny是像素單位的地圖網格的x、y方向上長度 //計算矩陣中的索引=地圖x向長度*點的y座標+點的x座標 unsigned int index = my * planner_->nx + mx; //potarr即Potential Array，勢場矩陣 //傳入索引，得該點勢場 return planner_->potarr[index]; }
<4> NavfnROS::getPlanFromPotential
它在makePlan中被調用，主要工做是調用了NavFn類的一些函數，設置目標、獲取規劃結果。
bool NavfnROS::getPlanFromPotential(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& 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(tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame_)){ ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame_).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str()); return false; }
下面將makePlan末尾處找到的goal附近的best_pose座標轉換到地圖座標系，並經過調用NavFn類的setStart函數傳遞，做爲路徑的實際終點，再調用NavFn類calcPath函數，完成路徑計算。
//儲存bestpose的座標 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; } //besepose轉換到map座標系後存儲 int map_goal[2]; map_goal[0] = mx; map_goal[1] = my; //調用navfn的設置起始、calcPath、getPathX等函數，並將計算出的路徑點依次存放plan，獲得全局規劃路線 planner_->setStart(map_goal); planner_->calcPath(costmap_->getSizeInCellsX() * 4);
調用NavFn的類方法獲取規劃結果的座標，填充plan以後將其發佈。
//extract the plan 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); } //publish the plan for visualization purposes publishPlan(plan, 0.0, 1.0, 0.0, 0.0); return !plan.empty(); } };