ROS Navigation之nav_core完全详解

分类: ROS Navigation源码完全详解

Posted by 白夜行的狼 on December 29, 2018

0. 写在最前面

本文持续更新地址:https://haoqchen.site/2018/12/29/nav-core-code/

本文将介绍自己在看ROS的Navigation stack中的nav_core包源代码时的一些理解。作者的ROS版本是indigo,nav_core版本是1.12.13。如有错误,欢迎在评论中指正。

如果觉得写得还不错,就请收藏一下啦~~~也可以找一下我写的其他包的源码解读来看一下。关注一下我的专栏什么的。

你的Star是作者坚持下去的最大动力哦~~~

1. package.xml与CMakeLists.txt

package说这个包目前提供了三个导航用到的,机器人特定动作的一般接口,分别是BaseGlobalPlanner, BaseLocalPlanner, RecoveryBehavior,即全局路径规划器、局部路径规划器和恢复行为规划器。接口的作用一般是为了统一不同规划器的输出、输入,使得后续程序可以适应不同规划器。

CMakeListscatkin_package的作用是,当其他包find_package(nav_core)时,应该迭代依赖std_msgs, geometry_msgs, tf, costmap_2d四个包,并且include的路径。其实整个包只包含了三个文件:base_global_planner.h、base_local_planner.h、recovery_behavior.h。这三个包分别定义了三个纯虚类,如下图所示:

1. base_global_planner.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
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  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(){}
  };
};

声明了一个纯虚的类,用户在实现全局规划器时需继承自该类,并给出这几个函数的具体实现。其中makePlan函数在move_basevoid planThread()中被调用,用于根据参数一(起始点)以及参数二(终点)来规划路径,并通过引用的形式返回给参数三。

目前Navigation Stack实现的全局路径规划器有:

  • global_planner: 一个快速的,内插值的路径规划器,其能更灵活地代替navfn
  • navfn: 一个基于栅格的全局路径规划器,利用导航函数来计算路径
  • carrot_planner: 一个简单的全局规划器,其接收用户指定的全局点,并尝试让机器人尽可能靠近目标点(目标点可以为障碍)。

2. base_local_planner.h

同上,定义了局部路径规划器的纯虚类

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
namespace nav_core {
 
  class BaseLocalPlanner{
    public:
      virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
      
      virtual bool isGoalReached() = 0;
      
      virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
     
      virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;

      virtual ~BaseLocalPlanner(){}
    protected:
      BaseLocalPlanner(){}
  };
};

其中setPlan函数在move_base中的MoveBase::executeCycle函数中被调用,当全局路径规划成功就将其传递到局部路径规划器。

然后在MoveBase::executeCycle中调用computeVelocityCommands计算出速度发送到cmd_vel话题。

目前Navigation Stack实现的局部路径规划器有:

  • base_local_planner: 实现了DWA和Trajectory Rollout
  • dwa_local_planner: 相对于上者的DWA,使用了更清晰、更容易的接口来实现了一个更容易明白的DWA模块,并为完整机器人提供了更灵活的的y轴。
  • eband_local_planner: 实现了Elastic Band method on the SE2 manifold,我也不知道是啥。
  • teb_local_planner: 实现了Timed-Elastic-Band method for online trajectory optimization,也没听过。

3. recovery_behavior.h

同上

1
2
3
4
5
6
7
8
9
10
11
12
namespace nav_core {
  class RecoveryBehavior{
    public:
      virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap) = 0;

      virtual void runBehavior() = 0;

      virtual ~RecoveryBehavior(){}
    protected:
      RecoveryBehavior(){}
  };
};

move_base中多处用到了runBehavior()这个函数,一般是出现了异常情况,需要恢复,或者不知道干啥的时候,具体看文章。

目前Navigation Stack实现的恢复路径规划器有:

  • clear_costmap_recovery: 一定程度上恢复代价地图
  • rotate_recovery: 360°旋转来尝试清出空间

参考

nav_core官网


喜欢我的文章的话Star一下呗Star

版权声明:本文为白夜行的狼原创文章,未经允许不得以任何形式转载