ROS move_base.cpp源码分析（一）主函数movebase

ROS move_base

摘要：
move_base包提供了一个动作的实现（请参见actionlib包），给定一个世界上的目标，该动作将尝试通过移动基地来实现。move_base节点将全局计划程序和本地计划程序链接在一起，以完成其全局导航任务。它支持任何全球规划师坚持在指定的nav_core :: BaseGlobalPlanner接口nav_core包和任何地方规划师秉承nav_core :: BaseLocalPlanner接口指定nav_core包。move_base节点还维护两个用于完成导航任务的成本图，一个用于全局计划程序，一个用于本地计划程序（请参阅costmap_2d包）。
参考：http://wiki.ros.org/move_base/

Movebase源码位于Navigation包中，大家可在Git官方代码包中进行安装：

git clone https://github.com/ros-planning/navigation.git

MoveBase::MoveBase(tf2_ros::Buffer& tf):
    tf_(tf),
    as_(NULL),
    planner_costmap_ros_(NULL),controller_costmap_ros_(NULL),
    bgp_loader_("nav_core","nav_core::BaseGlobalPlanner"),
    blp_loader_("nav_core","nav_core::BaseLocalPlanner"),
    recovery_loader_("nav_core","nav_core::RecoveryBehavior"),
    planner_plan_(NULL),latest_plan_(NULL),controller_plan_(NULL),
    runPlanner_(false),setup_(false),p_freq_change_(false),c_freq_change_(false),
    new_global_plan_(false){
        
/*---------------------------------------------------------------
1.创建一个Action Server::MoveBaseActionServer,绑定回调函数,该服务器的Callback为MoveBase::executeCb 
actionlib会启动一个线程，当外部请求到来时，调用MoveBase::executeCb回调函数处理。
---------------------------------------------------------------*/
    as_ = new MoveBaseActionServer(ros::NodeHandle(),"move_base",boost::bind(&MoveBase::executeCb,this,_1),false);
    //定义一个名为move_base的SimpleActionServer
    
    ros::NodeHandle private_nh("~");
    ros::NodeHandle nh;

    recovery_trigger_ = PLANNING_R;

/*---------------------------------------------------------------
2.读取配置:参数从参数服务器获取一些参数，包括两个规划器名称、代价地图坐标系、规划频率、控制周期等 
---------------------------------------------------------------*/
    std::string global_planner,local_planner;
    private_nh.param("base_global_planner",global_planner,std::string("navfn/NavfnROS"));
    private_nh.param("base_local_planner",local_planner,std::string("base_local_planner/TrajectoryPlannerROS"));
    private_nh.param("global_costmap/robot_base_frame",robot_base_frame_,std::string("base_link"));
    private_nh.param("planner_frequency",planner_frequency_,0.0);
    private_nh.param("controller_frequency",planner_frequency_,20.0);
    private_nh.param("planner_patience",planner_patience_,5.0);
    private_nh.param("controller_patience",controller_patience_,15.0);
    private_nh.param("max_planning_retries",max_planning_retries_,-1);
    private_nh.param("oscillation_timeout",oscillation_timeout_,0.0);
    private_nh.param("oscillation_distance",oscillation_distance_,0.5);
    private_nh.param("make_plan_clear_costmap", make_plan_clear_costmap_, true);
    private_nh.param("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, true);
/*---------------------------------------------------------------
          3.为Global Planner分配存放路径的内存，使用Vector储存
---------------------------------------------------------------*/
    planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
/*---------------------------------------------------------------
                4.创建并启动Global Planner线程
---------------------------------------------------------------*/    
    planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread,this));
/*---------------------------------------------------------------
                  5.订阅和发布相关的Topic
---------------------------------------------------------------*/  
    vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel",1); //发布 /cmd_vel
    current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal",0); //发布 /move_base/current_goal

    ros::NodeHandle action_nh("move_base");
    action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal",1);// 发布 /move_base/goal
    recovery_status_pub_= action_nh.advertise<move_base_msgs::RecoveryStatus>("recovery_status", 1);
    
    //提供消息类型为geometry_msgs::PoseStamped的发送goals的接口，比如cb为MoveBase::goalCB，在rviz中输入的目标点就是通过这个函数来响应的
    ros::NodeHandle simple_nh("move_base_simple");
    goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal",1,boost::bind(&MoveBase::goalCB,this,_1));// 订阅 /move_base_simple/goal

    private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
    private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
    private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
    private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);

    private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
    private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
    private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);
/*---------------------------------------------------------------
            6.创建和初始化Global Planner的costmap
---------------------------------------------------------------*/
    planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
    planner_costmap_ros_->pause();  //先暂停运行
    /*---------costmap的动态库位于 /opt/ros/kenetic/lib/libcostmap_2d.so
    会先启动一个线程处理工作流程：costmap_2d::Costmap2DROS::UdateLoop(double)*/
/*---------------------------------------------------------------
              7.创建和初始化Global Planner
---------------------------------------------------------------*/     
    try {
          planner_ = bgp_loader_.createInstance(global_planner); //使用的planner为global_planner/GlobalPlanner
          planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);//加载和初始化相应的动态库/opt/ros/kenetic/lib/libglobal_planner.so
        } 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);
        }
/*---------------------------------------------------------------
              8.创建和初始化Local Planner的costmap
---------------------------------------------------------------*/ 
    controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
    controller_costmap_ros_->pause(); //先暂停运行
/*---------------------------------------------------------------
                9.创建和初始化Local Planner
---------------------------------------------------------------*/
//create a local planner
    try {
          tc_ = blp_loader_.createInstance(local_planner); //使用的Planner为teb_local_planner/TebLocalPlannerROS
          ROS_INFO("Created local_planner %s", local_planner.c_str());
          tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_); //加载和初始化相应的动态库/opt/ros/kenetic/lib/libteb_local_planner.so
        } 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", local_planner.c_str(), ex.what());
          exit(1);
        }
/*---------------------------------------------------------------
              10.启动global和local costmap的处理
---------------------------------------------------------------*/          
    planner_costmap_ros_->start();
    controller_costmap_ros_->start();
/*---------------------------------------------------------------
          11.启动两个Service：创建地图,清除地图服务
---------------------------------------------------------------*/
    make_plan_srv_ = private_nh.advertiseService("make_plan",&MoveBase::planService,this);
    //定义一个名为clear_costmaps的服务，cb为MoveBase::clearCostmapsService 
    clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps",&MoveBase::clearCostmapsService,this); 
    //如果不小心关闭了costmap
    if(shutdown_costmaps_){
      ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
      planner_costmap_ros_->stop();
      controller_costmap_ros_->stop();
    }
/*---------------------------------------------------------------
            12.加载Recovery Behavior恢复器
---------------------------------------------------------------*/
    if(!loadRecoveryBehaviors(private_nh)){
      loadDefaultRecoveryBehaviors();
    }
/*---------------------------------------------------------------
              13.设置MoveBase的初始状态为Planning
---------------------------------------------------------------*/
    state_ = PLANNING;
/*---------------------------------------------------------------
              14.执行Recovery Behavior
---------------------------------------------------------------*/
    recovery_index_ = 0;     
/*---------------------------------------------------------------
  15.启动执行Action Server：此时，Move Base就可以处理外部导航请求了
---------------------------------------------------------------*/
    as_->start();
/*---------------------------------------------------------------
  16.启动动态配置参数功能：Move Base的一些参数支持修改，通过此功能实现
---------------------------------------------------------------*/    
    dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
    //启动动态参数服务器，回调函数为reconfigureCB
    dynamic_reconfigure::Server<MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB,this,_1,_2);
    dsrv_->setCallback(cb);
  }