ros navigation分析-2

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

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9

這部分是構造函數的初始化列表，可以看到幾個重要的參數：

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

• 1
• 2
• 3
• 4

• 1
• 2
• 3
• 4

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

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13

進入構造函數，第一句：

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

• 1

• 1

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

• 1
• 2
• 3

• 1
• 2
• 3

注意，這三個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));

• 1
• 2

• 1
• 2

接下來設置一堆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_);

• 1
• 2

• 1
• 2

開啓costmap基於傳感器數據的更新：

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

• 1
• 2
• 3

• 1
• 2
• 3

載入recovery behavior，這部分是狀態機的設計問題，可以採用默認的設定，也可以用戶指定：

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

• 1
• 2

• 1
• 2

然後開啓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;
      }

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8

因此線程繼續運行依賴於變量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();

• 1
• 2
• 3
• 4
• 5

• 1
• 2
• 3
• 4
• 5

然後進入死循環：首先判斷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;
    }

• 1
• 2
• 3
• 4
• 5
• 6
• 7

• 1
• 2
• 3
• 4
• 5
• 6
• 7

然後檢查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};

• 1
• 2

• 1
• 2

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

• 1

• 1

然後由baseLocalPlanner計算cmd_vel:

if(tc_->computeVelocityCommands(cmd_vel)){

• 1

• 1

成功則可以控制地盤了，如果失敗，檢查是否超時，如果超時：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;
}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16

moveBase基本上主要內容就是這些，其他未分析的函數都比較簡單。但是在moveBase的實現中，主要依賴於costmap_2d 以及global planner 和local planner。還有pluginlib的使用，簡化了各種具體實現，因此在接下來的文章中着重分析這幾部分。