移动机器人gazebo仿真(5)—规划算法Ａ*_ros gazebo global-planning

参考博客：

MoveBace.cpp阅读笔记

贪心算法的一个典型案例——Dijkstra算法:     浅谈路径规划算法之Dijkstra算法

A*:      A*寻路算法   

关于寻路算法的一些思考（1）——A*算法介绍

ROS的全局规划代码介绍（A*）

重要：ROS Navigation的global_planner类继承关系与实现算法

导航实际流程为：

进行全局路径规划，在进行局部路径规划，然后发布速度

全局路径规划在makePlan函数中，该函数中调用了 planner_的 makePlan和 empty接口。
planner_为继承于BaseGlobalPlanner的实例，由pluginlib通过具体类的名字进行装载。
之后，调用tc_的setPlan接口，对局部路径规划器进行全局路径设置，然后，调用tc_的isReached接口进行判断，然后调用tc_的computeVelocityCommands接口，进行速度计算，然后进行速度下发。

tc_为继承于BaseLocalPlanner的实例，也是由pluginlinb通过具体类的名字进行装载。

下面带来两个问题，planner_怎么进行路径规划，以及tc_如何计算速度。
planner_在初始化时候，被塞入了planner_costmap_ros_
tc_在初始化时，被塞入了controller_costmap_ros_



在global planner的包中，注册了插件：global planner::GlobalPlanner

代码阅读：

global_planne

1、plan_node.cpp  

plan_node.cpp是全局规划代码的入口(代码注释都是自己理解然后添加，也许会有错误)

#include <global_planner/planner_core.h>
#include <navfn/MakeNavPlan.h>
#include <boost/shared_ptr.hpp>
#include <costmap_2d/costmap_2d_ros.h>
 
namespace cm = costmap_2d;
namespace rm = geometry_msgs;
 
using std::vector;
using rm::PoseStamped;
using std::string;
using cm::Costmap2D;
using cm::Costmap2DROS;
 
namespace global_planner {
 
class PlannerWithCostmap : public GlobalPlanner {
    public:
        PlannerWithCostmap(string name, Costmap2DROS* cmap);
        bool makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp);
 
    private:
        void poseCallback(const rm::PoseStamped::ConstPtr& goal);
        Costmap2DROS* cmap_;
        ros::ServiceServer make_plan_service_;
        ros::Subscriber pose_sub_;
};
//Publish a path for visualization purposes
bool PlannerWithCostmap::makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp) {
    vector<PoseStamped> path;
 
    req.start.header.frame_id = "/map";
    req.goal.header.frame_id = "/map";
    bool success = makePlan(req.start, req.goal, path);
    resp.plan_found = success;
    if (success) {
        resp.path = path;
    }
 
    return true;
}
 
void PlannerWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
    tf::Stamped<tf::Pose> global_pose;
    cmap_->getRobotPose(global_pose);//获取机器人起始位姿
    vector<PoseStamped> path;
    rm::PoseStamped start;
    start.header.stamp = global_pose.stamp_;
    start.header.frame_id = global_pose.frame_id_;
    start.pose.position.x = global_pose.getOrigin().x();
    start.pose.position.y = global_pose.getOrigin().y();
    start.pose.position.z = global_pose.getOrigin().z();
    start.pose.orientation.x = global_pose.getRotation().x();
    start.pose.orientation.y = global_pose.getRotation().y();
    start.pose.orientation.z = global_pose.getRotation().z();
    start.pose.orientation.w = global_pose.getRotation().w();
    makePlan(start, *goal, path);//路径规划
}
 
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.subsc