ROS-Navigation学习（4）—global_palnner学习(2)

plan_node.cpp

planner的入口部分如下，主要部分是class::plannerWithCostmap的构造函数

    int main(int argc, char** argv) {
    ros::init(argc, argv, "global_planner");

    tf::TransformListener tf(ros::Duration(10));

    costmap_2d::Costmap2DROS lcr("costmap", tf);

    global_planner::PlannerWithCostmap pppp("planner", &lcr);

    ros::spin();
    return 0;
}

其中构造函数为：

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

在这里面，创建了一个make_plan服务端以及订阅了goal的话题。服务端的函数是makePlanService,话题的回调函数是poseCallback.
服务端的函数如下:

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

一旦有客户端来了请求，就调用 makePlan(req.start, req.goal, path);最后得到plan.并且返回true。如果得不到plan，则 vector <PoseStamped> path还是空.不会有有效路径。
相似的，订阅goal话题后，在得到goal后，在回调函数poseCallback中调用makePlan,得到规划的路径plan.其中回调函数如下：

void PlannerWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
    tf::Stamped<tf::Pose> global_pose;
    cmap_->getRobotPose(global_pose);
    vector<PoseStamped> path;
    //在调用makePlan之前，做一些预处理，得到机器人当前位置的位姿作为起始点。
    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);
}

Plan_node文件就是一个入口函数，主要的是调用makeplan。makeplan函数在planner_core文件中。

planner_core

在这个文件中，主体函数是初始化函数initialize函数和makeplan函数。其他的clearrobotcell(), getplanfrompotential(); publishpotential();等函数被这两个函数调用。
首先需要进行初始化。主要是得到一些参数，以及一些参数的设置。如 old_navfn_behavior, use_quadratic, use_dijkstra, use_grid_path。如下图的选择：

其中的old_navfn_behavior的选择决定了covert_offset.从而决定了由世界到栅格地图坐标的转换。如果old_navfn_behavior为true，则covert_offset就是0.0。否则就是0.5。

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->getSizeInCellsX(), cy = costmap->getSizeInCellsY();

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

        bool use_quadratic;
        private_nh.param("use_quadratic", use_quadratic, true);
        if (use_quadratic)
            p_calc_ = new QuadraticCalculator(cx, cy);
        else
            p_calc_ = new PotentialCalculator(cx, cy);

        bool use_dijkstra;
        private_nh.param("use_dijkstra", use_dijkstra, true);
        if (use_dijkstra)
        {
            DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
            if(!old_navfn_behavior_)
                de->setPreciseStart(true);
            planner_ = de;
        }
        else
            planner_ = new AStarExpansion(p_calc_, cx, cy);

        bool use_grid_path;
        private_nh.param("use_grid_path", use_grid_path, false);
        if (use_grid_path)
            path_maker_ = new GridPath(p_calc_);
        else
            path_maker_ = new GradientPath(p_calc_);

初始化函数中的其他部分就是设置一些参数，然后在初始化成功后，将initlialized_设置为true.。代码注释如下：

	    //定义了一个滤波器
	    orientation_filter_ = new OrientationFilter();
		//设置两个发布器，对应话题是plan和potential.
        plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
        potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);

		//获取参数 "****" 的值到 *** 。如果get不到指定的“”，则将后面的赋值给***
        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);

        //get the tf prefix
        ros::NodeHandle prefix_nh;
        tf_prefix_ = tf::getPrefixParam(prefix_nh);
		//设置一个服务端，调用函数是makePlanService函数，此函数调用makeplan
        make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);
		//加载动态参数，调用了函数reconfigureCB.
        dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
        dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
                &GlobalPlanner::reconfigureCB, this, _1, _2);
        dsrv_->setCallback(cb);
		//设置为true，以防已经初始化后再次进行初始化。
        initialized_ = true;

其中加载的动态参数包括：（这些参数意义还不确定）

void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig& config, uint32_t level) {
    planner_->setLethalCost(config.lethal_cost);
    path_maker_->setLethalCost(config.lethal_cost);
    planner_->setNeutralCost(config.neutral_cost);
    planner_->setFactor(config.cost_factor);
    publish_potential_ = config.publish_potential;
    orientation_filter_->setMode(config.orientation_mode);
    orientation_filter_->setWindowSize(config.orientation_window_size);
}

服务端的调用函数是makeplanaService,在这个函数中调用makeplan进行路径的规划。在此makeplan只说明一下代码，至于调用的A* 和 Dijkstra算法，use_grid等算法在其他博文介绍。
除去算法本身，makeplan就是做了一些数据的处理以及参数检测等。
先是判断初始化是否成功

if (!initialized_) {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return false;
    }

还有就是判断起始点和目标点的坐标系是否是在global_frame下。如果不是就返回false。

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

然后将起始点和目标点转换到cell坐标。这里是否是old_navfn_behavior会决定输入参数是int 还是 double。而之前是否加0.5的conver_offset也与此有关，猜想是因为int 会向下取整。因此在计算中，如果得到旧的计算是6.4，但是由于Int，所以还是6，而当前的navfn则是double型，则就是6.4,因此最后需要再减去0.5。以致于这两个更加接近。（自己猜想）

 	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;

    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;
    }
    if(old_navfn_behavior_){
        start_x = start_x_i;
        start_y = start_y_i;
    }else{
        worldToMap(wx, wy, start_x, start_y);
    }

目标点同理。
这里涉及的worldtomap函数就是将时间坐标希转换到栅格地图的坐标。
函数如下

 	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;

原理如下图（比较简单）并且由此可以得到maptworld函数。

其中resolution即分辨率，originx 相当于原点的世界坐标。
然后将开始点设置为FREE_SPACE.代价地图相关知识参考：
Costmap2D类解析
Costmao2D包的一些理解

	tf::Stamped<tf::Pose> 	start_pose;
    tf::poseStampedMsgToTF(start, start_pose);
    clearRobotCell(start_pose, start_x_i, start_y_i);

接下来分配和costmap一样大小的空间

	int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();

    //make sure to resize the underlying array that Navfn uses
    p_calc_->setSize(nx, ny);
    planner_->setSize(nx, ny);
    path_maker_->setSize(nx, ny);
    potential_array_ = new float[nx * ny];

设置地图的边为lethal_obstacle(在前文参考博客中有介绍)

outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);

然后就是主要的算法计算potential。

  bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                    nx * ny * 2, potential_array_);

planner有Astar 和 Dijkstra两种方式。在初始化的时候会有选择。
然后会根据计算的potential进行路径规划，

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

    if (found_legal) {
        //extract the plan
        if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
            //make sure the goal we push on has the same timestamp as the rest of the 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 when a legal potential was found. This shouldn't happen.");
        }
    }else{
        ROS_ERROR("Failed to get a plan.");
    }

其中主要是getplanfrompotential.在这个函数中调用了path_maker的getpath.path_maker_的选择则是use_grid和 use_gradient.已经在之前有所介绍。
最后对路径增加orientation信息，最后发布。

	// add orientations if needed
    orientation_filter_->processPath(start, plan);

    //publish the plan for visualization purposes
    publishPlan(plan);
    delete potential_array_;
    return !plan.empty();

这个文件中最主要的就是计算potential和getplanfrompotential。即astar算法和dijkstra算法。以及use_grid和use_gradient.