ROS plugin原理

在costmap_2d 中，大量使用了plugin技术实现可配置的layer和planner，为程序提供了很好的扩展性，而且这种扩展是不需要重新编译就能够实现。
首先来看costmap_2d中使用到的plugin：

bob@bob-Think:~$ rospack plugins --attrib=plugin costmap_2d
costmap_2d /opt/ros/indigo/share/costmap_2d/costmap_plugins.xml
bob@bob-Think:~$ cat /opt/ros/indigo/share/costmap_2d/costmap_plugins.xml
<class_libraries>
  <library path="liblayers">
    <class type="costmap_2d::InflationLayer"  base_class_type="costmap_2d::Layer">
      <description>Inflates obstacles to speed collision checking and to make robot prefer to stay away from obstacles.</description>
    </class>
    <class type="costmap_2d::ObstacleLayer"   base_class_type="costmap_2d::Layer">
      <description>Listens to laser scan and point cloud messages and marks and clears grid cells.</description>
    </class>
    <class type="costmap_2d::StaticLayer"     base_class_type="costmap_2d::Layer">
      <description>Listens to OccupancyGrid messages and copies them in, like from map_server.</description>
    </class>
    <class type="costmap_2d::VoxelLayer"     base_class_type="costmap_2d::Layer">
      <description>Similar to obstacle costmap, but uses 3D voxel grid to store data.</description>
    </class>
  </library>
</class_libraries>

这里注册了4个plugin，正如之前的关于这些类的分析。

然后再将这个plugin配置文件写入到package.xml:

bob@bob-Think:/opt/ros/indigo/share/costmap_2d$ cat package.xml 
<package>
    <name>costmap_2d</name>
    <version>1.12.5</version>
    <description>
        This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending on whether a voxel based implementation is used), and inflates costs in a 2D costmap based on the occupancy grid and a user specified inflation radius. This package also provides support for map_server based initialization of a costmap, rolling window based costmaps, and parameter based subscription to and configuration of sensor topics.
    </description>
    <author>Eitan Marder-Eppstein</author>
    <author>David V. Lu!!</author>
    <author>Dave Hershberger</author>
    <author>[email protected]</author>
    <maintainer email="[email protected]">David V. Lu!!</maintainer>
    <maintainer email="[email protected]">Michael Ferguson</maintainer>
    <license>BSD</license>
    <url>http://wiki.ros.org/costmap_2d</url>

    <buildtool_depend>catkin</buildtool_depend>

    <build_depend>cmake_modules</build_depend>
    <build_depend>dynamic_reconfigure</build_depend>
    <build_depend>geometry_msgs</build_depend>
    <build_depend>laser_geometry</build_depend>
    <build_depend>map_msgs</build_depend>
    <build_depend>message_filters</build_depend>
    <build_depend>message_generation</build_depend>
    <build_depend>nav_msgs</build_depend>
    <build_depend>pcl_conversions</build_depend>
    <build_depend>pcl_ros</build_depend>
    <build_depend>pluginlib</build_depend>
    <build_depend>roscpp</build_depend>
    <build_depend>rostest</build_depend>
    <build_depend>sensor_msgs</build_depend>
    <build_depend>std_msgs</build_depend>
    <build_depend>tf</build_depend>
    <build_depend>visualization_msgs</build_depend>
    <build_depend>voxel_grid</build_depend>

    <run_depend>dynamic_reconfigure</run_depend>
    <run_depend>geometry_msgs</run_depend>
    <run_depend>laser_geometry</run_depend>
    <run_depend>map_msgs</run_depend>
    <run_depend>message_filters</run_depend>
    <run_depend>message_runtime</run_depend>
    <run_depend>nav_msgs</run_depend>
    <run_depend>pcl_conversions</run_depend>
    <run_depend>pcl_ros</run_depend>
    <run_depend>pluginlib</run_depend>
    <run_depend>rosconsole</run_depend>
    <run_depend>roscpp</run_depend>
    <run_depend>rostest</run_depend>
    <run_depend>sensor_msgs</run_depend>
    <run_depend>std_msgs</run_depend>
    <run_depend>tf</run_depend>
    <run_depend>visualization_msgs</run_depend>
    <run_depend>voxel_grid</run_depend>

    <test_depend>map_server</test_depend>
    <test_depend>rosbag</test_depend>

    <export>
      <costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
    </export>
</package>

最后三行，指定了刚才设定的plugin文件。

然后任意选择一个plugin实例类：InflationLayer：
在它的cpp文件中，加入这行：

PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer)

这样就完成了plugin 类的实现。
如何使用的plugin类呢？还是来看costmap是怎么做的：
在costmap_2d这个package里面：
class costmap_2d_ros.h 中定义的数据成员pluginlib::ClassLoader<Layer> plugin_loader_;
然后在这个类的构造函数中：
初始化列表：plugin_loader_("costmap_2d", "costmap_2d::Layer"), publisher_(NULL) 参数类型是std::string package, std::string base_class。

 if (private_nh.hasParam("plugins"))
  {
    XmlRpc::XmlRpcValue my_list;
    private_nh.getParam("plugins", my_list);
    for (int32_t i = 0; i < my_list.size(); ++i)
    {
      std::string pname = static_cast<std::string>(my_list[i]["name"]);
      std::string type = static_cast<std::string>(my_list[i]["type"]);
      ROS_INFO("Using plugin \"%s\"", pname.c_str());
    //这样就创建了一个实例。
      boost::shared_ptr<Layer> plugin=plugin_loader_.createInstance(type);
      layered_costmap_->addPlugin(plugin);
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
    }

这里再来看另外一个关于plugin的应用实例：
声明一个纯虚基类：
test_base.h

#ifndef PLUGINLIB_TEST_BASE_H_
#define PLUGINLIB_TEST_BASE_H_

namespace test_base
{
  class Fubar
  {
  public:
    virtual void initialize(double foo) = 0;
    virtual double result() = 0;
    virtual ~Fubar(){}

  protected:
    Fubar(){}
  };
};
#endif

声明两个派生类：
test_plugins.h

#ifndef PLUGINLIB_TEST_PLUGINS_H_
#define PLUGINLIB_TEST_PLUGINS_H_
#include "test_base.h"
#include <cmath>

namespace test_plugins
{
class Bar : public test_base::Fubar
{
public:
  Bar(){}

  void initialize(double foo)
  {
    foo_ = foo;
  }

  double result()
  {
    return 0.5 * foo_ * getBar();
  }

  double getBar()
  {
    return sqrt((foo_ * foo_) - ((foo_ / 2) * (foo_ / 2)));
  }

private:
  double foo_;
};

class Foo : public test_base::Fubar
{
public:
  Foo(){}

  void initialize(double foo)
  {
    foo_ = foo;
  }

  double result()
  {
    return foo_ * foo_;
  }

private:
  double foo_;

};
};
#endif

实现文件中声明plugin的宏：
test_plugin.cpp

#include <pluginlib/class_list_macros.h>
#include "test_base.h"
#include "test_plugins.h"

PLUGINLIB_DECLARE_CLASS(pluginlib, foo, test_plugins::Foo, test_base::Fubar)
PLUGINLIB_DECLARE_CLASS(pluginlib, bar, test_plugins::Bar, test_base::Fubar)

写一个测试文件：
utest.cpp

#include <pluginlib/class_loader.h>
#include "test_base.h"
#include <gtest/gtest.h>

TEST(PluginlibTest, unknownPlugin)
{
  pluginlib::ClassLoader<test_base::Fubar> test_loader("pluginlib", "test_base::Fubar");
 // test_base::Fubar* foo = NULL;
 boost::shared_ptr<test_base::Fubar> foo;
  try
  {
    //foo = test_loader.createClassInstance(("pluginlib/foobar"));
    foo = test_loader.createInstance("pluginlib/foobar");
    foo->initialize(10.0);
  }
  catch(pluginlib::LibraryLoadException& ex)
  {
    SUCCEED();
    return;
  }
  catch(...)
  {
    FAIL() << "Uncaught exception";
  }
  ADD_FAILURE() << "Didn't throw exception as expected";
}
TEST(PluginlibTest, createManagedInstanceAndUnloadLibrary)
{
  ROS_INFO( "Making the ClassLoader..." );
  pluginlib::ClassLoader<test_base::Fubar> pl("pluginlib", "test_base::Fubar");

  ROS_INFO( "Instantiating plugin..." );
  {
    boost::shared_ptr<test_base::Fubar> inst = pl.createInstance("pluginlib/foo");
  }

  ROS_INFO( "Checking if plugin is loaded with isClassLoaded..." );
  if( pl.isClassLoaded( "pluginlib/foo" ) )
    ROS_INFO( "Class is loaded" );
  else
  {
    FAIL() <<  "Library containing class should be loaded but isn't.";
  }

  ROS_INFO( "Trying to unload class with unloadLibraryForClass..." );
  try
  {
    pl.unloadLibraryForClass("pluginlib/foo");
  }
  catch(pluginlib::PluginlibException& e)
  {
    FAIL() << "Could not unload library when I should be able to.";
  }
  ROS_INFO( "Done." );
}

// Run all the tests that were declared with TEST()
int main(int argc, char **argv){
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}

将派生类注册到xml：
test_plugins.xml

<library path="lib/libtest_plugins">
  <class name="pluginlib/foo" type="test_plugins::Foo" base_class_type="test_base::Fubar">
    <description>This is a foo plugin.</description>
  </class>
  <class name="pluginlib/bar" type="test_plugins::Bar" base_class_type="test_base::Fubar">
    <description>This is a bar plugin.</description>
  </class>
  <class name="pluginlib/none" type="test_plugins::None" base_class_type="test_base::Fubar">
    <description>This is a broken none plugin.</description>
  </class>
</library>

并且在整个package的配置文件中加入这个xml：

  <export>
    <pluginlib plugin="${prefix}/test/test_plugins.xml"/>
  </export>

CMakeLists.txt配置：

if(CATKIN_ENABLE_TESTING)
  add_library(test_plugins EXCLUDE_FROM_ALL SHARED test/test_plugins.cpp)

  catkin_add_gtest(${PROJECT_NAME}_utest test/utest.cpp)
  if(TARGET ${PROJECT_NAME}_utest)
    target_link_libraries(${PROJECT_NAME}_utest ${TinyXML_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
    add_dependencies(${PROJECT_NAME}_utest test_plugins)
  endif()
endif()

测试结果

[==========] Running 2 tests from 1 test case.
[----------] Global test environment set-up.
[----------] 2 tests from PluginlibTest
[ RUN      ] PluginlibTest.unknownPlugin
[       OK ] PluginlibTest.unknownPlugin (57 ms)
[ RUN      ] PluginlibTest.createManagedInstanceAndUnloadLibrary
[ INFO] [1452581264.700534202]: Making the ClassLoader...
[ INFO] [1452581264.751571120]: Instantiating plugin...
[ INFO] [1452581264.818041299]: Checking if plugin is loaded with isClassLoaded...
[ INFO] [1452581264.818101394]: Class is loaded
[ INFO] [1452581264.818127187]: Trying to unload class with unloadLibraryForClass...
[ INFO] [1452581264.818242212]: Done.
[       OK ] PluginlibTest.createManagedInstanceAndUnloadLibrary (118 ms)
[----------] 2 tests from PluginlibTest (175 ms total)

[----------] Global test environment tear-down
[==========] 2 tests from 1 test case ran. (175 ms total)
[  PASSED  ] 2 tests.
-- run_tests.py: verify result "/home/bob/.CLion12/system/cmake/generated/8fb52bd3/8fb52bd3/__default__/test_results/pluginlib/gtest-pluginlib_utest.xml"
[100%] Built target run_tests_pluginlib_gtest_pluginlib_utest

从测试用例就能看到plugin怎么用了的吧。

OK，然后来看具体的实现吧：
在使用中，首先调用pluginlib::ClassLoader<test_base::Fubar> pl("pluginlib", "test_base::Fubar"); 即ClassLoader类的构造函数：

ClassLoader(std::string package, std::string base_class, std::string attrib_name = std::string("plugin"),std::vector<std::string> plugin_xml_paths =std::vector<std::string>());

这个构造函数的实现：

  template <class T>
  ClassLoader<T>::ClassLoader(std::string package, std::string base_class, std::string attrib_name, std::vector<std::string> plugin_xml_paths) :
  plugin_xml_paths_(plugin_xml_paths),
  package_(package),
  base_class_(base_class),
  attrib_name_(attrib_name),
  lowlevel_class_loader_(false) //NOTE: The parameter to the class loader enables/disables on-demand class loading/unloading. Leaving it off for now...libraries will be loaded immediately and won't be unloaded until class loader is destroyed or force unload.
  /***************************************************************************/
  //default:attrib_name ="plugins" plugin_xml_paths= std::vector<std::string>()
  //in essential ,it only determine the xml location, as same as the command "rospack plugins --attrib=plugin costmap_2d"
  {
    ROS_DEBUG_NAMED("pluginlib.ClassLoader","Creating ClassLoader, base = %s, address = %p", base_class.c_str(), this);
    if (ros::package::getPath(package_).empty())
    {
      throw pluginlib::ClassLoaderException("Unable to find package: " + package_);
    }

    if (plugin_xml_paths_.size() == 0)
    {
      plugin_xml_paths_ = getPluginXmlPaths(package_, attrib_name_);
    }
    classes_available_ = determineAvailableClasses(plugin_xml_paths_);
    ROS_DEBUG_NAMED("pluginlib.ClassLoader","Finished constructring ClassLoader, base = %s, address = %p", base_class.c_str(), this);
  }

其实仅仅是定位找到那个表征子类和父类的xml文件而已。 然后通过这个xml文件给classes_available_ 赋值，这个参数的声明为std::map

template <class T>boost::shared_ptr<T> ClassLoader<T>::createInstance(const std::string& lookup_name)
  /***************************************************************************/
  {
    ROS_DEBUG_NAMED("pluginlib.ClassLoader","Attempting to create managed instance for class %s.", lookup_name.c_str());

    if(!isClassLoaded(lookup_name))
      loadLibraryForClass(lookup_name);

    try
    {
      std::string class_type = getClassType(lookup_name);
      ROS_DEBUG_NAMED("pluginlib.ClassLoader","%s maps to real class type %s", lookup_name.c_str(), class_type.c_str());    

      boost::shared_ptr<T> obj = lowlevel_class_loader_.createInstance<T>(class_type);

      ROS_DEBUG_NAMED("pluginlib.ClassLoader","boost::shared_ptr to object of real type %s created.", class_type.c_str());    

      return obj;
    }
    catch(const class_loader::CreateClassException& ex)
    {
      ROS_DEBUG_NAMED("pluginlib.ClassLoader","Exception raised by low-level multi-library class loader when attempting to create instance of class %s.", lookup_name.c_str());
      throw(pluginlib::CreateClassException(ex.what()));
    }
  }

哦哦，这里又出现了类里面的一堆相互调用了，放出调用关系图：

进入函数之后，首先检查是否已经被load了，如果没有，则调用函数执行loadvoid ClassLoader<T>::loadLibraryForClass(const std::string& lookup_name)

  {

    ClassMapIterator it = classes_available_.find(lookup_name);
    if (it == classes_available_.end())
    {
    throw pluginlib::LibraryLoadException(getErrorStringForUnknownClass(lookup_name));
    }

    std::string library_path = getClassLibraryPath(lookup_name);
    if (library_path == "")
    {
      std::ostringstream error_msg;
      error_msg << "Could not find library corresponding to plugin ";
      throw pluginlib::LibraryLoadException(error_msg.str());
    }

    try
    {
      lowlevel_class_loader_.loadLibrary(library_path);
      it->second.resolved_library_path_ = library_path;
    }
    catch(const class_loader::LibraryLoadException& ex)
    {
      throw pluginlib::LibraryLoadException(error_string);
    }
  }