MoveBase::planService分析

bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){
//req包含了起点信息和目标信息
    if(as_->isActive()){
      ROS_ERROR("move_base must be in an inactive state to make a plan for an external user");
      return false;
    }//必须处于非活动状态才能为外部用户制定计划
    //make sure we have a costmap for our planner
    if(planner_costmap_ros_ == NULL){//判断是否存在代价地图
      ROS_ERROR("move_base cannot make a plan for you because it doesn't have a costmap");
      return false;
    }
   geometry_msgs::PoseStamped start;
    //if the user does not specify a start pose, identified by an empty frame id, then use the robot's pose
    if(req.start.header.frame_id.empty())
    {
        tf::Stamped<tf::Pose> global_pose;
        if(!planner_costmap_ros_->getRobotPose(global_pose)){
          ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
          return false;
        }
        tf::poseStampedTFToMsg(global_pose, start);
    }
    else
    {
        start = req.start;
    }//使用默认或者自定义的数据

首先判断global planner 的costmap是否存在以及是否能够得到机器人所在位置

clearCostmapWindows：首先通过planner_costmap_ros_->getRobotPose(global_pose)获取在全局地图的pose，然后以这个点为中心，找到以size_x和size_y 为边长的矩形的四个顶点，调用planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);完成对机器人所在区域的clear工作。同样的操作在controller_costmap_ros_上也操作一遍，这样关于globa costmap 和local costmap都已经在机器人所在的区域完成clear工作。

clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);

核心计算部分

//first try to make a plan to the exact desired goal
    std::vector<geometry_msgs::PoseStamped> global_plan;
    if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){
      ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance", 
          req.goal.pose.position.x, req.goal.pose.position.y);

      //search outwards for a feasible goal within the specified tolerance
      geometry_msgs::PoseStamped p;
      p = req.goal;
      bool found_legal = false;
      float resolution = planner_costmap_ros_->getCostmap()->getResolution();
      float search_increment = resolution*3.0;
      if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance;//确定梯度
      for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) {
        for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) {
          for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) {

            //don't search again inside the current outer layer
            if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue;//在目标区域附近搜索

            //search to both sides of the desired goal构造正负号，在两边进行搜索
            for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) {

              //if one of the offsets is 0, -1*0 is still 0 (so get rid of one of the two)
              if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue;

              for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) {
                if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue;

                p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult;
                p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult;

                if(planner_->makePlan(start, p, global_plan)){
                  if(!global_plan.empty()){

                    //adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there
                    //(the reachable goal should have been added by the global planner)
                    global_plan.push_back(req.goal);

                    found_legal = true;
                    ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
                    break;
                  }
                }
                else{
                  ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
                }
              }
            }
          }
        }
      }
    }

    //copy the plan into a message to send out
    resp.plan.poses.resize(global_plan.size());
    for(unsigned int i = 0; i < global_plan.size(); ++i){
      resp.plan.poses[i] = global_plan[i];
    }

    return true;

planner_->makePlan(start, p, global_plan)​​​​​​​

虚拟布尔nav_core :: BaseGlobalPlanner :: makePlan	（	const geometry_msgs :: PoseStamped＆	开始，
		const geometry_msgs :: PoseStamped＆	目标，
		std :: vector <geometry_msgs :: PoseStamped>＆	计划
	）			[pure virtual]

给定一个世界上的目标姿势，计算一个计划。

参数：

	开始	起始姿势
	目标	目标姿势
	计划	计划...由计划者填写

返回值：

如果找到有效计划，则为true，否则为false