如何替換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方法相似。