ros navigation源码学习（1）nav_core

1 功能包的构建与管理
在ros的项目中常常是以catkin构建，因此在学习这个功能包之前先看一下package。xml和CmakeLists.txt文件，从这个当中可以看出项目的构建过程。
1.1 package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
    <name>nav_core</name>
    <version>1.17.3</version>
    <description>

        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.

    </description>
    <author>Eitan Marder-Eppstein</author>
    <author>contradict@gmail.com</author>
    <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
    <maintainer email="mfergs7@gmail.com">Michael Ferguson</maintainer>
    <maintainer email="ahoy@fetchrobotics.com">Aaron Hoy</maintainer>
    <license>BSD</license>
    <url>http://wiki.ros.org/nav_core</url>

    <buildtool_depend>catkin</buildtool_depend>

    <depend>costmap_2d</depend>
    <depend>geometry_msgs</depend>
    <depend>std_msgs</depend>
    <depend>tf2_ros</depend>

</package>

从这里可以明显看出nav_core依赖于costmap_2d、geometry_msgs、std_msgs和std_msgs功能包，每个功能包的作用是做什么的，大家可以了解一下，这里最重要的costmap_2d这个包，也就是代价地图，有兴趣的可以读一下这篇论文。
Layered Costmaps for Context-Sensitive Navigation
1.2 CmakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(nav_core)

find_package(catkin REQUIRED
        COMPONENTS
            std_msgs
            geometry_msgs
            tf2_ros
            costmap_2d
        )

#表示该包所有的头文件在include目录下
catkin_package(
    INCLUDE_DIRS
        include
    CATKIN_DEPENDS
            std_msgs
            geometry_msgs
            tf2_ros
            costmap_2d
)


## Install project namespaced headers
install(DIRECTORY include/${PROJECT_NAME}/
  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
  FILES_MATCHING PATTERN "*.h"
  PATTERN ".svn" EXCLUDE)

在CmakeLists.txt声明了需要的依赖包以及告知构建系统时在哪里寻找头文件
2 nav_core框架

可以看出nav_core被包含在move_base中，nav_core包含了三个接口global_planner和local_planner和recovery_behaviors。其输入是机器人的目标位姿,输出是机器人的速度命令。恢复行为则是根据代价地图产生，全局规划器则需要继承nav_core::BaseGlobalPlanner接口，当自定义自己的全局规划器时需要继承该类，而本地规划器则是需要继承nav_core::BaseLocalPlanner这个接口，同样当自定义的的本地规划器时需要继承该类。
3 源码学习
知道了nav_core的框架之后，再来看代码思路就会理清很多了。
3.1 base_global_planner.h

#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>
// 全局规划的接口,目前在navigation中集成的全局路径规划器
/*
  note:
  global_planner: 一个快速的，内插值的路径规划器，其能更灵活地代替navfn
  navfn: 一个基于栅格的全局路径规划器，利用导航函数来计算路径
  carrot_planner: 一个简单的全局规划器，其接收用户指定的全局点，并尝试让机器人尽可能靠近目标点（目标点可以为障碍）。

*/
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
       */
      // 函数重载
      //  note:move_base的void planThread()中被调用
      virtual bool makePlan(const geometry_msgs::PoseStamped& start, 
                            const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan,
                            double& cost)
      {
        cost = 0;
        // 调用上面的makePlan函数
        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(){}
    // 不能通过 BaseGlobalPlanner是实例化一个对象，只能用于派生类的调用
    protected:
      BaseGlobalPlanner(){}
  };
};  // namespace nav_core

#endif  // NAV_CORE_BASE_GLOBAL_PLANNER_H

可以看出就是在nav_core的命名空间中定义了一个纯虚类，其中声明了两个函数makePlan和initialize,
其中initialize用于全局规划器的初始化，定义其名字。最重要的是makePlan函数，根据给定的起始点和目标点生成路径，并对其进行了重载，加入代价的计算。可以发现成员函数全部都是虚函数，因此总的来说就是定义了接口。
3.2 base_local_planner.h

#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>
/*
  note:
  本地规划器的接口：目前在navigationg中实现的本地接口有：
  base_local_planner: 实现了DWA和Trajectory Rollout
  dwa_local_planner: 相对于上者的DWA，使用了更清晰、更容易的接口来实现了一个更容易明白的DWA模块，并为完整机器人提供了更灵活的的y轴。
  eband_local_planner: 
  teb_local_planner: 

*/
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
       */
      // note:move_base中的MoveBase::executeCycle函数中被调用，当全局路径规划成功就将其传递到局部路径规划器。
      // 然后在MoveBase::executeCycle中调用computeVelocityCommands计算出速度发送到cmd_vel话题。
      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

解读同全局规划器一样，这里最重要的是setPlan函数，会在move_base中被调用，computeVelocityCommands根据机器人的当前位置，朝向和速度，计算发送到底座的速度命令。
isGoalReached() 判断是否到达目标点。initialize初始化自己的本地规划器。
3.3 recovery_behavior.h

#ifndef NAV_CORE_RECOVERY_BEHAVIOR_H
#define NAV_CORE_RECOVERY_BEHAVIOR_H

#include <costmap_2d/costmap_2d_ros.h>
#include <tf2_ros/buffer.h>
// 恢复行为接口
/*
  note:目前在navigation中的恢复行为
  clear_costmap_recovery: 一定程度上恢复代价地图
  rotate_recovery: 360°旋转来尝试清出空间
*/
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
       */
      // 在move_base中调用
      virtual void runBehavior() = 0;

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

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

#endif  // NAV_CORE_RECOVERY_BEHAVIOR_H

恢复行为的接口，主要是runBehavior() 函数，在move_base被调用。
从上面的三个接口的头文件中可以看出，都是定义接口，都有成员函数会被move_base调用，因此，mose_base在导航中是一个非常重要的一个节点。
附
parameter_magic.h

#ifndef NAV_CORE_PARAMETER_MAGIC_H
#define NAV_CORE_PARAMETER_MAGIC_H

namespace nav_core
{

/**
 * @brief Load a parameter from one of two namespaces. Complain if it uses the old name.
 * @param nh NodeHandle to look for the parameter in
 * @param current_name Parameter name that is current, i.e. not deprecated
 * @param old_name Deprecated parameter name
 * @param default_value If neither parameter is present, return this value
 * @return The value of the parameter or the default value
 */
template<class param_t>
param_t loadParameterWithDeprecation(const ros::NodeHandle& nh, const std::string current_name,
                                     const std::string old_name, const param_t& default_value)
{
  param_t value;
  if (nh.hasParam(current_name))
  {
    nh.getParam(current_name, value);
    return value;
  }
  if (nh.hasParam(old_name))
  {
    ROS_WARN("Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
    nh.getParam(old_name, value);
    return value;
  }
  return default_value;
}

/**
 * @brief Warn if a parameter exists under a deprecated (and unsupported) name.
 *
 * Parameters loaded exclusively through dynamic reconfigure can't really use loadParamWithDeprecation.
 */
void warnRenamedParameter(const ros::NodeHandle& nh, const std::string current_name, const std::string old_name)
{
  if (nh.hasParam(old_name))
  {
    ROS_WARN("Parameter %s is deprecated (and will not load properly). Use %s instead.", old_name.c_str(), current_name.c_str());
  }
}

}  // namespace nav_core

#endif  // NAV_CORE_PARAMETER_MAGIC_H

这个跟代码逻辑没有什么关系，知识ros中参数的设定，推荐使用新的参数。
总结：nav_core功能包定义了三个类的接口，当使用自己的规划器时，可以继承相应的父类，然后以插件的形式登记在ros系统中被调用。