ROS随笔 如何替换ROS中默认的Planner

官方文档参阅：http://wiki.ros.org/pluginlib

有时候，可能会需要将替换ROS默认的planner替换成别的planner或我们自己的planner。这就涉及到了新planner包的建立和配置。

建立一个新的planner，大致分为以下几个步骤：

1. 实现nav_core包中的base_global_planner或base_local_planner接口，来建立一个新的planner包。

2. 在planner源码中添加：PLUGINLIB_EXPORT_CLASS宏，用于注册planner，否则ROS会不知道你的这个类是一个planner。

3. 在你的项目中添加your_planner_plugin.xml，用于声明你的planner。

4. 在package.xml中添加export。这里特别需要注意一点，如果你的export看起来像下面这样：

那么，一定要记得在package.xml中添加：

<build_depend>nav_core</build_depend>
<exec_depend>nav_core</exec_depend>

否则，你会发现编译全对，但启动move_base就是找不到你的planner。会出来类似下面的错误：

Failed to create the your_planner/YourPlannerROS planner, are you sure it is properly registered and that the containing library is built?
Exception: According to the loaded plugin descriptions the class your_planner/YourPlannerROS with base class type nav_core::BaseGlobalPlanner does not exist.
Declared types are carrot_planner/CarrotPlanner global_planner/GlobalPlanner navfn/NavfnROS.

无论你怎么调试，系统就是找不到你的planner。

5. 最后一步是编写CMakefileLists.txt。一定要注意install你的lib文件和plugin.xml文件。不install的话有时会因找不到这些文件而失败：
   复制代码

install(TARGETS your_planner
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)

install(FILES your_planner_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

复制代码

使用自己的planner进行测试时，推荐使用catkin_make_isolated --install进行编译，然后source install_isolated/setup.bash。使用devel_isolated/setup.bash有时会找不到planner。

PS：如果调试的时候发现还是出错，想查具体错误原因，可以修改move_base中下面这段：
复制代码

//initialize the global planner
try {
  planner_ = bgp_loader_.createInstance(global_planner);
  planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
  ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
  exit(1);
}

复制代码

将它修改为：
复制代码

//initialize the global planner
try {
  planner_ = bgp_loader_.createInstance(global_planner);
  planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
}
catch (const pluginlib::LibraryLoadException& ex) {
    ROS_FATAL("pluginlib::LibraryLoadException");
    exit(1);
}
catch (const pluginlib::ClassLoaderException& ex) {
    ROS_FATAL("pluginlib::ClassLoaderException");
    exit(1);
}
catch (const pluginlib::LibraryUnloadException& ex) {
    ROS_FATAL("pluginlib::LibraryUnloadException");
    exit(1);
}
catch (const pluginlib::CreateClassException& ex) {
    ROS_FATAL("pluginlib::CreateClassException");
    exit(1);
}
catch (const pluginlib::PluginlibException& ex) {
  ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
  exit(1);
}

复制代码

就可以看到具体的错误原因了，local planner和global planner方法相似。