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的这张图：