12.1 ROS NavFn全局規划源碼_1

博客轉載自：https://blog.csdn.net/Neo11111/article/details/104643134

Movebase使用的全局規划器默認為NavFn，默認使用Dijkstra算法，在地圖上的起始點和目標點間規划出一條最優路徑，供局部規划器具體導航使用。在理解這部分的過程中也參考了很多博客，NavFn的源碼中實際上是有A*規划算法的函數的，但關於為什么不在NavFn中使用A*，ROS Wiki上對提問者的回答是，早期NavFn包中的A*有bug，沒有處理，后來發布了global_planner，修改好了A*的部分。global_planner封裝性更強，它和NavFn都是用於全局路徑規划的包。

【相關文件】

•  navfn/src/navfn_ros.cpp
•  navfn/src/navfn.cpp

navfn_ros.cpp中定義了NavfnROS類，navfn.cpp中定義了NavFn類，ROS Navigation整個包的一個命名規則是，帶有ROS后綴的類完成的是該子過程與整體和其他過程的銜接框架和數據流通，不帶ROS后綴的類中完成該部分的實際工作，並作為帶有ROS后綴的類的成員。

本篇記錄對navfn_ros.cpp中定義的NavfnROS類的閱讀和理解。

【代碼分析】

navfn_ros.cpp

–目錄–
准備工作 NavfnROS::initialize | 初始化
核心函數 NavfnROS::makePlan | 調用成員類NavFn的規划函數
獲取單點Potential值 NavfnROS::getPointPotential | 在NavfnROS::makePlan中被調用
獲取規划結果 NavfnROS::getPlanFromPotential | 在NavfnROS::makePlan中被調用

<1> NavfnROS::initialize

這里主要對參數進行初始化，在MoveBase中首先被調用。這里先用參數傳入的costmap對地圖進行初始化。

  void NavfnROS::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame){
    if(!initialized_){
      costmap_ = costmap;//全局代價地圖
      global_frame_ = global_frame;

並對成員類NavFn初始化，這個類將完成全局規划實際計算。

      //指向NavFn類實例，傳入參數為地圖大小
      planner_ = boost::shared_ptr<NavFn>(new NavFn(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()));

      //創建全局規划器名稱下的句柄
      ros::NodeHandle private_nh("~/" + name);
      //發布全局規划器名稱/plan話題
      plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
      //獲取參數服務器上的參數，如果沒有，就使用默認值
      private_nh.param("visualize_potential", visualize_potential_, false);

      //如果要將potential array可視化，則發布節點名稱下的/potential話題，需要的用戶可以訂閱
      if(visualize_potential_)
        potarr_pub_.advertise(private_nh, "potential", 1);

      //從參數服務器上獲取以下參數
      private_nh.param("allow_unknown", allow_unknown_, true);
      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);

      //獲取tf前綴
      ros::NodeHandle prefix_nh;
      tf_prefix_ = tf::getPrefixParam(prefix_nh);

      //發布make_plan的服務
      make_plan_srv_ =  private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this);

      //初始化標志位設為真
      initialized_ = true;
    }
    else
      //否則，已經被初始化過了，打印提示即可，不重復初始化
      ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
  }

<2> NavfnROS::makePlan

makePlan是在Movebase中對全局規划器調用的函數，它是NavfnROS類的重點函數，負責調用包括Navfn類成員在內的函數完成實際計算，控制着全局規划的整個流程。它的輸入中最重要的是當前和目標的位置。

  bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start, 
      const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan){
    boost::mutex::scoped_lock lock(mutex_);
    //檢查是否初始化
    if(!initialized_){
      ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
      return false;
    }

准備工作：規划前先清理plan，等待tf，存儲當前起點位置並轉換到地圖坐標系，並將全局costmap上起點的cell設置為FREE_SPACE。

    //清理plan
    plan.clear();
    ros::NodeHandle n;
    //確保收到的目標和當前位姿都是基於當前的global frame
    //注：tf::resolve或者TransformListener::resolve方法的作用就是使用tf_prefix參數將frame_name解析為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;
    }

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

    //起始位姿wx、wy
    double wx = start.pose.position.x;
    double wy = start.pose.position.y;

    //全局代價地圖坐標系上的起始位姿mx、my
    unsigned int mx, my;
    if(!costmap_->worldToMap(wx, wy, mx, my)){
      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;
    }

    //清理起始位置cell（必不是障礙物）
    tf::Stamped<tf::Pose> start_pose;
    tf::poseStampedMsgToTF(start, start_pose);
    clearRobotCell(start_pose, mx, my);

planner指向的是NavFn類，這里調用它的setNavArr函數，主要作用是給定地圖的大小，創建NavFn類中使用的costarr數組（記錄全局costmap信息）、potarr數組（儲存各cell的Potential值）、以及x和y向的梯度數組（用於生成路徑），這三個數組構成NavFn類用Dijkstra計算的主干，在NavFn類中詳述。
調用setCostmap函數給全局costmap賦值。

#if 0
    {
      static int n = 0;
      static char filename[1000];
      snprintf( filename, 1000, "navfnros-makeplan-costmapB-%04d.pgm", n++ );
      costmap->saveRawMap( std::string( filename ));
    }
#endif

    //重新設置Navfn使用的underlying array的大小，並將傳入的代價地圖設置為將要使用的全局代價地圖
    planner_->setNavArr(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
    planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);

這里和上邊有兩部分用於保存不同格式的地圖，與主體關系不大。

#if 0
    {
      static int n = 0;
      static char filename[1000];
      snprintf( filename, 1000, "navfnros-makeplan-costmapC-%04d", n++ );
      planner_->savemap( filename );
    }
#endif
    //起始位姿存入map_start[2]
    int map_start[2];
    map_start[0] = mx;
    map_start[1] = my;

    //獲取global系下的目標位置
    wx = goal.pose.position.x;
    wy = goal.pose.position.y;

    //坐標轉換到地圖坐標系
    if(!costmap_->worldToMap(wx, wy, mx, my)){
      if(tolerance <= 0.0){
        ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
        return false;
      }
      mx = 0;
      my = 0;
    }
    //目標位置存入map_goal[2]
    int map_goal[2];
    map_goal[0] = mx;
    map_goal[1] = my;

接下來將設置NavFn類的起點和目標位置，並調用該類的calcNavFnDijkstra函數，這個函數可以完成全局路徑的計算。

    //傳入Navfn實例中
    planner_->setStart(map_goal);
    planner_->setGoal(map_start);

    planner_->calcNavFnDijkstra(true);

接下來，在目標位置附近2*tolerance的矩形范圍內，尋找與目標位置最近的、且不是障礙物的cell，作為全局路徑實際的終點，這里調用了類內getPointPotential函數，目的是獲取單點Potential值，與DBL_MAX比較，確定是否是障礙物。

    double resolution = costmap_->getResolution();
    geometry_msgs::PoseStamped p, best_pose;
    p = goal;

    bool found_legal = false;
    double best_sdist = DBL_MAX;

    p.pose.position.y = goal.pose.position.y - tolerance;

    while(p.pose.position.y <= goal.pose.position.y + tolerance){
      p.pose.position.x = goal.pose.position.x - tolerance;
      while(p.pose.position.x <= goal.pose.position.x + tolerance){
        double potential = getPointPotential(p.pose.position);
        double sdist = sq_distance(p, goal);
        if(potential < POT_HIGH && sdist < best_sdist){
          best_sdist = sdist;
          best_pose = p;
          found_legal = true;
        }
        p.pose.position.x += resolution;
      }
      p.pose.position.y += resolution;
    }

若成功找到實際終點best_pose，調用類內getPlanFromPotential函數，將best_pose傳遞給NavFn，獲得最終Plan並發布。

    if(found_legal){
      //extract the plan
      if(getPlanFromPotential(best_pose, plan)){
        //make sure the goal we push on has the same timestamp as the rest of the plan
        geometry_msgs::PoseStamped goal_copy = best_pose;
        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.");
      }
    }

potarr數組的發布，與主體關系不大。

    if (visualize_potential_){
      //發布Potential數組
      pcl::PointCloud<PotarrPoint> pot_area;
      pot_area.header.frame_id = global_frame_;
      pot_area.points.clear();
      std_msgs::Header header;
      pcl_conversions::fromPCL(pot_area.header, header);
      header.stamp = ros::Time::now();
      pot_area.header = pcl_conversions::toPCL(header);

      PotarrPoint pt;
      float *pp = planner_->potarr;
      double pot_x, pot_y;
      for (unsigned int i = 0; i < (unsigned int)planner_->ny*planner_->nx ; i++)
      {
        if (pp[i] < 10e7)
        {
          mapToWorld(i%planner_->nx, i/planner_->nx, pot_x, pot_y);
          pt.x = pot_x;
          pt.y = pot_y;
          pt.z = pp[i]/pp[planner_->start[1]*planner_->nx + planner_->start[0]]*20;
          pt.pot_value = pp[i];
          pot_area.push_back(pt);
        }
      }
      potarr_pub_.publish(pot_area);
    }

    //plan發布
    publishPlan(plan, 0.0, 1.0, 0.0, 0.0);

    return !plan.empty();
  }

<3> NavfnROS::getPointPotential

它在makePlan中被調用，主要工作是獲取NavFn類成員-potarr數組記錄的對應cell的Potential值。

  double NavfnROS::getPointPotential(const geometry_msgs::Point& world_point){
    //檢查是否初始化
    if(!initialized_){
      ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
      return -1.0;
    }

    //將點轉換到地圖坐標系下
    unsigned int mx, my;
    if(!costmap_->worldToMap(world_point.x, world_point.y, mx, my))
      return DBL_MAX;

    //nx、ny是像素單位的地圖網格的x、y方向上長度
    //計算矩陣中的索引=地圖x向長度*點的y坐標+點的x坐標
    unsigned int index = my * planner_->nx + mx;
    //potarr即Potential Array，勢場矩陣
    //傳入索引，得該點勢場
    return planner_->potarr[index];
  }

<4> NavfnROS::getPlanFromPotential

它在makePlan中被調用，主要工作是調用了NavFn類的一些函數，設置目標、獲取規划結果。

  bool NavfnROS::getPlanFromPotential(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
    if(!initialized_){
      ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
      return false;
    }

    //clear the plan, just in case
    plan.clear();

    //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
    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;
    }

下面將makePlan末尾處找到的goal附近的best_pose坐標轉換到地圖坐標系，並通過調用NavFn類的setStart函數傳遞，作為路徑的實際終點，再調用NavFn類calcPath函數，完成路徑計算。

    //儲存bestpose的坐標
    double wx = goal.pose.position.x;
    double wy = goal.pose.position.y;

    //the potential has already been computed, so we won't update our copy of the costmap
    unsigned int mx, my;
    if(!costmap_->worldToMap(wx, wy, mx, my)){
      ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
      return false;
    }
    //besepose轉換到map坐標系后存儲
    int map_goal[2];
    map_goal[0] = mx;
    map_goal[1] = my;

   //調用navfn的設置起始、calcPath、getPathX等函數，並將計算出的路徑點依次存放plan，得到全局規划路線
    planner_->setStart(map_goal);

    planner_->calcPath(costmap_->getSizeInCellsX() * 4);

調用NavFn的類方法獲取規划結果的坐標，填充plan之后將其發布。

    //extract the plan
    float *x = planner_->getPathX();
    float *y = planner_->getPathY();
    int len = planner_->getPathLen();
    ros::Time plan_time = ros::Time::now();

    for(int i = len - 1; i >= 0; --i){
      //convert the plan to world coordinates
      double world_x, world_y;
      mapToWorld(x[i], y[i], 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);
    }

    //publish the plan for visualization purposes
    publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
    return !plan.empty();
  }
};