全局规划器-NavfnROS

最新推荐文章于 2025-04-16 09:06:59 发布

慢慢的回味

最新推荐文章于 2025-04-16 09:06:59 发布

阅读量1.1k

点赞数

分类专栏： ROS小车

本文链接：https://blog.youkuaiyun.com/bbxfnet/article/details/118255853

版权

本文链接地址: 全局规划器-NavfnROS

Content:

全局规划器就如同平时生活中的地图导航一样。

调用路径

在Move Base中有全局规划器的如下调用，完成了全局规划器的实例化：

    try {
      planner_ = bgp_loader_.createInstance(global_planner);
      planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
    } catch (const pluginlib::PluginlibException& ex) {
      ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
      exit(1);
    }

planner初始化

planner_由类NavFn实现，plan_pub_用于发布规划路径，make_plan_srv_提供路径规划服务。

 void NavfnROS::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame){
    if(!initialized_){
      costmap_ = costmap;
      global_frame_ = global_frame;
      planner_ = boost::shared_ptr(new NavFn(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()));
 
      ros::NodeHandle private_nh("~/" + name);
 
      plan_pub_ = private_nh.advertise("plan", 1);
 
      private_nh.param("visualize_potential", visualize_potential_, false);
 
      //if we're going to visualize the potential array we need to advertise
      if(visualize_potential_)
        potarr_pub_ = private_nh.advertise("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);
 
      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");
  }

规划路径

NavfnROS::makePlan方法生成规划路径：

  bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start, 
      const geometry_msgs::PoseStamped& goal, double tolerance, std::vector& 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;
    }
 
    //clear the plan, just in case
    plan.clear();
 
    ros::NodeHandle n;
 
    //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
    if(goal.header.frame_id != global_frame_){
      ROS_ERROR("The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", 
                global_frame_.c_str(), goal.header.frame_id.c_str());
      return false;
    }
 
    if(start.header.frame_id != global_frame_){
      ROS_ERROR("The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", 
                global_frame_.c_str(), start.header.frame_id.c_str());
      return false;
    }
//获取开始位置，并确保开始位置在地图内
    double wx = start.pose.position.x;
    double wy = start.pose.position.y;
 
    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 localiz