Navigation（二）nav_core源码详解

 

一、写的不错的博客

　　这篇文章简单梳理了nav_core和move_base的关系，https://blog.csdn.net/u013834525/article/details/85545321

 

二、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>

namespace nav_core {
  //全局规划器的接口
  class BaseGlobalPlanner{
    public:
      /**
       * @brief 给个目标点，规划个路径
       * @param start 起始点
       * @param goal 目标点
       * @param plan 路径
       * @return 路径被找到返回true
       */
      virtual bool makePlan(const geometry_msgs::PoseStamped& start, 
          const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) = 0;

      /**
       * @brief 给个目标点，规划个路径
       * @param start 起始点
       * @param goal 目标点
       * @param plan 路径
       * @param cost 路径计算出的代价
       * @return 路径被找到就返回true
       */
      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  全局规划器的初始化函数
       * @param  name 规划器的名字
       * @param  costmap_ros 指向规划所使用的指向costmap的指针
       */
      virtual void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) = 0;

      //虚析构函数
      virtual ~BaseGlobalPlanner(){}

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

#endif  // NAV_CORE_BASE_GLOBAL_PLANNER_H

 

三、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>

namespace nav_core {
  /**
   * @class BaseLocalPlanner
   * @brief 局部规划接口
   */
  class BaseLocalPlanner{
    public:
      /**
       * @brief  被传入机器人当前位置、方向和速度，计算相对于base坐标系的速度
       * @param cmd_vel 机器人的速度
       * @return 有速度指令，则为true
       */
      virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;

      /**
       * @brief  检查局部规划器的目标位姿是否到达
       * @return 如果到达了返回true
       */
      virtual bool isGoalReached() = 0;

      /**
       * @brief  设置局部规划器正在执行的路径
       * @param plan 即将被传递给局部规划器的路径
       * @return 路径被成功标记则返回true
       */
      virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;

      /**
       * @brief  构建局部规划器
       * @param name 局部规划器的名称
       * @param tf tf的指针
       * @param costmap_ros 局部规划器的costmap
       */
      virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;

      //虚析构函数
      virtual ~BaseLocalPlanner(){}

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

#endif  // NAV_CORE_BASE_LOCAL_PLANNER_H

 

四、parameter_magic.h

#ifndef NAV_CORE_PARAMETER_MAGIC_H
#define NAV_CORE_PARAMETER_MAGIC_H

namespace nav_core
{

/**
 * @brief 加载参数服务器
 * @param nh 加载参数服务器的句柄
 * @param current_name 当前参数名
 * @param old_name 已经弃用的参数名
 * @param default_value 如果参数都不存在，则返回此值
 * @return 参数值或者默认值
 */
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;
}

//如果参数以不支持的名称出现则警告。参数服务器将专门加载不能真正使用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

 

五、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>

namespace nav_core {
  //为导航的Recovery指令提供接口，必须遵循
  class RecoveryBehavior{
    public:
      /**
       * @brief  初始化函数
       * @param tf 指向tf监听者的指针
       * @param global_costmap 指向全局costmap的指针
       * @param local_costmap 指向局部costmap的指针
       */
      virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap) = 0;

      //运行Recovery指令
      virtual void runBehavior() = 0;

      //虚析构函数
      virtual ~RecoveryBehavior(){}

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

#endif  // NAV_CORE_RECOVERY_BEHAVIOR_H