ROS naviagtion analysis: move

ROS naviagtion analysis: move_base

博客转载自：https://blog.csdn.net/u013158492/article/details/50483123

这是navigation的第一篇文章，主要通过分析ROS代码级实现，了解navigation。本文从move_base入手。

机器人导航主要框架

Navigation Stack主要组成部分：move_base

用户调用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<nav_core::BaseGlobalPlanner> bgp_loader_; 
pluginlib::ClassLoader<nav_core::BaseLocalPlanner> 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<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();

注意，这三个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<geometry_msgs::PoseStamped>& plan){}
首先lock 住global costmap：
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> 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<boost::mutex> 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<boost::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<geometry_msgs::PoseStamped>& 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<boost::mutex> lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
resetState();
return true;
}

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