System overview
move_base 主要有以下两部分
- Planner
- nav_core::BaseGlobalPlanner
- nav_core::BaseLocalPlanner
- nav_core::RecoveryBehavior
- costmap
- global planner costmap
- local planner costmap
注意上面灰色的模块是可选的,地图也是可有可无的,蓝色是根据不同的机器模块进行
配置的,白色模块是必须的
move_base
源码
namespace move_base{
enum MoveBaseState {
PLANNING,
CONTROLLING,
CLEARING
};
enum RecoveryTrigger
{
PLANNING_R,
CONTROLLING_R,
OSCILLATION_R
};
MoveBase::MoveBase(tf::TransformListener& tf){
#action server
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
#参数的配置读取
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
.........
//set up plan triple buffer
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
//set up the planner's thread
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
#global costmap,
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
#global planner
planner_ = bgp_loader_.createInstance(global_planner);
#local costmap
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
#local planner
tc_ = blp_loader_.createInstance(local_planner);
// Start actively updating costmaps based on sensor data
planner_costmap_ros_->start();
controller_costmap_ros_->start();
}
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal){
//we have a goal so start the planner(通知planner线程进行路径规划)
boost::unique_lock<boost::mutex> lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
#通知规划路径线程
planner_cond_.notify_one();
lock.unlock();
ros::NodeHandle n;
while(n.ok())
{
#被抢占了(可能是发出新的goal,也可能是取消了)
if(as_->isPreemptRequested()){
if(as_->isNewGoalAvailable()){
#发布新的goal,通知planner线程工作。
planner_cond_.notify_one();
}
else{
//if we've been preempted explicitly we need to shut things down
//强制退出
return;
}
}
//the real work on pursuing a goal is done here
//真正工作的代码
bool done = executeCycle(goal, global_plan);
//这个是一般的警告信息,规划的时间超时
if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
}
}
void MoveBase::planThread(){
ros::NodeHandle n;
boost::unique_lock<boost::mutex> lock(planner_mutex_);
while(n.ok()){
//等待上面的executeCb函数使得runPlanner_ = true
while(wait_for_wake || !runPlanner_){
//if we should not be running the planner then suspend this thread
ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
}
//time to plan! get a copy of the goal and unlock the mutex
geometry_msgs::PoseSta