TOP

move_base代码学习一(一)
2017-10-13 10:23:26 】 浏览:10926
Tags:move_base 代码 学习

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  
		
move_base代码学习一(一) https://www.cppentry.com/bencandy.php?fid=49&id=161007

首页 上一页 1 2 3 4 5 6 7 下一页 尾页 1/8/8
】【打印繁体】【投稿】【收藏】 【推荐】【举报】【评论】 【关闭】 【返回顶部
上一篇洛谷P1177快速排序 下一篇GCC与G++的区别