global_planner解析（5）

前面介绍了我们的搜索算法，下面来介绍我们的主要的插件实现函数 planner_core.cpp

planner_core.h

#ifndef _PLANNERCORE_H
#define _PLANNERCORE_H
#define POT_HIGH 10e10

#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <nav_msgs/Path.h>
#include <tf/transform_datatypes.h>
#include <vector>
#include <nav_core/base_global_planner.h>
#include <nav_msgs/GetPlan.h>
#include <dynamic_reconfigure/server.h>
//潜力计算器
#include <nav_planner/potential.h>
//扩展器
#include <nav_planner/expander.h>
//追溯
#include <nav_planner/traceback.h>
//给路径加方向
#include <nav_planner/orientation_filter.h>

namespace nav_planner
{

class Expander;
class GridPath;

//
//global_planner规划器提供ROS包装器，该包装器在成本图上运行快速的内插导航功能。

class GlobalPlanner : public nav_core::BaseGlobalPlanner{

public:
    //构造函数
    GlobalPlanner();
    GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
    //析构函数
    ~GlobalPlanner();
    //初始化函数
    void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
    void initialize(std::string name, costmap_2d::Costmap2D* costmap, std:: string frame_id);
    //makeplan 函数,服务回调就是调用这个;
    bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                    std::vector<geometry_msgs::PoseStamped>& plan);
    bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance,
                    std::vector<geometry_msgs::PoseStamped>& plan);
    
    //从给定的世界点开始计算地图的完整功能
    //计算潜力
    bool computePotentialCalculator(const geometry_msgs::Point& world_point);
    //在已经计算出起点的可能性之后，将计划计算为目标（注意：您应该首先调用computePotentialCalculator）
    //从潜力中获取计划
    //估计是从潜力计算图中获取路径
    bool getPlanFromPotentialCalculator(double start_x, double start_y, double end_x, double end_y,
                                const geometry_msgs::PoseStamped& goal
                                std::vector<geometry_msgs::PoseStamped>& paln);
    //获取世界上给定点的潜力或导航成本（注意：您应该先调用“computePotentialCalculator-计算潜力”）
    //应该是获取改点的潜力
    double getPointPotentialCalculator(const geometry_msgs::Point& world_point);

    //在世界上的给定点检查有效的潜在值（注意：您应该先调用computePotentialCalculator）
    bool validPointPotentialCalculator(const geometry_msgs::Point& world_point);
    //tolerance-公差
    bool validPointPotentialCalculator(const geometry_msgs::Point& world_point, double tolerance);

    //发布路径
    void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
    //makeplan回调函数
    void makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& res);

protected:
    //将当前代价地图存储在这里 由makeplan调用
    costmap_2d::Costmap2D* costmap_;
    std::string frame_id;
    ros::Publisher plan_pub_;
    bool initialized_,allow_unknown_;

private:
    //ros的一些发布者等
    //发布启发过的地图
    ros::Publisher potential_pub_;
    //makeplan
    ros::ServiceServer make_plan_srv_;

    //将map坐标转化为世界坐标
    void mapToWorld(double mx, double my, double& wx, double& wy);
    //将世界坐标转化为map坐标
    bool worldToMap(double wx, double wy, double& mx, double& xy);
    //应该是清空栅格的代价值,或者潜力值
    void clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my);
    //发布扩展完成后的点
    void publishPotentialCalculator(float* potential);

    double planner_window_x_, planner_window_y_, default_tolerance;
    std::string tf_prefix_;
    boost::mutex mutex_;

    PotentialCalculator* p_calc_;
    Expander* planner_;
    Traceback* path_marker_;
    OrientationFilter* orientation_filter_;

    //启发过的地图相关数据
    bool publish_potential_;
    int publisher_scale_;

    void outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value);
    unsigned char* cost_array_;
    float* potential_array_;
    unsigned int start_x_, start_y_, end_x_, end_y_;

    //是否用老版的navfn的计算方式
    bool old_navfn_behavior_;
    float convert_offset_;
    //动态调参的相关,
    // dynamic_reconfigure::Server<lnglobal_planner::GlobalPlannerConfig> *dsrv_;
    // void reconfigureCB(lnglobal_planner::GlobalPlannerConfig &config, uint32_t level);
};
}
#endif

里面定义了主要的函数

bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance,
                std::vector<geometry_msgs::PoseStamped>& plan);

是makeplan服务被调用后的回调函数，该函数是导航过程中的主要运行函数，里面包括路径搜索，路径回溯，添加方向等，发布扩展列表话题“potential”，发布可视化路径话题“plan”。
下面来贴出planner_core.cpp的代码

#include <nav_planner/planner_core.h>
#include <pluginlib/class_list_macros.h>
#include <tf/transform_listener.h>
#include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_2d.h>

#include <nav_planner/dijkstra.h>
#include <nav_planner/expander.h>
#include <nav_planner/gradient_path.h>
#include <nav_planner/orientation_filter.h>
#include <nav_planner/planner_core.h>
#include <nav_planner/potential.h>
#include <nav_planner/traceback.h>
/*
map坐标系，单位为m，及真实的tf或amcl获得的未知
world坐标系，即为栅格坐标系

*/
PLUGINLIB_EXPORT_CLASS(nav_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)
namespace nav_planner
{
//将地图外轮廓设为障碍
void GlobalPlanner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value)
{
    unsigned char* pc = costarr;
    //第一行
    for (size_t i = 0; i < nx; i++)
        *pc++ = value;
    //最后一行
    pc = costarr + (ny - i) * nx;
    for (size_t i = 0; i < nx; i++)
        *pc++ = value;
    //第一列
    pc = costarr;
    for (size_t i = 0; i < ny; i++, pc += nx)
        *pc = value;
    //最后一列
    pc = costarr + nx - 1;
    for (size_t i = 0; i < ny; i++, pc += nx)
        *pc = value; 
}
//构造函数
GlobalPlanner::GlobalPlanner():costmap_(NULL),initialized_(false),allow_unknown_(true)
{}
//构造函数1
GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id)
                            : costmap_(NULL), initialized_(false), allow_unknown_(true)
{
                            //调用初始化函数
                            initialize(name, costmap, framer_id);
}
//析构函数
GlobalPlanner::~GlobalPlanner()
{
    if(p_calc_)
        delete p_calc_;
    if(planner_)
        delete planner_;
    if(path_marker_)
        delete path_marker_;
    if(dsrv_)
        delete dsrv_;
}
//初始化函数1
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
{
    //调用初始化函数2
    //将 Costmap2DROS 转化位 costmap 提取其 frameid
    initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
}
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id)
{
    //判断是否已经初始化完毕
    if (!initialized_)
    {
        //获取参数 节点句柄
        ros::NodeHandle private_nh("~/"+name);
        costmap_ = costmap;
        frame_id_ = frame_id;

        //获得代价地图的长和宽
        unsigned int cx = costmap->getSizeInCellX();
        unsigned int cy = costmap->getSizeInCellY();

        //是否使用navfn的方式  no
        convert_offset_ = 0.5;

        //是否使用二次近似函数    no
        p_calc_ = new PotentialCalculator(cx, cy);

        //使用地杰斯特拉算法 yes
        DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
            //是否使用老旧的计算方式 no
            de->setPreciseStart(true);
        planner_ = de;

        //延边界创建路径 yes  使用梯度下降 no
        path_marker_ = new GridPath(p_calc_);

        //给路径加方向
        orientation_filter_ = new orientationFilter();

        //发布消息
        plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan",1);
        potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential",1);

        //允许规划 未知（unknow）中的全局路径 no
        planner_->setHasUnknown(false);

        //设置允许目标点误差范围
        default_tolerance_ = 0.0；

        //获取tf 的prefix(字首)
        ros::NodeHandle prefix_nh;
        tf_prefix = tf:: getPrefixParam(prefix_nh);

        //发布make plan服务
        make_plan_src_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

        //动态调参 略去

        initialized_ = true;
    }else{
        //这个已经被初始化，您不能调用两次
        ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
    }
}
//将某个costmap设置为free
void GlobalPlanner::clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my)
{
    if (initialized_){
        costmap->setCost(mx, my, costmap_2d::FREE_SPACE);
    }else{
        ROS_ERROR("this planner has not been initialized yet");
    }
}
//makeplan 回调函数
bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& res)
{
    //调用makeplan函数
    makePlan(req.start, req.goal, res.plan.poses);

    //赋予时间及frame id
    res.plan.header.stamp = ros::Time::now();
    res.plan.header.frame_id = frame_id_;

    return true;
}
//将map上的坐标系转化为world上的坐标系
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_) * cosrmap_->getResolution();
}
//将world上的坐标转化为map上的坐标
bool GlobalPlanner::worldToMap(double wx, double wy, double& mx, double& my)
{
    //获得地图的长宽（栅格地图）
    double origin_x = costmap_->getOriginX();
    double origin_y = costmap_->getOriginY();

    double resolution = costmap_->getResolution();

    //如果超出范围
    if (wx < origin_x || wy < origin_y)
        return false;
    // x = (当前点 - 长度)/分辨率  - 转换偏移
    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;
}
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start,
                             const geometry_msgs::PoseStamped& goal,
                             std::vector<geometry_msgs::PoseStamped>& plan)
{
    return makePlan(start, goal, default_tolerance_, plan);
}
//主要函数：路径规划函数
//start 起始点的位资
//goal 目标点的位资
//telerance 目标点的允许误差
//plan 路径
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start,
                             const geometry_msgs::PoseStamped& goal,
                             double tolerance,
                             std::vector<geometry_msgs::PoseStamped>& plan)
{
    //boost区域锁，用来对线程同步进行资源保护的
    boost::mutex::scoped_lock lock(mutex_);

    if (!initialized_)
    {
        ROS_ERROR("Uninitialized")；
        return false;
    }
    //清空路径vector
    plan.clear();

    ros::NodeHandle n;
    std::string global_frame = 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;
    }
    
    //获取起始点的世界坐标系坐标
    double wx = start.pose.position.x;
    double wy = start.pose.position.y;

    unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
    double start_x, start_y, goal_x, goal_y;
    
    //将起始点的世界坐标系坐标转化为 map坐标系坐标
    if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i))
    {
        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;
    }
    //判断一下是否还用老旧的navfn计算方式      no
    worldToMap(wx, wy, start_x, start_y);

    //获取目标点world坐标
    wx = goal.pose.position.x;
    wy = goal.pose.position.y;

    if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i))
    {
        ROS_WARN_THROTTLE(1.0,
                "The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
        return false;
    }
    tf::Stamped<tf::Pose> start_pose;
    tf::poseStampedMsgToTF(start, start_pose);
    //清空起始点的代价值，因为不知道其是否为障碍
    clearRobotCell(start_pose, start_x_i, start_y_i);
    //获取代价地图的长宽
    int nx = costmap_->getSizeInCellsX();
    int ny = costmap_->getSizeInCellsY();
    //分配空间，确保使用基础数组的大小
    //搜索算法
    p_calc_->setSize(nx, ny);
    //搜索算法设置长宽
    planner_->setSize(nx, ny);
    //路径回溯
    path_marker_->setSize(nx, ny);
    //扩展的点
    potential_array_ = new float[nx * ny];
    //路径搜索
    bool found_legal = planner_->calcuatePotentialCalculators(costmap_->getCharMap(),
                                                    start_x,
                                                    start_y,
                                                    goal_x,
                                                    goal_y,
                                                    nx * ny * 2,
                                                    potential_array_);
    //判断是否用老式的计算方式  no
    planner_->clearEndpoint(costmap_->getCharMap(),
                            potential_array_,
                            goal_x_i,
                            goal_y_i);
    //是否发布可行性点到话题   no
    //publishPotentialCalculator(potential_array_);
    //判断搜索算法是否找到了目标点
    if (found_legal)
    {
        //从potential可行性点里面找出来路径
        if(getPlanFromPotentialCalculator(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);
        }else{
            ROS_ERROR("Failed to get a plan from potential");
        }
    }else{
        ROS_ERROR("failed to get a plan");
    }
    //给路径加方向
    orientation_filter_->processPath(start, plan);
    //发布可视化路径   no
    //publishPlan(plan);
    
    delete potential_array_;
    return !plan.empty();
}
//提取路径
bool GlobalPlanner::getPlanFromPotentialCalculator(double start_x, double start_y,
                                        double goal_x, double goal_y,
                                        const geometry_msgs::PoseStamped& goal,
                                        std::vector<geometry_msgs::PoseStamped>& plan)
{
    if (!initialized_)
    {
        ROS_ERROR("This planner has not been initialized_.");
        return false;
    }
    std::string global_frame = frame_id_;

    //清空路径
    plan.clear();
    std::vector<std::pair<float,float>> path;

    //调用getPath
    if (!path_marker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path))
    {
        ROS_ERROR("NO PATH!");
        return false;
    }
    //将path赋值给ros标准消息
    ros::Time plan_time = ros::Time::now();
    for (int i = path.size() - 1; i >= 0; i--)
    {
        std::pair<float, float> point = path[i];
        double world_x, world_y;
        //将提取的路径转换为世界坐标系
        mapToWorld(point.first, point.second, 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);
    }
    return !plan.empty();
}
//发布可视化路径
//发布可行性点
}