ROS : Navigation - move_base 功能包源碼分析

- move_base 的整體架構
- move_base 的輸入輸出
- move_base 中各函數的主要作用

move_base 的整體架構

move_base 是一個開源 2D 移動機器人導航包，實作機器人自主導航到給定目标點的功能。 move_base 提供配置、運作以及跟導航棧互動的接口，基本架構如下圖所示，其核心包含了 3 大功能子產品，并以插件的形式加載到 move_base 中：

全局路徑規劃 nav_core::BaseGlobalPlanner
局部路徑規劃 nav_core::BaseLocalPlanner
恢複機制 nav_core::RecoveryBehavior

ROS : Navigation - move_base 功能包源碼分析

使用者可以自定義任意的全局路徑規劃器、局部路徑規劃器，隻需要将其跟 nav_core::BaseGlobalPlanner 、 nav_core::BaseLocalPlanner 接口對接上即可。這兩個接口的實作在 nav_core 包。

move_base 的輸入輸出

move_base 的輸入是一個 goal (geometry_msgs/PoseStamped) ，輸出是一系列 command (geometry_msgs/Twist) 。詳細一點來說，就是在已知環境地圖和機器人在地圖中位置的情況下，利用地圖和傳感器資料處理得到的 global costmap 和 local costmap ，調用全局路徑規劃器得到一條全局路徑，調用局部路徑規劃器使機器人在移動過程中能在大緻沿着路徑前進并動态規避障礙物，整個行進過程中還要處理一部分異常情況即 recovery_behaviors 。

ROS : Navigation - move_base 功能包源碼分析

move_base 通過訂閱 /move_base_simple/goal 或者 /move_base/goal 話題消息接收目标點。釋出一個目标點的兩種方式示例如下：

[email protected]:~/catkin_ws# rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped "
header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: "map"
pose:
  position:
    x: 2.0
    y: -1.0
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.471599513115
    w: 0.881812848188
"

[email protected]:~/catkin_ws# rostopic pub /move_base/goal move_base_msgs/MoveBaseActionGoal "
header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  target_pose:
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs: 0
      frame_id: "map"
    pose:
      position:
        x: 2.0
        y: -1.0
        z: 0.0
      orientation:
        x: 0.0
        y: 0.0
        z: 0.474801462745
        w: 0.880092933147
"

move_base 中各函數的主要作用

MoveBase ：構造函數
goalCB ：将 goal 轉換為 move_base_msgs::MoveBaseActionGoal 類型
clearCostmapWindows ：清空機器人周圍的正方形空間
clearCostmapsService ：将 costmap 重置為 unknow
planService ：服務 make_plan 的回調函數。接受 Service 的消息後，開始路徑規劃，得到 global_plan 的路徑，跟 as_ 得到的 action 一樣，都會 make_plan ，但是不做局部路徑規劃（隻是提供了個全局路徑規劃的接口）
~MoveBase ：析構函數
makePlan ：擷取機器人的位姿作為起點，然後調用全局規劃器的 makePlan 傳回規劃路徑，存儲在 planner_plan_
publishZeroVelocity ：釋出速度為 0 的 cmd_vel
isQuaternionValid ：驗證是否是四元數
goalToGlobalFrame ：将 goal 轉化為 geometry_msgs::PoseStamped 格式的 goal_pose_msg
wakePlanner ：通過 boost thread 的條件變量 planner_cond_ 喚醒線程 planThread
planThread ：是全局路徑規劃的線程，調用全局路徑規劃擷取路徑，同時保證規劃的周期性以及規劃逾時清除 goal
executeCb ：是 move_base 接收到新目标時調用的函數，執行局部路徑規劃和導航
distance ：計算 p1 和 p2 兩個點之間的距離
executeCycle ：是局部路徑規劃的核心函數
loadRecoveryBehaviors ：加載 Recovery 的行為
loadDefaultRecoveryBehaviors ：加載預設的 Behaviors
resetState

ROS : Navigation - move_base 功能包源碼分析

ROS : Navigation - move_base 功能包源碼分析