global_planner

ROS

全局的规划的规划，我们可以有三种方式：carrt_planner, navfn和global_planner。
carrot_planner
这是最简单的一种方式。carrot允许机器人尽可能的靠近用户指定的目标点。不需要任何全局的路径规划。但是只是针对短距离的范围。在一些的复杂的室内环境，这种方式并不是很适合。
navfn和global_planner
navfn使用的是Dijkstra算法https://www.cnblogs.com/biyeymyhjob/archive/2012/07/31/2615833.html，找到一条全局的路径，在启点和终点之间。global_planner是一种更加灵活的方式，来替代navfn. global_planner支持A*算法。触发二次方式的逼近。触发网格路径。

在move_base.cpp中planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);

global_planner功能包解析

1.plan_node.cpp
（1）我们的编程在换命名空间名的时候可以这么替换：
namespace cm = costmap_2d;
namespace rm = geometry_msgs;
（2）int main(int argc, char** argv)在其中：
costmap_2d::Costmap2DROS lcr("costmap", buffer);
//需要一个costmap，传递给planner初始化
global_planner::PlannerWithCostmap pppp("planner", &lcr);
（3）这里开启了两个线程，第一个是提供plan service，一旦有请求，就调用bool success = makePlan(req.start, req.goal, path);第二个是去订阅goal，拿到goal之后，就调用makePlan(start, goal, path) 。
定义了PlannerWithCostmap类，PlannerWithCostmap(string name, Costmap2DROS cmap)是其构造函数。

PlannerWithCostmap::PlannerWithCostmap(string name, Costmap2DROS* cmap) :
        GlobalPlanner(name, cmap->getCostmap(), cmap->getGlobalFrameID()) {
    ros::NodeHandle private_nh("~");
    cmap_ = cmap;
    make_plan_service_ = private_nh.advertiseService("make_plan", &PlannerWithCostmap::makePlanService, this);
    pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &PlannerWithCostmap::poseCallback, this);
}

2.planner_core.h
（1）
geometry_msgs/PoseStamped.h
geometry_msgs/Point.h
nav_msgs/Path.h
nav_msgs/GetPlan.h
dynamic_reconfigure/server.h
该头文件包含的这些头文件都得有。

其次，看该头文件包含的如下几个头文件：
（2）nav_core/base_global_planner.h：

namespace nav_core {

   // @class BaseGlobalPlanner
   // @**brief Provides an interface for global planners used in navigation. All global planners written as plugins for the navigation stack must adhere to this interface.**
  class BaseGlobalPlanner{

    public:
       //@param plan The plan... filled by the planner
       // @return True if a valid plan was found, false otherwise
      virtual bool makePlan(const geometry_msgs::PoseStamped& start,const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) = 0;//初始化未找到规划的路径


       // @param cost The plans calculated cost
      virtual bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan,double& cost)
      {
        cost = 0;
        return makePlan(start, goal, plan);
      }

该函数有 4 个输入，其中 start 、 goal 、 plan 这三个参数都属于 引用传入函数参数 （这是 & 的作用），当 makePlan 中这些变量的值改变了，那么调用该函数时对应传入的变量值也跟着改变了：
const geometry_msgs::PoseStamped& start ：起始点的位姿
const geometry_msgs::PoseStamped& goal ：目标点的位姿
double tolerance ：路径规划器规划出的终点容错距离
std::vector& plan ：初始时会清除路径 plan.clear(); ，执行完 makePlan 后会规划出路径（一系列 pose ）
该函数的输出值为 bool 类型，如能规划出非空路径则返回 True ，即 return !plan.empty(); 。

这里主要用到了 geometry_msgs/PoseStamped 消息类型：
root@eln-pc:~/catkin_ws# rosmsg show geometry_msgs/PoseStamped
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x //直角坐标系utm，属于ROS中的world，东为X正轴，北为Y正轴
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w

补充的知识点： 欧拉角、四元数
欧拉角rpy：欧拉角是yaw-pitch-roll次序.yaw偏航指水平方向的机头指向，它绕z轴旋转，z轴指向天/地，北偏东多少度。Pitch俯仰指与水平方向的夹角，绕y轴旋转，y轴向东。Roll指飞机的翻滚，绕x轴旋转，x轴向北。
给定一个欧拉旋转(X, Y, Z)，则对应的四元数为：
x = sin(Y/2)sin(Z/2)cos(X/2)+cos(Y/2)cos(Z/2)sin(X/2)
y = sin(Y/2)cos(Z/2)cos(X/2)+cos(Y/2)sin(Z/2)sin(X/2)
z = cos(Y/2)sin(Z/2)cos(X/2)-sin(Y/2)cos(Z/2)sin(X/2)
w = cos(Y/2)cos(Z/2)cos(X/2)-sin(Y/2)sin(Z/2)sin(X/2)
q = ((x, y, z), w)

功能函数 makePlan 会根据几个关键配置参数决定所要调用的算法：
use_quadratic (bool, default: true) ：如果设置为 true ，使用 二次函数近似势函数（ quadratic_calculator.cpp ） 。否则使用 更加简单的计算方式（ potential_calculator.h ）
use_dijkstra (bool, default: true) ：如果设置为 true ，则使用 dijkstra 算法（ dijkstra.cpp ） ，否则使用 A-Star 算法（ astar.cpp ）
use_grid_path (bool, default: false) ：如果设置为 true ，则 创建一条沿着网格边界的路径（ grid_path.cpp ） ，否则使用 梯度下降法（ gradient_path.cpp ）

功能函数 makePlan流程：
https://www.freesion.com/images/593/ea5b25a43628a12674324383d2f86349.png

（3）global_planner/potential_calculator.h
定义了 PotentialCalculator 类，其中有一个成员函数：

virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential=-1){
    if(prev_potential < 0){
        // get min of neighbors,就关注左右上下四个邻居
        float min_h = std::min( potential[n - 1], potential[n + 1] ),
              min_v = std::min( potential[n - nx_], potential[n + nx_]);
        prev_potential = std::min(min_h, min_v);
    }

    return prev_potential + cost;
}

我们可以选择属于PotentialCalculator类的这个函数定义的方法计算Potential

（4）Quadratic_Calculator.h和Quadratic_Calculator.cpp
定义了一个类QuadraticCalculator ，继承了PotentialCalculator 类。
我们可以选择这个头文件对应的Quadratic_Calculator.cpp中的float QuadraticCalculator::calculatePotential(float* potential, unsigned char cost, int n, float prev_potential) 方法计算Potential。

float QuadraticCalculator::calculatePotential(float* potential, unsigned char cost, int n, float prev_potential) {
    // get neighbors
    float u, d, l, r;
    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[n], l, r, u, d);
    //  ROS_INFO("[Update] cost: %d\n", costs[n]);

    // 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;
    }
}

（5）global_planner/expander.h
定义了expander类,有两个类继承它：class DijkstraExpansion : public Expander和class AStarExpansion : public Expander 。
以下的实际使用时planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);该函数把终点周围的点更新了一下.

       void clearEndpoint(unsigned char* costs, float* potential, int gx, int gy, int s){
            int startCell = toIndex(gx, gy);
            for(int i=-s;i<=s;i++){
            for(int j=-s;j<=s;j++){
                int n = startCell+i+nx_*j;
                if(potential[n]<POT_HIGH)
                    continue;
                float c = costs[n]+neutral_cost_;
                float pot = p_calc_->calculatePotential(potential, c, n); //PotentialCalculator* p_calc_;
                potential[n] = pot;
            }
            }
        }

关于此函数，在planner_core.cpp中被调用：

if(!old_navfn_behavior_)
        planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);

（6）global_planner/traceback.h
定义了类Traceback，其中成员函数virtual bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector >& path) = 0;//追溯形成最终路径
class GridPath : public Traceback
class GradientPath : public Traceback

（7）global_planner/orientation_filter.h和orientation_filter.cpp
在生成path之后，对其中的每一个路径点生成orientation.
enum OrientationMode { NONE, FORWARD, INTERPOLATE, FORWARDTHENINTERPOLATE, BACKWARD, LEFTWARD, RIGHTWARD };
涉及到GlobalPlanner.cfg中的配置，下面的参数设置是选用哪个方向的为坐标轴，即朝向生成使用的坐标轴。

//x正轴是正前方，y正轴是左
    orientation_enum = gen.enum([
        gen.const("None", int_t, 0, "No orientations added except goal orientation"),
        gen.const("Forward", int_t, 1,
                  "Positive x axis points along path, except for the goal orientation"),
        gen.const("Interpolate", int_t, 2, "Orientations are a linear blend of start and goal pose"),
        gen.const("ForwardThenInterpolate",
                  int_t, 3, "Forward orientation until last straightaway, then a linear blend until the goal pose"),
        gen.const("Backward", int_t, 4,
                  "Negative x axis points along the path, except for the goal orientation"),
        gen.const("Leftward", int_t, 5,
                  "Positive y axis points along the path, except for the goal orientation"),
        gen.const("Rightward", int_t, 6,
                  "Negative y axis points along the path, except for the goal orientation"),
    ], "How to set the orientation of each point")

    gen.add("orientation_mode", int_t, 0, "How to set the orientation of each point", 1, 0, 6,
            edit_method=orientation_enum)#括号中的1可能需要改成0
    gen.add("orientation_window_size", int_t, 0, "What window to use to determine the orientation based on the position derivative specified by the orientation mode", 1, 1, 255)？？？？？

用OrientationFilter类的成员函数void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,std::vector& path)，该函数中调用了void setAngleBasedOnPositionDerivative(std::vector& path, int index)和
void interpolate(std::vector& path, int start_index, int end_index)两个成员函数，而这两个函数又都调用了void set_angle(geometry_msgs::PoseStamped* pose, double angle)函数：

void set_angle(geometry_msgs::PoseStamped* pose, double angle)
{
  tf2::Quaternion q;
  q.setRPY(0, 0, angle);
  tf2::convert(q, pose->pose.orientation);
}

终点和起点的朝向怎么生成，要看orientation_filter.cpp，最终结合tf2::convert(q, pose->pose.orientation)生成。

3.planner_core.cpp

3.1包含的头文件和相关的cpp:
（1）global_planner/dijkstra.h和dijkstra.cpp
定义了class DijkstraExpansion : public Expander

涉及的知识点：
最短路径—Dijkstra算法:https://www.cnblogs.com/biyeymyhjob/archive/2012/07/31/2615833.html;
dijkstra算法中遇到两个路径距离相同怎么办?https://zhidao.baidu.com/question/1796072563103347027.html；

C++中 delete 和 delete[] 的区别：https://www.cnblogs.com/simplepaul/p/6861210.html

void *memset(void *s, int ch, size_t n);
函数解释：将s中当前位置后面的n个字节 （typedef unsigned int size_t ）用 ch 替换并返回 s 。
memset：作用是在一段内存块中填充某个给定的值，它是对较大的结构体或数组进行清零操作的一种最快方法 。
memset()函数原型是extern void *memset(void *buffer, int c, int count) buffer：为指针或是数组,c：是赋给buffer的值,count：是buffer的长度.

注意：start_x，start_y，分别表示起始栅格的列号，行号；
end_x, end_y，分别表示终止目的地栅格的列号，行号
需要我们生成

**nx/xs是地图宽度，ny/ys是地图高度，需要订阅map消息**

（2）global_planner/astar.h和astar.cpp
定义了class AStarExpansion : public Expander
queue是open_list
pop_heap将堆顶(所给范围的最前面)元素移动到所给范围的最后，并且将新的最大值置于所给范围的最前面
push_heap当已建堆的容器范围内有新的元素插入末尾后，应当调用push_heap将该元素插入堆中。

参数cycles： The maximum number of iterations to run for，其值为nx*ny*2,见planner_core.cpp中的代码bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);//nx * ny * 2为cycles参数的值传入。即最长的路径栅格数不超过全部格子数量的2倍，即来回，单程为全部格子。

（3）global_planner/grid_path.h和grid_path.cpp
class GridPath : public Traceback
追溯，获得路径getpath

(4)global_planner/gradient_path.h和gradient_path.cpp
class GradientPath : public Traceback
我们不用这种方法获得路径，用GridPath

        // 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
        //
 bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path);

3.2结合其头文件的具体代码解析
（1）//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

依附于nav_core::BaseGlobalPlanner的另外两个插件是：
PLUGINLIB_EXPORT_CLASS(navfn::NavfnROS, nav_core::BaseGlobalPlanner)
PLUGINLIB_EXPORT_CLASS(carrot_planner::CarrotPlanner, nav_core::BaseGlobalPlanner)
（2） void GlobalPlanner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value)
////调用以下函数将costmap的四个边的全部cell都设置为LETHAL_OBSTACLE（对应value参数）或其它，我们得改这个，只把出发那个设为LETHAL_OBSTACLE？？

  void GlobalPlanner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) {
        unsigned char* pc = costarr;
        for (int i = 0; i < nx; i++)
            *pc++ = value;//地图下边界
        pc = costarr + (ny - 1) * nx;
        for (int i = 0; i < nx; i++)
            *pc++ = value;//地图上边界
        pc = costarr;
        for (int i = 0; i < ny; i++, pc += nx)
            *pc = value;//地图左边界
        pc = costarr + nx - 1;
        for (int i = 0; i < ny; i++, pc += nx)
            *pc = value;//地图右边界
    }

在该函数中将costmap的四个边的全部cell都设置为了LETHAL_OBSTACLE：
if(outline_map_)
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);

（3）GlobalPlanner();
* @brief Default constructor for the PlannerCore object
（4）GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
这里需要订阅到的地图消息，并传入The name of this planner。
* @brief Constructor for the PlannerCore object
* @param name The name of this planner
* @param costmap A pointer to the costmap to use
* @param frame_id Frame of the costmap
调用了initialize(name, costmap, frame_id)函数，具体定义为
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) ，该函数也被void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 函数调用
（5）void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id)
**需要进行设置修改的：

private_nh.param("use_quadratic", use_quadratic, true);
private_nh.param("use_dijkstra", use_dijkstra, true);
private_nh.param("use_grid_path", use_grid_path, false);**

//？？？怎么设

private_nh.param("allow_unknown", allow_unknown_, true);
planner_->setHasUnknown(allow_unknown_);
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);
private_nh.param("publish_scale", publish_scale_, 100);//将potential范围限制在0~100，在void GlobalPlanner::publishPotential(float* potential)中使用：grid.data[i] = potential_array_[i] * publish_scale_ / max，max是最大的栅格potential
private_nh.param("outline_map", outline_map_, true); 

**发布的：

plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);

xiaoyao@xiaoyao-VirtualBox:~$ rosmsg show nav_msgs/Path

std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/PoseStamped[] poses
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w

potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);

xiaoyao@xiaoyao-VirtualBox:~$ rosmsg show nav_msgs/OccupancyGrid

std_msgs/Header header
uint32 seq
time stamp
string frame_id
nav_msgs/MapMetaData info
time map_load_time
float32 resolution
uint32 width
uint32 height
geometry_msgs/Pose origin # The origin of the map [m, m, rad]. This is the real-world pose of the cell (0,0) in the map.
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
int8[] data # The map data, in row-major order, starting with (0,0). Occupancy以行的顺序存储probabilities are in the range [0,100]. Unknown is -1.

（6）void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig& config, uint32_t level)
该函数读取配置文件中的设置。

（7）void GlobalPlanner::clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my)
与机器人占据的栅格有关,即如何设置其cost,设置为FREE_SPACE
//set the associated costs in the cost map to be free
costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);
此setCost函数在cost_map2d.cpp中：
void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
{
costmap_[getIndex(mx, my)] = cost;
}
(8)bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) {
makePlan(req.start, req.goal, resp.plan.poses);
将接收的消息作为参数传入后，开始makeplan

(9)convert_offset_???//??///???
注意下面两行！！
// double wx = start.pose.position.x;位置坐标
//double wy = start.pose.position.y;
//mx，my是地图坐标，行列号的浮点型，相对于地图原点，原点在左下角
//convert_offset_是转换偏移

void GlobalPlanner::mapToWorld(double mx, double my, double& wx, double& wy) {
    wx = costmap_->getOriginX() + (mx+convert_offset_) * costmap_->getResolution();
    wy = costmap_->getOriginY() + (my+convert_offset_) * costmap_->getResolution();
}


bool GlobalPlanner::worldToMap(double wx, double wy, double& mx, double& my) {
    double origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY();
    double resolution = costmap_->getResolution();

    if (wx < origin_x || wy < origin_y)
        return false;

    mx = (wx - origin_x) / resolution - convert_offset_;
    my = (wy - origin_y) / resolution - convert_offset_;

    if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY())
        return true;

    return false;
}

  //convert_offset_是转换偏移，怎么设置？？
   答：在void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) 函数中：

private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
    if(!old_navfn_behavior_)
        convert_offset_ = 0.5;
    else
        convert_offset_ = 0.0;

生成mx,my时减去convert_offset_是为了在toIndex(int x, int y)传入强制转变为int时行列号不越界。

(10)bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,double tolerance, std::vector& plan)
* @param tolerance The tolerance on the goal point for the planner
在3.2（5）中表明了设置的地方。
//clear the plan, just in case需要改
plan.clear();

//global_frame全局坐标系
if (goal.header.frame_id != global_frame)

//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;
}

unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;//map坐标下的起点终点的列号行号
double start_x, start_y, goal_x, goal_y;//直角坐标系位置坐标

//clear the starting cell within the costmap because we know it can't be an obstacle
clearRobotCell(start, start_x_i, start_y_i);

void publishPlan(const std::vector& path);
* @brief Publish a path for visualization purposes

编程的时候注意按照以下来定义：

int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();//地图宽和高
double resolution = costmap_->getResolution();
nav_msgs::OccupancyGrid grid;
// Publish Whole Grid
grid.header.frame_id = frame_id_;
grid.header.stamp = ros::Time::now();
grid.info.resolution = resolution;

grid.info.width = nx;
grid.info.height = ny;

double wx, wy;
costmap_->mapToWorld(0, 0, wx, wy);
grid.info.origin.position.x = wx - resolution / 2;
grid.info.origin.position.y = wy - resolution / 2;
grid.info.origin.position.z = 0.0;
grid.info.origin.orientation.w = 1.0;

grid.data.resize(nx * ny);

为什么要减去resolution / 2？是因为：
void GlobalPlanner::mapToWorld(double mx, double my, double& wx, double& wy) {
wx = costmap_->getOriginX() + (mx+convert_offset_) * costmap_->getResolution();
wy = costmap_->getOriginY() + (my+convert_offset_) * costmap_->getResolution();

}