版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/Nksjc/article/details/78812066
navigation
nav_core
navigation包里有一个nav_core，是一个纯粹的接口package，里面规定了核心的几个类应该有基本功能。BaseGlobalPlanner是全局导航的接口，规定一个功能函数makePlan，给定起始跟目标，输出路径(一系列pose)走过去；BaseLocalPlanner规定了一个核心函数computeVelocityCommands，就是计算局部地图内的下一步控制指令(线速度，角速度)；还有一个RecoveryBehavior，规定一个runBehavior，在小车卡住情况下执行运动恢复，回到正常的导航状态。
global_planner
在nav_core的global_planner中的继承关系:
可以知道在nav_core的接口上，又提供了carrot_planner::CarrotPlanner,navfn::NavfnROS，这里介绍的是global_planner::GlobalPlanner的实现。
在全局规划中，提供了例如A star和Dijstra算法;提供处理规划之后的路径的类型的算法。
expander规定了calculatePotentials的接口，根据costmap跟起始终止点计算网格的potential。关于算法，D..就不看了，A*的算法思想如下:
A* Algorithm
cost // the cost of every grid in the map
potential // the cost of every grid from the start node
OPEN // the set of nodes to be evaluated
CLOSED // the set of nodes already evaluated
add the start node to OPEN
loop
current = node in OPEN with the lowest f_cost // from cost list
remove current from OPEN
add current to CLOSED
if current is the target node // path has been found
return
foreach neighbor of the current node
if neighbour is not traversable or neighbor is in CLOSED
skip to the next neighbor
if new path to neighbour is shorter OR neighbour is not in OPEN
set f_cost of neighbour
set parent of neighbour to current
if neighbour is not in OPEN
add neighbour to OPEN通过设置的代价potential[next_i] + distance * neutral_cost_ 代表了A*的核心思想，寻找距离start和goal代价距离都最少的点。 A* 算法是策略寻路，不保证一定是最短路径。 Dijkstra 算法是全局遍历，确保运算结果一定是最短路径。 Dijkstra 需要载入全部数据，遍历搜索。
bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
int cycles, float* potential)
{
queue_.clear();
int start_i = toIndex(start_x, start_y);
queue_.push_back(Index(start_i, 0)); //push the start point into OPEN queue_
std::fill(potential, potential + ns_, POT_HIGH); //initial all the potential as very large value 1e10
potential[start_i] = 0; //initial the start position at potential as 0
int goal_i = toIndex(end_x, end_y);
int cycle = 0;
while (queue_.size() > 0 && cycle < cycles) //loop
{
Index top = queue_[0]; //get the Index with lowest cost (set to current)
std::pop_heap(queue_.begin(), queue_.end(), greater1());//build the heap sort
queue_.pop_back(); //remove the Index with mini cost (remove from OPEN)
int i = top.i; //target node the Index's i from (i,cost)
if (i == goal_i)
return true;
//foreach neighbour node (*) of the current node(0),
// + * +
// * 0 *
// + * +
add(costs, potential, potential[i], i + 1, end_x, end_y);
add(costs, potential, potential[i], i - 1, end_x, end_y);
add(costs, potential, potential[i], i + nx_, end_x, end_y);
add(costs, potential, potential[i], i - nx_, end_x, end_y);
}
return false;
}
void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,int end_y)
{
if (potential[next_i] < POT_HIGH) //means the potential node was traversable
return;
if(costs[next_i]>=lethal_cost_ &&
!(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION)) //means this node is a obstacle
return;
// compute the next_i node in potential
// costs[next_i] + neutral_cost_:original cost + extra cost
// potential[next_i] means the cost from start to current
potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);
int x = next_i % nx_, y = next_i / nx_;
float distance = abs(end_x - x) + abs(end_y - y);
//distance * neutral_cost_(50) means the current to the target
//f(n)=g(n)+h(n)
queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
std::push_heap(queue_.begin(), queue_.end(), greater1()); //sort about the previous insert element
}在代码之中的A*只是计算了一个Potential,并没有直接得到一条路径。在Traceback中规定了getpath的接口，根据potential把得到的全局路径找出来，得到一系列路径点（std::vector<std::pair<float, float> >& path）。得到的方式由上图关于Trackback的继承关系，可以看出GradientPath,GridPath。
bool GridPath::getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) {
std::pair<float, float> current;
current.first = end_x;
current.second = end_y;
int start_index = getIndex(start_x, start_y);
path.push_back(current); // insert goal to path
int c = 0;
int ns = xs_ * ys_;
while (getIndex(current.first, current.second) != start_index) {
float min_val = 1e10;
int min_x = 0, min_y = 0;
// find the lowest cost neighbour(*) arround current node(0) and insert the lowest to path
// * * *
// * 0 *
// * * *
for (int xd = -1; xd <= 1; xd++) {
for (int yd = -1; yd <= 1; yd++) {
if (xd == 0 && yd == 0)
continue;
int x = current.first + xd, y = current.second + yd;
int index = getIndex(x, y);
if (potential[index] < min_val) {
min_val = potential[index];
min_x = x;
min_y = y;
}
}
}
if (min_x == 0 && min_y == 0)
return false;
// refresh the current node and we konw the start node's cost is zero
// get the path using gradient descent method
current.first = min_x;
current.second = min_y;
path.push_back(current);
if(c++>ns*4){
return false;
}
}
return true;
}路径点找出来就好办了，在GlobalPlanner::makePlan里调用GlobalPlanner::getPlanFromPotential，把path简单转换下变成std::vector<geometry_msgs::PoseStamped>& plan，然后makePlan再用OrientationFilter做完平滑之后发布出去。函数的主体如下:
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan)
{
plan.clear();
double wx = start.pose.position.xi; double wy = start.pose.position.y;
costmap_->worldToMap(wx, wy, start_x_i, start_y_i);
costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i));
clearRobotCell(start_pose, start_x_i, start_y_i); // set the start node as free node
p_calc_->setSize(nx, ny); planner_->setSize(nx, ny);
path_maker_->setSize(nx, ny);
potential_array_ = new float[nx * ny];
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y, nx * ny * 2, potential_array_);
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if (found_legal)
{
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan))
{
geometry_msgs::PoseStamped goal_copy = goal;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);
}
}
orientation_filter_->processPath(start, plan);
}使用方式:
robot@robot:~/catkin_ws$ rospack plugins --attrib=plugin nav_core
clear_costmap_recovery /home/robot/catkin_ws/src/navigation/clear_costmap_recovery/ccr_plugin.xml
base_local_planner /home/robot/catkin_ws/src/navigation/base_local_planner/blp_plugin.xml
dwa_local_planner /home/robot/catkin_ws/src/navigation/dwa_local_planner/blp_plugin.xml
carrot_planner /home/robot/catkin_ws/src/navigation/carrot_planner/bgp_plugin.xml
rotate_recovery /home/robot/catkin_ws/src/navigation/rotate_recovery/rotate_plugin.xml
move_slow_and_clear /home/robot/catkin_ws/src/navigation/move_slow_and_clear/recovery_plugin.xml
navfn /home/robot/catkin_ws/src/navigation/navfn/bgp_plugin.xml
global_planner /home/robot/catkin_ws/src/navigation/global_planner/bgp_plugin.xmlMoveBase使用插件的方式加载进来，以nav_core::BaseGlobalPlanner为例，实例化一个对象的过程如下。
MoveBase::MoveBase(tf::TransformListener& tf) :
tf_(tf),
as_(NULL),
planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
planner_ = bgp_loader_.createInstance(global_planner);
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
-----------------------------------------------------------
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));move_base的那个变量参数默认的是navfn，也就是说没用global_planner。那么如何使用global_planner包而不是navfn包呢？只要将move_base的参数base_global_planner用global_planner/PlannerCore替代就行了。