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