Movebase使用的全局規(guī)劃器默認(rèn)為NavFn但校，默認(rèn)使用Dijkstra算法光稼，在地圖上的起始點(diǎn)和目標(biāo)點(diǎn)間規(guī)劃出一條最優(yōu)路徑坤次，供局部規(guī)劃器具體導(dǎo)航使用。
在理解這部分的過(guò)程中也參考了很多博客揪胃，NavFn的源碼中實(shí)際上是有A* 規(guī)劃算法的函數(shù)的璃哟，但關(guān)于為什么不在NavFn中使用A* ，ROS Wiki上對(duì)提問(wèn)者的回答是喊递，早期NavFn包中的A* 有bug沮稚，沒(méi)有處理，后來(lái)發(fā)布了global_planner册舞，修改好了A* 的部分。
global_planner封裝性更強(qiáng)障般，它和NavFn都是用于全局路徑規(guī)劃的包调鲸。
【結(jié)構(gòu)示意圖】
【相關(guān)文件】
- navfn/src/navfn_ros.cpp
- navfn/src/navfn.cpp
navfn_ros.cpp中定義了NavfnROS類(lèi)盛杰，navfn.cpp中定義了NavFn類(lèi)，ROS Navigation整個(gè)包的一個(gè)命名規(guī)則是藐石，帶有ROS后綴的類(lèi)完成的是該子過(guò)程與整體和其他過(guò)程的銜接框架和數(shù)據(jù)流通即供，不帶ROS后綴的類(lèi)中完成該部分的實(shí)際工作，并作為帶有ROS后綴的類(lèi)的成員于微。
本篇記錄對(duì)navfn_ros.cpp中定義的NavfnROS類(lèi)的閱讀和理解逗嫡。
【代碼分析】
navfn_ros.cpp
–目錄–
一、準(zhǔn)備工作 NavfnROS::initialize | 初始化
二株依、核心函數(shù) NavfnROS::makePlan | 調(diào)用成員類(lèi)NavFn的規(guī)劃函數(shù)
三驱证、獲取單點(diǎn)Potential值 NavfnROS::getPointPotential | 在NavfnROS::makePlan中被調(diào)用
四、獲取規(guī)劃結(jié)果 NavfnROS::getPlanFromPotential | 在NavfnROS::makePlan中被調(diào)用
一恋腕、NavfnROS::initialize
這里主要對(duì)參數(shù)進(jìn)行初始化抹锄，在MoveBase中首先被調(diào)用。這里先用參數(shù)傳入的costmap對(duì)地圖進(jìn)行初始化荠藤。
void NavfnROS::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame){
if(!initialized_){
costmap_ = costmap;//全局代價(jià)地圖
global_frame_ = global_frame;
并對(duì)成員類(lèi)NavFn初始化伙单，這個(gè)類(lèi)將完成全局規(guī)劃實(shí)際計(jì)算。
//指向NavFn類(lèi)實(shí)例哈肖，傳入?yún)?shù)為地圖大小
planner_ = boost::shared_ptr<NavFn>(new NavFn(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()));
//創(chuàng)建全局規(guī)劃器名稱(chēng)下的句柄
ros::NodeHandle private_nh("~/" + name);
//發(fā)布全局規(guī)劃器名稱(chēng)/plan話(huà)題
plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
//獲取參數(shù)服務(wù)器上的參數(shù)吻育，如果沒(méi)有，就使用默認(rèn)值
private_nh.param("visualize_potential", visualize_potential_, false);
//如果要將potential array可視化淤井，則發(fā)布節(jié)點(diǎn)名稱(chēng)下的/potential話(huà)題布疼，需要的用戶(hù)可以訂閱
if(visualize_potential_)
potarr_pub_.advertise(private_nh, "potential", 1);
//從參數(shù)服務(wù)器上獲取以下參數(shù)
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);
//發(fā)布make_plan的服務(wù)
make_plan_srv_ = private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this);
//初始化標(biāo)志位設(shè)為真
initialized_ = true;
}
else
//否則，已經(jīng)被初始化過(guò)了庄吼，打印提示即可缎除，不重復(fù)初始化
ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
}
二、NavfnROS::makePlan
makePlan是在Movebase中對(duì)全局規(guī)劃器調(diào)用的函數(shù)总寻，它是NavfnROS類(lèi)的重點(diǎn)函數(shù)器罐，負(fù)責(zé)調(diào)用包括Navfn類(lèi)成員在內(nèi)的函數(shù)完成實(shí)際計(jì)算，控制著全局規(guī)劃的整個(gè)流程渐行。它的輸入中最重要的是當(dāng)前和目標(biāo)的位置轰坊。
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;
}
準(zhǔn)備工作：規(guī)劃前先清理plan，等待tf祟印，存儲(chǔ)當(dāng)前起點(diǎn)位置并轉(zhuǎn)換到地圖坐標(biāo)系肴沫，并將全局costmap上起點(diǎn)的cell設(shè)置為FREE_SPACE。
//清理plan
plan.clear();
ros::NodeHandle n;
//確保收到的目標(biāo)和當(dāng)前位姿都是基于當(dāng)前的global frame
//注：tf::resolve或者TransformListener::resolve方法的作用就是使用tf_prefix參數(shù)將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;
//全局代價(jià)地圖坐標(biāo)系上的起始位姿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類(lèi)，這里調(diào)用它的setNavArr函數(shù)，主要作用是給定地圖的大小站蝠，創(chuàng)建NavFn類(lèi)中使用的costarr數(shù)組（記錄全局costmap信息）汰具、potarr數(shù)組（儲(chǔ)存各cell的Potential值）、以及x和y向的梯度數(shù)組（用于生成路徑）菱魔，這三個(gè)數(shù)組構(gòu)成NavFn類(lèi)用Dijkstra計(jì)算的主干留荔，在NavFn類(lèi)中詳述。
調(diào)用setCostmap函數(shù)給全局costmap賦值澜倦。
#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
//重新設(shè)置Navfn使用的underlying array的大小聚蝶，并將傳入的代價(jià)地圖設(shè)置為將要使用的全局代價(jià)地圖
planner_->setNavArr(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
這里和上邊有兩部分用于保存不同格式的地圖，與主體關(guān)系不大藻治。
#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系下的目標(biāo)位置
wx = goal.pose.position.x;
wy = goal.pose.position.y;
//坐標(biāo)轉(zhuǎn)換到地圖坐標(biāo)系
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;
}
//目標(biāo)位置存入map_goal[2]
int map_goal[2];
map_goal[0] = mx;
map_goal[1] = my;
接下來(lái)將設(shè)置NavFn類(lèi)的起點(diǎn)和目標(biāo)位置碘勉，并調(diào)用該類(lèi)的calcNavFnDijkstra函數(shù)，這個(gè)函數(shù)可以完成全局路徑的計(jì)算栋艳。
//傳入Navfn實(shí)例中
planner_->setStart(map_goal);
planner_->setGoal(map_start);
planner_->calcNavFnDijkstra(true);
接下來(lái)恰聘，在目標(biāo)位置附近2*tolerance的矩形范圍內(nèi)，尋找與目標(biāo)位置最近的吸占、且不是障礙物的cell晴叨，作為全局路徑實(shí)際的終點(diǎn)，這里調(diào)用了類(lèi)內(nèi)getPointPotential函數(shù)矾屯，目的是獲取單點(diǎn)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;
}
若成功找到實(shí)際終點(diǎn)best_pose孙技，調(diào)用類(lèi)內(nèi)getPlanFromPotential函數(shù)，將best_pose傳遞給NavFn排作，獲得最終Plan并發(fā)布牵啦。
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數(shù)組的發(fā)布，與主體關(guān)系不大妄痪。
if (visualize_potential_){
//發(fā)布Potential數(shù)組
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發(fā)布
publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
return !plan.empty();
}
三哈雏、NavfnROS::getPointPotential
它在makePlan中被調(diào)用，主要工作是獲取NavFn類(lèi)成員-potarr數(shù)組記錄的對(duì)應(yīng)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;
}
//將點(diǎn)轉(zhuǎn)換到地圖坐標(biāo)系下
unsigned int mx, my;
if(!costmap_->worldToMap(world_point.x, world_point.y, mx, my))
return DBL_MAX;
//nx裳瘪、ny是像素單位的地圖網(wǎng)格的x、y方向上長(zhǎng)度
//計(jì)算矩陣中的索引=地圖x向長(zhǎng)度*點(diǎn)的y坐標(biāo)+點(diǎn)的x坐標(biāo)
unsigned int index = my * planner_->nx + mx;
//potarr即Potential Array罪针，勢(shì)場(chǎng)矩陣
//傳入索引彭羹，得該點(diǎn)勢(shì)場(chǎng)
return planner_->potarr[index];
}
四、NavfnROS::getPlanFromPotential
它在makePlan中被調(diào)用泪酱，主要工作是調(diào)用了NavFn類(lèi)的一些函數(shù)派殷，設(shè)置目標(biāo)还最、獲取規(guī)劃結(jié)果。
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坐標(biāo)轉(zhuǎn)換到地圖坐標(biāo)系毡惜，并通過(guò)調(diào)用NavFn類(lèi)的setStart函數(shù)傳遞憋活，作為路徑的實(shí)際終點(diǎn)，再調(diào)用NavFn類(lèi)calcPath函數(shù)虱黄，完成路徑計(jì)算。
//儲(chǔ)存bestpose的坐標(biāo)
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轉(zhuǎn)換到map坐標(biāo)系后存儲(chǔ)
int map_goal[2];
map_goal[0] = mx;
map_goal[1] = my;
//調(diào)用navfn的設(shè)置起始吮成、calcPath橱乱、getPathX等函數(shù)，并將計(jì)算出的路徑點(diǎn)依次存放plan粱甫，得到全局規(guī)劃路線
planner_->setStart(map_goal);
planner_->calcPath(costmap_->getSizeInCellsX() * 4);
調(diào)用NavFn的類(lèi)方法獲取規(guī)劃結(jié)果的坐標(biāo)泳叠，填充plan之后將其發(fā)布。
//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();
}
};
轉(zhuǎn)載：https://blog.csdn.net/Neo11111/article/details/104643134
