ROS navigation stack 各個package的作用

nav_core

---

This package provides common interfaces for navigation specific robot actions. Currently, this package provides the BaseGlobalPlanner, BaseLocalPlanner, and RecoveryBehavior interfaces, which can be used to build actions that can easily swap their planner, local controller, or recovery behavior for new versions adhering to the same interface.

該包爲機器人的導航提供了公共的接口。當前，該包提供了BaseGlobalPlannert(全局路徑規劃)，BaseLocalPlanner(局部路徑規劃)，以及RecoveryBehavior(恢復行爲)的接口，它能夠在新版本中秉承相同的接口去創建和更換他們的全局規劃，局部控制以及恢復行爲。

---

global_planner、navfn、carrot_planner

---

首先看一下nav_core中關於BaseGlobalPlannert接口的定義，如下：

#ifndef NAV_CORE_BASE_GLOBAL_PLANNER_H
#define NAV_CORE_BASE_GLOBAL_PLANNER_H

#include <geometry_msgs/PoseStamped.h>
#include <costmap_2d/costmap_2d_ros.h>

namespace nav_core {
  /**
   * @class BaseGlobalPlanner
   * @brief Provides an interface for global planners used in navigation. All global planners written as plugins for the navigation stack must adhere to this interface.
   */
  class BaseGlobalPlanner{
    public:
      /**
       * @brief Given a goal pose in the world, compute a plan
       * @param start The start pose 
       * @param goal The goal pose 
       * @param plan The plan... filled by the planner
       * @return True if a valid plan was found, false otherwise
       */
      virtual bool makePlan(const geometry_msgs::PoseStamped& start,
          const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) = 0;

      /**
       * @brief Given a goal pose in the world, compute a plan
       * @param start The start pose 
       * @param goal The goal pose 
       * @param plan The plan... filled by the planner
       * @param cost The plans calculated cost
       * @return True if a valid plan was found, false otherwise
       */
      virtual bool makePlan(const geometry_msgs::PoseStamped& start,
                            const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan,
                            double& cost)
      {
        cost = 0;
        return makePlan(start, goal, plan);
      }

      /**
       * @brief  Initialization function for the BaseGlobalPlanner
       * @param  name The name of this planner
       * @param  costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
       */
      virtual void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) = 0;

      /**
       * @brief  Virtual destructor for the interface
       */
      virtual ~BaseGlobalPlanner(){}

    protected:
      BaseGlobalPlanner(){}
  };
};  // namespace nav_core

這三個package都是繼承並實現了nav_core中定義的BaseGlobalPlannert類(全局路徑規劃)，前兩者都包含了A*和Dijkstra算法的實現，可以說是global_planner是navfn的升級版，代碼結構更優，更清晰。關於global_planner源碼分析可以參考這篇博文（https://blog.csdn.net/u011608180/article/details/94002838）。

當前ros版本中默認使用的是navfn。carrot_planner是另外一種全局的路徑規劃，暫時不解釋。

---

base_local_planner、dwa_local_planner

---

這兩個package是關於局部路徑規劃，機器人運動速度的具體生成在此包當中完成。目前有兩種局部路徑規劃算法實現，一是航跡推算法（TrajectoryROS），一是動態窗口法(DWA)，該包內部的默認實現是航跡推算法，但是留出了DWA的定義接口，DWA的實現在dwa_local_planner中。兩者都是繼承並實現了nav_core中定義的BaseLocalPlanner類，定義如下：

#ifndef NAV_CORE_BASE_LOCAL_PLANNER_H
#define NAV_CORE_BASE_LOCAL_PLANNER_H

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <tf2_ros/buffer.h>

namespace nav_core {
  /**
   * @class BaseLocalPlanner
   * @brief Provides an interface for local planners used in navigation. All local planners written as plugins for the navigation stack must adhere to this interface.
   */
  class BaseLocalPlanner{
    public:
      /**
       * @brief  Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
       * @param cmd_vel Will be filled with the velocity command to be passed to the robot base
       * @return True if a valid velocity command was found, false otherwise
       */
      virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;

      /**
       * @brief  Check if the goal pose has been achieved by the local planner
       * @return True if achieved, false otherwise
       */
      virtual bool isGoalReached() = 0;

      /**
       * @brief  Set the plan that the local planner is following
       * @param plan The plan to pass to the local planner
       * @return True if the plan was updated successfully, false otherwise
       */
      virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;

      /**
       * @brief  Constructs the local planner
       * @param name The name to give this instance of the local planner
       * @param tf A pointer to a transform listener
       * @param costmap_ros The cost map to use for assigning costs to local plans
       */
      virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;

      /**
       * @brief  Virtual destructor for the interface
       */
      virtual ~BaseLocalPlanner(){}

    protected:
      BaseLocalPlanner(){}
  };
};  // namespace nav_core

#endif  // NAV_CORE_BASE_LOCAL_PLANNER_H

---

rotate_recovery、clear_costmap_recovery

---

這兩個包都繼承自nav_core中定義的recovery_behavior類， 具體實現是：當導航發現無路可走的時候，機器人在原地旋轉，並更新周圍障礙物信息看是否有動態障礙物運動開，如果能夠找到路就繼續走。

#ifndef NAV_CORE_RECOVERY_BEHAVIOR_H
#define NAV_CORE_RECOVERY_BEHAVIOR_H

#include <costmap_2d/costmap_2d_ros.h>
#include <tf2_ros/buffer.h>

namespace nav_core {
  /**
   * @class RecoveryBehavior
   * @brief Provides an interface for recovery behaviors used in navigation. All recovery behaviors written as plugins for the navigation stack must adhere to this interface.
   */
  class RecoveryBehavior{
    public:
      /**
       * @brief  Initialization function for the RecoveryBehavior
       * @param tf A pointer to a transform listener
       * @param global_costmap A pointer to the global_costmap used by the navigation stack 
       * @param local_costmap A pointer to the local_costmap used by the navigation stack 
       */
      virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap) = 0;

      /**
       * @brief   Runs the RecoveryBehavior
       */
      virtual void runBehavior() = 0;

      /**
       * @brief  Virtual destructor for the interface
       */
      virtual ~RecoveryBehavior(){}

    protected:
      RecoveryBehavior(){}
  };
};  // namespace nav_core

#endif  // NAV_CORE_RECOVERY_BEHAVIOR_H

在move_base當中，默認使用的是recovery_behavior，先進行一次旋轉，然後進行一次小半徑的障礙物更新，然後再進行一次旋轉，再進行一次大範圍的障礙物更新，每進行一次recovery_behavior，都會重新嘗試進行一次局部尋路，如果沒找到，纔會再執行下一個recovery_behavior。這兩個包的代碼非常簡單，不看也不會影響理解偏差，所以可以不用浪費時間看，當然因爲簡單要看也是分分鐘的事。

---

map_server

---

該package的主要功能是通過map_server節點讀取yaml配置文件、加載pgm地圖文件，發佈static_map service，發佈map_metadata 和 map 的topic。命令行如下：

rosrun map_server map_server 1.yaml

另外map_server還提供地圖保存的功能，floor 1爲要保存的地圖名稱，命令如下：

rosrun map_server map_saver -f floor1

---

costmap_2d

---

該包以層的概念來組織圖層，move_base中默認的層有static_layer（通過訂閱map_server的/map主題）來生成，obstacle_layer根據傳感器獲得的反饋來生成障礙物圖層， inflation_layer 則是將前兩個圖層的信息綜合進行緩衝區擴展。

---

move_base

---

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

---

最後：再來看navigation的這張圖：