ROS naviagtion analysis: move_base

这是navigation的第一篇文章,主要通过分析ROS代码级实现,了解navigation。本文从move_base入手。
机器人导航主要框架如图:
Navigation Stack主要组成部分:move_base:
ROS naviagtion analysis: move_base_第1张图片
ROS naviagtion analysis: move_base_第2张图片
ROS naviagtion analysis: move_base_第3张图片

用户调用movebase是通过传入带tf参数的构造函数:
move_base::MoveBase move_base( tf );

以下分析move_base的构造函数:

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

这部分是构造函数的初始化列表,可以看到几个重要的参数:

    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_costmap_ros_是用于全局导航的地图,controller_costmap_ros_ 是局部导航用的地图,地图类型为经过ROS封装的costmap_2d::Costmap2DROS* 。关于地图类型的分析会在接下来的文章中进行。
bgp_loader_ 是global planner, blp_loader_ 是local planner。二者的声明为:
pluginlib::ClassLoader bgp_loader_;
pluginlib::ClassLoader blp_loader_;

是属于一个模板类ClassLoader,实例化为BaseGlobalPlanner或者BaseLocalPlanner。关于pluginlib的分析也有在接下来的文章中进行。
bgp_loader_ 和 blp_loader_ 的作用是为以下类成员提供实例化:

      boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;
      boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
 //initialize the global planner
    try {
      planner_ = bgp_loader_.createInstance(global_planner);
      planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
    } `
    ` //create a local planner
    try {
      tc_ = blp_loader_.createInstance(local_planner);
      ROS_INFO("Created local_planner %s", local_planner.c_str());
      tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
    }

进入构造函数,第一句:

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

as_维护movebase的actionServer状态机,并且新建了一个executeCb线程。
接下来从private_nh中获取参数设定值。
设置plan buffer:

planner_plan_ = new std::vector();
latest_plan_ = new std::vector();
controller_plan_ = new std::vector();

注意,这三个plan都是global_plan,最终由controller_plan_ 传给 local planner:
Line821 if(!tc_->setPlan(*controller_plan_)){
开启planner thread:

//set up the planner's thread
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

接下来设置一堆publisher, 包括cmd_vel,current_goal,goal。
然后是机器人的几何尺寸设定等。
然后实例化planner_costmap_ros_,controller_costmap_ros_

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

开启costmap基于传感器数据的更新:

    // Start actively updating costmaps based on sensor data
    planner_costmap_ros_->start();
    controller_costmap_ros_->start();

载入recovery behavior,这部分是状态机的设计问题,可以采用默认的设定,也可以用户指定:

    if(!loadRecoveryBehaviors(private_nh)){
      loadDefaultRecoveryBehaviors(); }

然后开启actionserver: as_->start(); 开启参数动态配置服务,完了~这就是整个构造函数做的事情。

以下分析各个成员函数:

void MoveBase::clearCostmapWindows(double size_x, double size_y){
首先通过planner_costmap_ros_->getRobotPose(global_pose); 获取在全局地图的pose,然后以这个点为中心,找到以size_x和size_y 为边长的矩形的四个顶点,调用planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE); 完成对机器人所在区域的clear工作。同样的操作在controller_costmap_ros_ 上也操作一遍,这样关于globa costmap 和local costmap都已经在机器人所在的区域完成clear工作。

bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
req参数包含了起点和目标信息。首先判断global planner 的costmap是否存在以及是否能够得到机器人所在位置,然后调用clearCostmapWindows 完成对机器人区域的clear,然后调用 if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){ 这是完成plan计算的核心部分。逻辑判断这个调用是否成功,如果失败,则在目标区域附件搜索,更改req.goal的值,并重新调用makePlan,如果失败,则此次plan失败,无解。

bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector& plan){
首先lock 住global costmap:
boost::unique_lock lock(*(planner_costmap_ros_->getCostmap()->getMutex()));
然后判断global costmap是否存在以及是否能够获得机器人所在位置,如果失败,则直接返回失败。最后调用核心代码:if(!planner_->makePlan(start, goal, plan) || plan.empty()){

geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){
这个函数的核心功能是获得机器人坐标系和全局地图坐标系的关系:
tf_.transformPose(global_frame, goal_pose, global_pose);

void MoveBase::wakePlanner(const ros::TimerEvent& event)
planner_cond_.notify_one(); 通过boost thread的条件变量planner_cond_ 唤醒线程 planThread

MoveBase的核心函数是线程planThread:void MoveBase::planThread()
进入函数首先会将planner_mutex_ 锁住:boost::unique_lock lock(planner_mutex_);

   while(n.ok()){
      //check if we should run the planner (the mutex is locked)
      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");
        planner_cond_.wait(lock);
        wait_for_wake = false;
      }

因此线程继续运行依赖于变量runPlanner_ ,唤醒线程依赖于条件变量planner_cond_ 这两个变量是后续其他函数需要唤醒并执行线程planThread的关键。
然后调用makePlan:
//run planner
planner_plan_->clear();
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);

如果makePlan成功,则上锁,然后对planner_plan_ 拷贝,过程如下:
将最新的plan path给latest_plan_,然后将上一次的plan path给planner_plan_,这样这两个buffer就保存了最新的plan 和上一次的plan。

void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
首先检查传入的参数合法性,主要是pose.orientation检查。然后调用geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose) 将goal转换为在全局地图下的goal。
然后开启planner:

    boost::unique_lock::mutex> lock(planner_mutex_);
    planner_goal_ = goal;
    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();

然后进入死循环:首先判断as_是否是可抢占的,检查是否有新的goal,如果有则处理新的goal,判断new goal的合法性,并开启planThread 为new goal 生成plan。继续往下,判断goal的坐标系和planner_costmap_ros_ 的坐标系是否是一致的,如果不是,则调用goal = goalToGlobalFrame(goal) 转换到 planner_costmap_ros_的坐标系下,然后重新开启planThread。最终,我们拿到了global plan, 接下来调用核心代码:
bool done = executeCycle(goal, global_plan);
在死循环最后sleep足够时间,以满足frequency的要求。
如果死循环被退出,则在本函数末尾开启planThread,似的线程能正常执行到末尾,当线程再次在死循环中运行时,检查while(n.ok()){ 会失败,然后线程正常清理退出。

bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector& global_plan){
进入函数,第一行:boost::recursive_mutex::scoped_lock ecl(configuration_mutex_)会 锁住 函数void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){
处理震荡问题:

    if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
    {
      last_oscillation_reset_ = ros::Time::now();
      oscillation_pose_ = current_position;
      if(recovery_trigger_ == OSCILLATION_R)
        recovery_index_ = 0;
    }

然后检查controller_costmap_ros_ 是否是最新的if(!controller_costmap_ros_->isCurrent()){ 。然后检查是否有新的plan,如果有,则 上锁递推 controller_plan_latest_plan_
然后将global的plan传递给tc_: if(!tc_->setPlan(*controller_plan_)){, 如果失败则关闭planThread并直接退出。
然后进入movebase的控制逻辑的状态机:
状态机根据两个变量实现状态表示:{state_, recovery_trigger_}
这两个变量的取值:

 enum MoveBaseState{PLANNING, CONTROLLING, CLEARING};
 enum RecoveryTrigger{PLANNING_R, CONTROLLING_R,OSCILLATION_R};

RecoveryTrigger 对应的是在相应状态下出现失败。
注意,这个状态机和planThread 是在交互的。状态机一旦进入PLANNING状态,就唤醒planThread线程,在planThread线程中一旦找到plan就将state_设置为CONTROLLING,如果没有找到plan,则state_ = CLEARING;recovery_trigger_ = PLANNING_R;
如果状态机在CONTROLLING状态,首先检查是否到达目的地if(tc_->isGoalReached()){ ,如果到达目的地,则resetState(); 并关闭planThread runPlanner_ = false; 如果震荡条件满足,则:publishZeroVelocity();state_ = CLEARING; recovery_trigger_ = OSCILLATION_R;;然后锁住local costmap :

 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));

然后由baseLocalPlanner计算cmd_vel:

if(tc_->computeVelocityCommands(cmd_vel)){

成功则可以控制地盘了,如果失败,检查是否超时,如果超时:publishZeroVelocity();state_ = CLEARING;recovery_trigger_ = CONTROLLING_R; 没有超时:state_ = PLANNING;publishZeroVelocity(); 并唤醒planThread线程。
如果状态机在CLEARING状态:

if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size())
{
recovery_behaviors_[recovery_index_]->runBehavior();**重点内容**
last_oscillation_reset_ = ros::Time::now();
state_ = PLANNING;
recovery_index_++;
}
else
{
//disable the planner thread
boost::unique_lock::mutex> lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
resetState();
return true;
}

moveBase基本上主要内容就是这些,其他未分析的函数都比较简单。但是在moveBase的实现中,主要依赖于costmap_2d 以及global planner 和local planner。还有pluginlib的使用,简化了各种具体实现,因此在接下来的文章中着重分析这几部分。

你可能感兴趣的:(Mobile,Robot)