ros navigation-kinetic-stack 代碼閱讀

參考文章

navigation總覽
Navigation源碼閱讀之dwa_local_planner（DWA動態窗口法）
關於move_base的原理及應用
ROS navigation分析：navigation框架

0. nav_core

該包定義了整個導航系統關鍵包的接口函數，包括base_global_planner, base_local_planner以及recovery_behavior的接口。
裏面的函數全是虛函數，所以該包只是起到規範接口的作用，真正功能的實現在相應的包當中。

1. move_base

move_base可以說是整個navigation代碼閱讀的入口,整個導航系統的初始化也是在這裏進行,其主要功能如下

這個是整個navigation stack當中進行宏觀調控的看得見的手。
它主要乾的事情是這樣的：

• 維護一張全局地圖（基本上是不會更新的，一般是靜態costmap類型），維護一張局部地圖（實時更新，costmap類型）;
• 維護一個全局路徑規劃器global_planner完成全局路徑規劃的任務， 維護一個局部路徑規劃器base_local_planner完成局部路徑規劃的任務。
• 然後提供一個對外的服務，負責監聽nav_msgs::goal類型的消息，然後調動全局規劃器規劃全局路徑，再將全局路徑送進局部規劃器，
• 局部規劃器結合周圍障礙信息（從其維護的costmap中查詢），全局路徑信息，目標點信息採樣速度並評分獲得最高得分的軌跡（即是採樣的最佳速度），
• 然後返回速度值，由move_base發送Twist類型的cmd_vel消息上，從而控制機器人移動。完成導航任務。

move_base包運行後,我們僅需要在已經建立好的地圖上指定目標位置和方向, move_base便會根據傳感器(激光雷達,毫米波雷達等)獲得的新的環境信息,以及自身的位置姿態,規劃出一條全局的路徑,然後局部路勁規劃器根據機器人本身的狀態和運動特性, 速度,加速度,位姿等規劃出一系列的速度控制指令(並且進行速度模擬,碰撞檢查).

白色底色方框內就是move_base的內容

必要的輸入：

• goal ： 期望機器人在地圖中的目標位置
• tf ： 各個座標系之間的轉換關係。（具體/map frame --> /odom frame ，/odom frame --> /base_link frame）
• odom：根據機器人左右輪速度推算出的航向信息（即/odom座標系中機器人x,y座標以及航向角yaw，下面會具體介紹
• LaserScan：激光傳感器的信息，用於定位。（在這個系列教程中，我們沒有用到這個激光信息，而是在一個假的空白地圖上對機器人進行控制，並假定/map座標系和/odom座標系完全重合，在後面會有關於這兩個座標系的介紹）

可選的輸入：

• amcl：自適應蒙特卡羅定位，一種用於2D環境下移動機器人的概率統計定位方法
• map：地圖信息

輸出：

• cmd_vel：在cmd_vel這個主題上發佈Twist消息，這個消息包含的就是機器人的期望前進速度和轉向速度。
  move_base收到goal以後，將目標goal通過基於actionlib的client（客戶端）向服務器發送，服務器根據tf關係以及發佈的odom消息不斷反饋機器人的狀態（feedbackcall）到客戶端, 讓move_base做路徑規劃和控制twist。

1.1 MoveBase::MoveBase(tf::TransformListener& tf)

主要完成系統的默認參數的設置, 系統默認規劃器的選擇, 如全局規劃器與局部規劃器的選擇(下面會介紹).當然,這是代碼中的默認參數,也可以通過launch載入配置文件.xml進行修改.

<!-- The move_base node -->
  <include file="$(find rbx1_nav)/launch/move_base.launch" /> 

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

    //get some parameters that will be global to the move base node
    std::string global_planner, local_planner;
    private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
    private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
    private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
    private_nh.param("global_costmap/global_frame", global_frame_, std::string("/map"));
    private_nh.param("planner_frequency", planner_frequency_, 0.0);
    private_nh.param("controller_frequency", controller_frequency_, 20.0);
    private_nh.param("planner_patience", planner_patience_, 5.0);
    private_nh.param("controller_patience", controller_patience_, 15.0);
    private_nh.param("max_planning_retries", max_planning_retries_, -1);  // disabled by default

    private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
    private_nh.param("oscillation_distance", oscillation_distance_, 0.5);

1.2 MoveBase::executeCb(move_base_goal)

executeCb(move_base_goal)函數結構如下:

1. executeCb(move_base_goal)函數收到運動的目標後,會首先轉換到全局座標系下;
   然後開啓MoveBase::planThread線程, 用於全局路徑規劃;
2. 開啓costmap更新全局和局部costmap地圖;
3. 執行executeCycle() , 局部路徑規劃與控制部分

2. 全局路徑規劃

全局路徑規劃navigation中一共有三個實現,分別是global_planner, navfn,carrot_planner,都是實現當前點與目標點之間的路徑規劃, (在上面的MoveBase中可以看到默認使用的是navfn),其中global_planner, navfn內部都有Dijkstra算法和A*導航算法的實現.

planThread 中planner->makePlan(start, goal, plan)的全局路徑規劃代碼邏輯見下圖;

3. 局部路徑規劃

同樣局部路勁規劃也有兩種實現:一是航跡推算法（TrajectoryROS），一是動態窗口法(DWA),都使用了nav_core::BaseLocalPlanner父類提供的接口

3.1 executeCycle()中實現了以下功能:

• getRobotPose　//獲取機器人當前位置
• publishFeedback　as＿發佈位置狀態反饋
• 是否走的足夠遠重置oscillation (避免震盪)
• if(new_global_plan_) 使用最新的全局路徑
• move_base 狀態機: move_base 在三種狀態機中切換工作,PLANNING,CONTROLLING, CLEARING
  

3.2 tc_->computeVelocityCommands(cmd_vel) 計算局部控制速度併發布

• 1.將全局規劃從規劃幀轉換到評價地圖幀; transformGlobalPlan從全局路徑中截取一定距離閾值內的路徑作爲transformed_plan
• 2.prunePlan 將之前的global路徑點清除。（包括全局的global path和局部的global path）
• 3.robot_vel從發佈的odom中獲取速度信息
• 4.從全局plan中獲取目標點座標
• ５．與目標點距離是否小於閾值，或者已進入閾值內
• 6.findBestPath(global_pose, robot_vel, drive_cmds);
• 7.傳遞控制指令cmd_vel
• 8.發佈局部和全局plan

4. costmap_2d

ROS基礎教程–CostMap_2D包的一些理解這篇博客寫的十分詳細了,可以參考這篇博客.
costmap_2d主要是一個2D地圖的創建和管理, costmap_2d::Layer下有主要的四個子類,表示地圖中的四個子層,costmap初始化之後,便開始以update_frequency的頻率更新地圖,從外界傳感器獲得數據(如激光雷達,pointcloud),然後更新在地圖的相應層上.