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具體代碼的一些問題