ROS源码解读(二)--全局路径规划

博客转载自：https://blog.csdn.net/xmy306538517/article/details/79032324

ROS中，机器人全局路径规划默认使用的是navfn包 ，move_base的默认参数中可以找到

base_global_planner  (`string`, default: "navfn/NavfnROS")

navigation的源代码中还有一个global_planner的包里面已经有了A*,Dijkstra等算法的实现，但是navfn的源程序中也有这两个算法的实现，默认根本就没用到global_planner这个文件夹下的源程序。那么为什么有两个用于全局导航的包在ROS里面？它们又是什么关系呢？是因为早期的开发中是用navfn包做导航的并且默认使用Dijkstra做全局路径规划，并且有A * 的代码，那为什么没有使用A * 呢？ 因为这里的A*算法存在bug，没人有时间去弄！直到13年David Lu 才完成了这部分工作，重新发布了global_planner包，修改好的代码封装性更强，更清晰明了。因此，也可以认为global_planner是navfn的升级版替代者。那么问题来了David Lu为什么没用global_planner替代掉navfn？为了兼容呗。因此两个包都在，并且默认的是navfn，也就是没用global_planner。

如何使用global_planner包

只要将move_base的参数base_global_planner用global_planner/GlobalPlanner代替就行

将
<param name="base_global_planner"value="navfn/NavfnROS"/>
改为
<param name="base_global_planner"value="global_planner/GlobalPlanner"/>

move_base是如何调用各种global或者local planner包

ROS官方wiki教程里就提到过可以扩展自己的路径规划算法，思路是使用ROS的插件机制

1. 在自己写的global或者local planner算法里开头加上一句特定的程序(PLUGINLIB_EXPORT_CLASS(…..))就能注册插件机制 
2. 在xx_plugin.xml等文件里描述下这个插件 
3. 在package.xml显式的表明这个插件用来通知ROS我们将使用它

详细具体过程见官方wiki(ROS的插件机制)。 做完以上工作，我们就可以像上面一样将其用参数的形式直接传过去了

插件是如何工作的？(参见下文的补充说明)

以上插件的工作方式就告诉我们必须按照ROS提供的模板去实现，这正是nav_core这个包存在的意义。 在navigation的源代码中你会看到这个nav_core包中仅仅只有几个头文件，正是这些头文件提供了多个模板：

nav_core::BaseGlobalPlanner，
nav_core::BaseLocalPlanner，
nav_core::RecoveryBehavior

在官方的wiki文档里可以看到他们的相关介绍。所以按照这些模板的标准形式去写自己的planner算法就行了。了解了这些我们去看别人写的各种planner的插件就很简单，除了global_planner包和dwa_local_planner包，还有很多其他算法如sbql global planner，eband local planner，carrot_planner等等。关于具体的路径规划算法有很多可以从现有的两个入手先看看A*、Dijkstra算法的具体实现

global_planner/GlobalPlanner的实现

[global_planner/GlobalPlanner源码分析](http://blog.csdn.net/u013158492/article/details/50504963）

---

补充：插件是如何工作的？

原理 
- 要了解pluginlib的工作原理，让我们考虑一个小例子 
- 首先，假设存在包含多边形基类（“polygon_interface_package”）的ROS 包。 
- 也可以说有两种不同类型的多边形的：rectangle_plugin包（矩形）和triangle_plugin包（三角形） 
- rectangle_plugin和triangle_plugin使用都是在package.xml文件中包含指定的export项 
- 这告诉rosbuild构建系统，想在polygon_interface_package包里提供polygon类的插件。 
- 增加的export项，事实上是在build/packaging系统里注册这些类 
- 就是说可以通过rospack查询到所有可用的polygon类，它能返回所有可用的类列表，这里主要是rectangle和triangle。

图示类结构

1）注册/导出插件

• 为了允许类被动态加载，它必须被标记为导出类。
• 这是通过特殊宏PLUGINLIB_EXPORT_CLASS来完成的。
• 这个宏可以放在构成插件库的任何源（.cpp）文件中，但通常放在导出类的.cpp文件的末尾。
• 对于上面的示例，我们可能在包’example_pkg’中创建一个class_list.cpp文件
• 如下所示，并将其编译到librectangle库中：

#include <pluginlib/class_list_macros.h>
#include <polygon_interface_package/polygon.h>
#include <rectangle_package/rectangle.h>

//Declare the Rectangle as a Polygon class
PLUGINLIB_EXPORT_CLASS(rectangle_namespace::Rectangle, polygon_namespace::Polygon)

2）插件描述文件

• 该插件描述文件是用于存储所有关于插件的重要信息的XML文件
• 它包含有关插件所在的库的信息，插件的名称，插件的类型等
• 如果我们考虑上面讨论的rectangle_plugin包，插件描述文件（例如rectangle_plugin.xml）将看起来像这样：
• 我们需要这个文件除了代码宏，允许ROS系统自动发现，加载和解释插件。

<library path="lib/librectangle">
  <class type="rectangle_namespace::Rectangle" base_class_type="polygon_namespace::Polygon">
  <description>
  This is a rectangle plugin
  </description>
  </class>
</library>

（3）注册插件

• 为了让pluginlib查询跨所有ROS包的系统上的所有可用插件，每个包必须显式指定它导出的插件，以及哪些包库包含这些插件。
• 一个插件提供者必须在其package.xml中的export块指向它的插件描述文件。
• rectangle_plugin为例：

<export>
  <polygon_interface_package plugin="${prefix}/rectangle_plugin.xml" />
</export>

• 重要说明：为了使上述export命令正常工作，提供包必须直接依赖于包含插件接口的包。
• 例如，rectangle_plugin必须在其catkin/package.xml中具有以下行：

<build_depend>polygon_interface_package</build_depend>
<run_depend>polygon_interface_package</run_depend>

4）查询插件

• 可以通过rospack查询ROS包系统，以查看任何给定包可用的插件。

rospack plugins --attrib=plugin nav_core

5）使用插件

• pluginlib在class_loader.h头文件中提供了一个ClassLoader类，使得它能够快速和容易地使用提供的类。
• 有关此工具的代码级API的详细文档，请参阅pluginlib::ClassLoader文档。
• 例如一个使用ClassLoader在一些使用多边形的代码中创建矩形实例的简单示例：

#include <pluginlib/class_loader.h>
#include <polygon_interface_package/polygon.h>

//... some code ...

pluginlib::ClassLoader<polygon_namespace::Polygon> poly_loader("polygon_interface_package", "polygon_namespace::Polygon");

try
{
  boost::shared_ptr<polygon_namespace::Polygon> poly = poly_loader.createInstance("rectangle_namespace::Rectangle");

  //... use the polygon, boost::shared_ptr will automatically delete memory when it goes out of scope
}
catch(pluginlib::PluginlibException& ex)
{
  //handle the class failing to load
  ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
}

编辑整理自: 
1. 知行合一博客 
2. 为什么navfn是用 dijkstra? 
3. global_planner包和navfn包的关系？ 
4. ROS插件机制 
5. nav_core 官方wiki 
6. navfn具体代码的一些问题