ROS navigation中各个package的作用 (一)

Navigation，翻译成中文即为导航的意思。机器人怎么导航？首先机器人需要有一张地图，知道自己处于哪个大区域；然后精确定位自己的位置；知道自己的准确位置之后，机器人需要知道自己要到达的目标点，怎么才能到达目标点；找到合适的路径，避开障碍物，到达目标。如下给出一张机器人导航的漫画图

---

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

---

AMCL

该包是一个二维移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡罗定位方法，该方法使用粒子滤波器根据已知地图 ( 先验地图 ) 跟踪机器人的姿态。

---

nav_core

该包为机器人的导航提供了公共的接口。当前，该包提供了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。这两个包的代码非常简单，不看也不会影响理解偏差，所以可以不用浪费时间看，当然因为简单要看也是分分钟的事。

---

costmap_2d

该包以层的概念来组织图层，move_base中默认的层有static_layer（通过订阅map_server的/map主题）来生成，obstacle_layer根据传感器获得的反馈来生成障碍物图层， inflation_layer 则是将前两个图层的信息综合进行缓冲区扩展。

---

move_base

表面的翻译就是: 移动底盘. 主要的职责：

1. 维护一张全局地图（基本上是不会更新的，一般是静态costmap类型），维护一张局部地图（实时更新，costmap类型），
2. 维护一个全局路径规划器global_planner完成全局路径规划的任务， 维护一个局部路径规划器base_local_planner完成局部路径规划的任务。
3. 然后提供一个对外的服务，负责监听nav_msgs::goal类型的消息，然后调动全局规划器规划全局路径，再将全局路径送进局部规划器，
4. 局部规划器结合周围障碍信息（从其维护的costmap中查询），全局路径信息，目标点信息采样速度并评分获得最高得分的轨迹（即是采样的最佳速度），
5. 返回速度值，由move_base发送Twist类型的cmd_vel消息上，从而控制机器人移动。完成导航任务。

---

最后：再来看navigation的这张图：