movebase导航和地图数据的使用 - 3 movebase的初始化分析

MoveBase::MoveBase(tf::TransformListener& 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) {

//这个actionserver使用了actionlib中的逻辑，actionlib提供了一套框架，可以建立一个server,client可以发送可抢断的taskrequest, 可以将之前的taskrequest取消，覆盖。并且可以不断得到反馈的框架。，基于此框架，每当有新的targetgoal被设定时，会执行executeCb函数。

as_ = newMoveBaseActionServer(ros::NodeHandle(), "move_base",boost::bind(&MoveBase::executeCb, this, _1), false);

ros::NodeHandleprivate_nh("~"); 使用privatenamespace的handle,也就是使用move_base作为ns的handle.

ros::NodeHandle nh;

recovery_trigger_ = PLANNING_R;

//get some parameters that will be global to the move basenode

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

//实际使用的global/localplanner分别是：

//tshao@tshao-OptiPlex-9020:~$rosparam get /move_base/base_global_planner

//navfn/NavfnROS

//tshao@tshao-OptiPlex-9020:~$rosparam get /move_base/base_local_planner

//dwa_local_planner/DWAPlannerROS

private_nh.param("global_costmap/robot_base_frame",robot_base_frame_, std::string("base_link"));

private_nh.param("global_costmap/global_frame",global_frame_, std::string("/map"));

private_nh.param("planner_frequency",planner_frequency_, 0.0);

private_nh.param("controller_frequency",controller_frequency_, 20.0);

private_nh.param("planner_patience",planner_patience_, 5.0);

private_nh.param("controller_patience",controller_patience_, 15.0);

private_nh.param("oscillation_timeout",oscillation_timeout_, 0.0);

private_nh.param("oscillation_distance",oscillation_distance_, 0.5);

//set up plan triple buffer

planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();

latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();

controller_plan_ = newstd::vector<geometry_msgs::PoseStamped>();

//set up the planner's thread

//planThread,是工作线程？这个线程会根据一个target调用plan算法生成一个plan.一个Plan其实就是一些点，而之后具体使用这些点去驱动底座行走，并不是本thread的工作内容。

planner_thread_ = newboost::thread(boost::bind(&MoveBase::planThread, this));

//for comanding the base

//向mobilebase底座发送命令的topic,这个topic会被remap。

vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel",1);

current_goal_pub_ =private_nh.advertise<geometry_msgs::PoseStamped>("current_goal",0 );

ros::NodeHandle action_nh("move_base");

action_goal_pub_ =action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal",1);

//we'll provide a mechanism for some people to send goals asPoseStamped messages over a topic

//they won't get any useful information back about its status,but this is useful for tools

//like nav_view and rviz

//这里监听一个/move_base_simple/goal的topic.使用者可以向这个topic发送消息，设定一个目的地，我们监听这个消息，调用goalCB处理，其实具体的处理方法是将这个消息，转换成对actionServer的相应service的调用。所以这里其实会简介导致MoveBase::executeCb被调用。

ros::NodeHandlesimple_nh("move_base_simple");

goal_sub_ =simple_nh.subscribe<geometry_msgs::PoseStamped>("goal",1, boost::bind(&MoveBase::goalCB, this, _1));

//we'll assume the radius of the robot to be consistent withwhat's specified for the costmaps

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

//create the ros wrapper for the planner's costmap... andinitializer a pointer we'll use with the underlying map

//为globalplanner建立costmap.

planner_costmap_ros_ = newcostmap_2d::Costmap2DROS("global_costmap", tf_);

planner_costmap_ros_->pause();

//initialize the globalplanner

try {

//check if a non fully qualified name has potentially beenpassed in

if(!bgp_loader_.isClassAvailable(global_planner)){

std::vector<std::string> classes =bgp_loader_.getDeclaredClasses();

for(unsigned int i = 0; i < classes.size(); ++i){

if(global_planner == bgp_loader_.getName(classes[i])){

//if we've found a match... we'll get the fullyqualified name and break out of the loop

ROS_WARN("Planner specifications should nowinclude the package name. You are using a deprecated API. Pleaseswitch from %s to %s in your yaml file.",

global_planner.c_str(), classes[i].c_str());

global_planner = classes[i];

break;

}

}

}

//加载并初始化globalplanner

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 yousure it is properly registered and that the containing library isbuilt? Exception: %s", global_planner.c_str(), ex.what());

exit(1);

}

//create the ros wrapper for the controller's costmap... andinitializer a pointer we'll use with the underlying map

//建立localcostmap

controller_costmap_ros_ =new costmap_2d::Costmap2DROS("local_costmap", tf_);

controller_costmap_ros_->pause();

//create a local planner

try {

//check if a non fully qualified name has potentially beenpassed in

if(!blp_loader_.isClassAvailable(local_planner)){

std::vector<std::string> classes =blp_loader_.getDeclaredClasses();

for(unsigned int i = 0; i < classes.size(); ++i){

if(local_planner == blp_loader_.getName(classes[i])){

//if we've found a match... we'll get the fullyqualified name and break out of the loop

ROS_WARN("Planner specifications should nowinclude the package name. You are using a deprecated API. Pleaseswitch from %s to %s in your yaml file.",

local_planner.c_str(), classes[i].c_str());

local_planner = classes[i];

break;

}

}

}

//建立并初始化localplanner.

tc_ =blp_loader_.createInstance(local_planner);

ROS_INFO("Createdlocal_planner %s", local_planner.c_str());

tc_->initialize(blp_loader_.getName(local_planner), &tf_,controller_costmap_ros_);

} catch (const pluginlib::PluginlibException& ex)

{

ROS_FATAL("Failed to create the %s planner, are yousure it is properly registered and that the containing library isbuilt? Exception: %s", local_planner.c_str(), ex.what());

exit(1);

}

// Start actively updating costmaps based on sensor data

planner_costmap_ros_->start();

controller_costmap_ros_->start();

//advertise a service forgetting a plan

make_plan_srv_ = private_nh.advertiseService("make_plan",&MoveBase::planService, this);

//advertise a service forclearing the costmaps

clear_costmaps_srv_ =private_nh.advertiseService("clear_costmaps",&MoveBase::clearCostmapsService, this);

//if we shutdown our costmaps when we're deactivated... we'lldo that now

if(shutdown_costmaps_){

ROS_DEBUG_NAMED("move_base","Stoppingcostmaps initially");

planner_costmap_ros_->stop();

controller_costmap_ros_->stop();

}

//load any user specified recovery behaviors, and if thatfails load the defaults

if(!loadRecoveryBehaviors(private_nh)){

loadDefaultRecoveryBehaviors();

}

//initially, we'll need to make a plan

state_ = PLANNING;

//we'll start executing recovery behaviors at the beginning ofour list

recovery_index_ = 0;

//we're all set up now so we can start the action server

as_->start();

//设置reconfig的callback/

dsrv_ = newdynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));

dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackTypecb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);

dsrv_->setCallback(cb);

//http://docs.ros.org/api/costmap_2d/html/classcostmap__2d_1_1Costmap2DROS.html

//http://docs.ros.org/api/costmap_2d/html/classcostmap__2d_1_1Costmap2D.html#a43ca3645f610b4586810376c96d0d226

walk_thread_ = NULL;

private_nh.param<double>("square_size",square_size_, 5.0);

private_nh.param<double>("threshold", t_,2.0);

ROS_INFO("MoveBase::MoveBase, amc: square_size %lf,threshold %lf\n", square_size_, t_);

amcInProcess = false;

amc_service_srv_ =private_nh.advertiseService("automatic_map_construction",&MoveBase::amcCallback, this);

}