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