如何替换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看起来像下面这样：

    <export>
        <nav_core plugin="${prefix}/planner_plugin.xml" />
    </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方法相似。