ros自定义全局路径规划器并内置到ros工作空间

做个正直的人

最近做移动机器人的运动规划，需要用到分层自适应路径规划算法。ROS内部已经集成了多种全局路径规划器（BaseGlobalPlanner）和局部路径规划器（BaseLocalPlanner），包括用A*算法、Dijstra算法、DWA动态窗口法等。这些都封装在ros的move_base节点中。一开始我想自己实现一个move_base节点，后来想了想，还是算了吧。搞不赢。

所以就用插件的形式，做了一个全局路径规划器的插件（plugin），这样就可以用ros原本的导航框架了。省下不小的气力。
首先应该明白，我们自定义的全局路径规划器必须遵守ROS的nav_core::BaseGlobalPlanner C++ interface规范，这是ros的一套全局路径规划器都必须遵守的C++接口规范。其实说白了，就是你的规划器必须继承自nav_core::BaseGlobalPlanner 类，否则ROS是不认的。

1、定义头文件

为了方便，我们可以直接拿carrot_planner规划器的头文件来用，先搭起一个架子来。等到具体实现自己的算法的时候对头文件进行具体编写。说实话，如果我们还没有实现自己的规划算法，我又怎么会直到自己的头文件应该怎么写呢？应该声明哪些函数、哪些变量呢？
比如我们把我们的global_planner.h文件写成这个样子

/** include the libraries you need in your planner here */
 /** for global path planner interface */
 #include <ros/ros.h>
 #include <costmap_2d/costmap_2d_ros.h>
 #include <costmap_2d/costmap_2d.h>
 #include <nav_core/base_global_planner.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <angles/angles.h>
 #include <base_local_planner/world_model.h>
 #include <base_local_planner/costmap_model.h>
 
 using std::string;
 
 #ifndef GLOBAL_PLANNER_CPP
 #define GLOBAL_PLANNER_CPP
 
 namespace global_planner {
 
 class GlobalPlanner : public nav_core::BaseGlobalPlanner {
 public:
 
  GlobalPlanner();
  GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
 
  /** overridden classes from interface nav_core::BaseGlobalPlanner **/
  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
  bool makePlan(const geometry_msgs::PoseStamped& start,
                const geometry_msgs::PoseStamped& goal,
                std::vector<geometry_msgs::PoseStamped>& plan
               );
  };
 };
 #endif

头文件中有几个比较重要的点，一是最好为自己的算法单独开辟一块命名空间，这样可以把自己的算法很好的区分出来，这是一个很好的编程习惯。二是我们的路径规划算法必须继承nav_core::BaseGlobalPlanner基类，并实现其中makeplan函数。这意味着我们必须遵守ros的路径规划器的C++接口规范。接下来看看头文件都干了啥。

 #include <ros/ros.h>
 #include <costmap_2d/costmap_2d_ros.h>
 #include <costmap_2d/costmap_2d.h>
 #include <nav_core/base_global_planner.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <angles/angles.h>
 #include <base_local_planner/world_model.h>
 #include <base_local_planner/costmap_model.h>

引用ros.h这是必须的。 <costmap_2d/costmap_2d_ros.h>和<costmap_2d/costmap_2d.h>是为了获得地图作为输入。<nav_core/base_global_planner.h>是为了继承nav_core::BaseGlobalPlanner类以便实现我们自己的规划器。

GlobalPlanner();
  GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
 
  /** overridden classes from interface nav_core::BaseGlobalPlanner **/
  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
  bool makePlan(const geometry_msgs::PoseStamped& start,
                const geometry_msgs::PoseStamped& goal,
                std::vector<geometry_msgs::PoseStamped>& plan
               );

是我们继承自nav_core::BaseGlobalPlanner的成员函数，其中makeplan是我们实现自己的路径规划算法的函数和接口。

2、类实现

我们需要把继承过来的类进行实现，也就是写个cpp文件。并在头文件引用完了之后写上这一行代码。

PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

其中global_planner是我们定义的命名空间的名字，GlobalPlanner是我们要实现的全局路径规划器。这表明你的我们的Planner是继承的nav_core::BaseGlobalPlanner，并且是将来要被注册的。为此，必须在cpp文件中引用如下库。

#include <pluginlib/class_list_macros.h>

3、写好自己的cMakelist.txt文件

这一个文件非常的重要，一些小小的细节可能让我们的规划器根本跑不起来。
为了方便，可以直接把carrot_planenr的cMakelist.txt文件拷贝过来，把其中一些参数修改一下，比如修改项目名称，加上一些库引用。随后加上下面这一行。

add_library(my_planner_lib src/path_planner/src/my_planner.cpp)

这一行命令会在编译之后的devel/lib/目录下生成一个libglobal_planner_lib.so文件，这实际上是一个动态连接库，来供ros调用。

4、为我们的规划器写一个描述文件

也就是.xml文件，这句话告诉ros去哪里找我们刚才生成的那个动态连接库，ros实际上运行的是我们的动态连接库文件。不妨把描述文件命名为global_planner_plugin.xml。文件内容如下

<library path="lib/libglobal_planner_lib">
  <class name="global_planner/GlobalPlanner" type="global_planner::GlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>This is a my global planner plugin for ROS.</description>
  </class>
 </library>

5、修改package.xml文件

仅仅写一个规划器的描述文件是不够的，我们还必须显式的告诉程序，我生成了一个插件哦。不然的话，ros是找不到我们的插件的。比如虽然你知道你自己叫张三，但是别人不知道你是张三啊，别人找张三就是找不到，你必须告诉别人你就是张三，这样别人找张三的时候才能找到你。其实，这句话就是显示的声明我导出了一个插件。为此在package.xml文件中添加如下命令：

<export>
  <nav_core plugin="${prefix}/my_planner_plugin.xml" />
</export>

此外，务必保证package.xml中包含这两行：

<build_depend>nav_core</build_depend>
<run_depend>nav_core</run_depend>

6、运行catkin_make进行编译，如果有错误就修改

7、配置环境

最后还要为我们的插件配置环境。告诉电脑有这个东西，不然ros就找不到这个插件在哪里。
运行 gedit .bashrc命令，
在文件末尾添加 source ~/catkin_ws/devel/setup.bash，保存后退出。
运行source .bashrc使更改后的环境生效。
或者在catkin_ws/下运行source devel/setup.bash也可以，但是这只在当前窗口有效。

8、检查查件是否注册成功

运行如下命令查询当前可用的插件，不出意外，应该就能看到我们自己的写的路径规划器了。

rospack plugins --attrib=plugin nav_core

比如我自己实现了一个informed RRT*的规划器，就会查到如下结果：

rotate_recovery /opt/ros/hydro/share/rotate_recovery/rotate_plugin.xml
navfn /opt/ros/hydro/share/navfn/bgp_plugin.xml
base_local_planner /opt/ros/hydro/share/base_local_planner/blp_plugin.xml
move_slow_and_clear /opt/ros/hydro/share/move_slow_and_clear/recovery_plugin.xml
dwa_local_planner /opt/ros/hydro/share/dwa_local_planner/blp_plugin.xml
clear_costmap_recovery /opt/ros/hydro/share/clear_costmap_recovery/ccr_plugin.xml
carrot_planner /opt/ros/hydro/share/carrot_planner/bgp_plugin.xml
informed_rrt_star_globalplanner /home/wxt/RRT-GlobalPlannerPlugin/src/informed_rrt_star_globalplanner/informed_rrt_star_globalplanner_plugin.xml

9、如何把我们的全局路径规划器内置到ROS的工作目录呢？

在我把我的规划器调试完成之后，我就想仿真一下子试试，这一试不要紧，整个人瞬间感觉就不好了。因为我与到了一些奇奇怪怪的bug。

bug1——环境变量不对劲呀

我不喜欢把什么环境都写到.bashrc文件中去，这回让我的.bashrc文件看起来乱糟糟的。所以我都是用到哪一个就source 哪一个。可是今天运气实在不好。
我为了测试我的规划器，从github上git了一个turtlebot的仿真程序，把里面的规划器替换成了我的规划器，想着能见识一下自己写的规划器好不好，可以一运行就报错说找不到我写的规划器。What the Hell?

我赶快运行rospack plugins --attrib=plugin nav_core命令看看我的规划器还在不在。结果我的规划器消失了，取而代之的是git下来的那个程序里的规划器。我一想，我的规划器是写在环境配置文件中的，ros内置的规划器是在ros/kinetic目录下。那肯定是我在source git下来的那个程序的时候把我的规划器给覆盖掉了，看来只能source一次。所以我把这两个程序——我的规划器和git下来的那个规划器——都写进环境配置文件中，这部就好了？连source都不用source了。

结果我一试。。。头痛。发生什么事了？怎么还是只有git下来的一个？我把它们两个在环境配置文件中的位置交换了一下，结果只有我的规划器了，git下来的那一个又不见了。我一想就明白了，这是给覆盖掉了呀。

我眉头一皱，计上心来。既然ros内置的规划器雷打不动，那我干脆把我的规划器变成ros内置的规划器不就行了？但是我怎么知道怎样才能把我的规划器变成ros内置规划器呢？我眉头又一皱，又是一计上心来，我看看carrot_planner规划器在ros中是怎么配置的不就行了？

说干就干，我进到opt/ros/kinetic目录下，搜索一下carrot_planner，看看出来那些文件，在什么位置，照猫画虎，一样来一遍不久OK？

总共分成4步，步骤如下：
（1）将自己的规划器文件下的include文件夹里面的那个文件夹拷贝到kinect/include下
（2）将自己的devel/share下的那个cmake文件夹拷贝到kinect/share之下，并把那两个描述文件package.xml
和global_planner_plugin.xml拷贝到kinect/share里面
（3）将devel/lib下的那个so文件拷贝到kinect/lib目录之下
（4）将devel/lib/pkgconfig下的那个pc文件拷贝到kinect/lib/pkconfig之下
搞完这些，我心想，应该没问题了吧。结果结果结果。。。。

bug2——编译出错

opt/ros/kinetic/lib//libglobal_planner.so
这是什么鬼？我的路径写错了？检查了一个遍，也没有发现哪里路径写错了。上网一搜，怀疑可能是cMakelist.txt文件的问题，导致链接的时候，出现了问题。

我一看我的cMakelist.txt文件，还真有可能，在写到项目名称的时候，我全是写的informed_rrt_star_globalplanner，再看人家carrot_planer的cMakelist.txt，凡是需要项目名称的时候，写的全是PROJECT_NAME。其实照我看都一样嘛？这也能出问题？先试试再说，把informed_rrt_star_globalplanner全换成PROJECT_NAME，一看成了，通了。看来cMakelist.txt的写法相当重要啊。一不留神就是一个大坑。