为ROS navigation功能包添加自定义的全局路径规划器(Global Path Planner)

frank

ROS 的navigation官方功能包提供了三种全局路径规划器：carrot_planner、global_planner、navfn。我们通常使用的是navfn，如果机器人执行一些特殊任务而navfn不支持的话就需要自己写一个全局路径规划区并注册到ROS系统。
ROS的全局路径规划器是以lib插件形式（pluginlib）存在的，通过配置move_base 中base_global_planner参数指定实际路径规划执行时采用的全局路径规划器，这种设计大大提高了navigation包的可扩展性。
ROS官网wiki有详细的教程指导大家如何为ROS添加自定义的全局路径规划器，操作步骤挺繁琐的，在了解原理之后我们其实可以用下文的偷懒方法实现自己的全局路径规划器。主要思路是：直接拷贝一份现成的全局路径规划器代码包，将软件包中的相关名字修改成想要的，最后实现下自己的makeplan方法即可。

1.从navigation包中拷贝carrot_planner文件夹下的所有内容到ROS源码工作目录。

以小强ROS机器人为例:
cd Documents/ros/src/               进入ROS源码工作目录
cp -a  navigation/carrot_planner test_planner  拷贝一份carrot_planner代码到test_planner文件夹下，test可以改成自己想要的任何名字，这个名字即为下文咱们自定义的全局规划器

2.开始修改名字，将拷贝的文件夹（test_planner）内的所有文件中的carrot字符换成test，包括文件名中的字符

需要修改内容的文件有：
test_planner/include/carrot_planner/carrot_planner.h
test_planner/src/carrot_planner.cpp
test_planner/bgp_plugin.xml
test_planner/CMakeLists.txt
test_planner/package.xml
需要修改名字的文件有：
test_planner/include/carrot_planner/carrot_planner.h
test_planner/src/carrot_planner.cpp
需要修改名字的文件夹有：
include下的carrot_planner文件夹

3.尝试编译

以小强ROS机器人为例:
cd Documents/ros/
catkin_make

4.检查test_planner是否成功注册进ROS系统

rospack plugins --attrib=plugin nav_core
正常的话会返回如下内容，可以查看到test_planner
rotate_recovery /home/xiaoqiang/Documents/ros/src/navigation/rotate_recovery/rotate_plugin.xml
base_local_planner /home/xiaoqiang/Documents/ros/src/navigation/base_local_planner/blp_plugin.xml
global_planner /home/xiaoqiang/Documents/ros/src/navigation/global_planner/bgp_plugin.xml
navfn /home/xiaoqiang/Documents/ros/src/navigation/navfn/bgp_plugin.xml
move_slow_and_clear /home/xiaoqiang/Documents/ros/src/navigation/move_slow_and_clear/recovery_plugin.xml
dwa_local_planner /home/xiaoqiang/Documents/ros/src/navigation/dwa_local_planner/blp_plugin.xml
clear_costmap_recovery /home/xiaoqiang/Documents/ros/src/navigation/clear_costmap_recovery/ccr_plugin.xml
carrot_planner /home/xiaoqiang/Documents/ros/src/test_planner/bgp_plugin.xml
test_planner /home/xiaoqiang/Documents/ros/src/ test_planner/bgp_plugin.xml

5.根据自己的算法重写test_planner/src/test_planner.cpp中的makeplan方法，然后重新用catkin_make编译，至此咱们的全局路径规划器就打完收工了可以在move_base中使用了

6.在move_base节点中使用新建立的test_planner路径规划器

在move_base的启动launch文件节点中加入如下代码即可
<param name="base_global_planner" value="test_planner/testPlanner"/>

久安

@frank 局部路径规划器的编写也类似吗，想写一个人工势场的局部路径规划器麻烦吗？另外想咨询一下您知道有什么好的局部路径规划算法吗？谢谢大神，期待您的回复。

frank

@久安类似，很多同学在用teb规划器

Lee1004

大神好！请问“从navigation包中拷贝carrot_planner文件夹下的所有内容到ROS源码工作目录。”这里的navigation指的是哪里啊？我在ros系统安装目录下没有找到符合的文件内容呢

xiaoqiang

@Lee1004 源码在这里https://github.com/ros-planning/navigation.git

Lee1004

@xiaoqiang 谢大神！

Lee1004

@xiaoqiang 大神，我实现了上述方法之后，还有一个问题，就是我发现调用test_planner的时候，在rviz里面move_base只发布了局部规划器的path话题，global_planner的path话题找不到。但我通过在test_planner的.cpp文件中的makeplan()中添加debug输出标志，发现test_planner确实调用成功了。所以请教为什么会出现我所述的原因呢？

xiaoqiang

@Lee1004 这个全局规划器可能太简单了，没有发布global plan的代码，只是实现了makeplan的service.用rqt_graph分析一下节点话题数据流

Lee1004

@xiaoqiang 我也觉得应该是这个算法里面没有发布movebase/GlobalPlanner/plan，但是movebase/DWAPlannerROS/global_plan是可以找到的，我个人通过其他算法的仿真试验发现其实这两话题的内容其实是一样的，前者的结果实际就传递给后者作为全局参考了，不知道对不对

xiaoqiang

@Lee1004 dwa是通过makeplan这个service由move_base传给它的，正因为如此，所以这个发布就可有可无，只是一个显示用途