7.3 编写插件-世界插件

预备知识

• 模型操作（http://gazebosim.org/tutorials/?tut=plugins_model）
• 插件教程（http://gazebosim.org/tutorials/?tut=plugins_hello_world）

代码

来源：gazebo/examples/plugins/factory

https://bitbucket.org/osrf/gazebo/src/gazebo7/examples/plugins/factory

控制运行仿真中的模型，以及何时应该插入这些模型是很有用的。本教程演示如何在Gazebo中插入预定义的和自定义的模型。

使用以前的插件教程中的gazebo_plugin_tutorial

$ mkdir~/gazebo_plugin_tutorial

$ cd~/gazebo_plugin_tutorial

创建一个新的源文件：

$ gedit factory.cc

将下面的代码复制到factory.cc文件：

#include<ignition/math/Pose3.hh>
#include"gazebo/physics/physics.hh"
#include"gazebo/common/common.hh"
#include"gazebo/gazebo.hh"
 
namespacegazebo
{
class Factory :public WorldPlugin
{
  public: void Load(physics::WorldPtr _parent,sdf::ElementPtr /*_sdf*/)
  {
    // Option 1: Insert model from file viafunction call.
    // The filename must be in theGAZEBO_MODEL_PATH environment variable.
   _parent->InsertModelFile("model://box");
 
    // Option 2: Insert model from string viafunction call.
    // Insert a sphere model from string
    sdf::SDF sphereSDF;
    sphereSDF.SetFromString(
       "<sdf version ='1.4'>\
          <model name ='sphere'>\
            <pose>1 0 0 0 00</pose>\
            <link name ='link'>\
              <pose>0 0 .5 0 00</pose>\
              <collision name='collision'>\
                <geometry>\
                 <sphere><radius>0.5</radius></sphere>\
                </geometry>\
              </collision>\
              <visual name ='visual'>\
                <geometry>\
                 <sphere><radius>0.5</radius></sphere>\
                </geometry>\
              </visual>\
            </link>\
          </model>\
        </sdf>");
    // Demonstrate using a custom model name.
    sdf::ElementPtr model =sphereSDF.Root()->GetElement("model");
   model->GetAttribute("name")->SetFromString("unique_sphere");
    _parent->InsertModelSDF(sphereSDF);
 
    // Option 3: Insert model from file viamessage passing.
    {
      // Create a new transport node
      transport::NodePtr node(newtransport::Node());
 
      // Initialize the node with the worldname
      node->Init(_parent->GetName());
 
      // Create a publisher on the ~/factorytopic
      transport::PublisherPtr factoryPub =
     node->Advertise<msgs::Factory>("~/factory");
 
      // Create the message
      msgs::Factory msg;
 
      // Model file to load
     msg.set_sdf_filename("model://cylinder");
 
      // Pose to initialize the model to
      msgs::Set(msg.mutable_pose(),
          ignition::math::Pose3d(
            ignition::math::Vector3d(1, -2, 0),
            ignition::math::Quaterniond(0, 0,0)));
 
      // Send the message
      factoryPub->Publish(msg);
    }
  }
};
 
// Registerthis plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(Factory)
}

代码解释

代码的第一部分创建了一个世界插件。

#include<ignition/math/Pose3.hh>
#include"gazebo/physics/physics.hh"
#include"gazebo/common/common.hh"
#include"gazebo/gazebo.hh"
 
namespacegazebo
{
class Factory :public WorldPlugin
{
  public: void Load(physics::WorldPtr _parent,sdf::ElementPtr /*_sdf*/)

在Load函数中有三种不同的模型插入方法。

第一种方法使用一个World方法来加载基于gazebomodelpath环境变量定义的资源路径中的文件的模型。

    // Option 1: Insert model fromfile via function call.
    // The filename must be in theGAZEBO_MODEL_PATH environment variable.
   _parent->InsertModelFile("model://box");

第二种方法使用一个世界方法来加载基于字符串数据的模型。

    // Option 2: Insert model fromstring via function call.
    // Insert a sphere model fromstring
    sdf::SDF sphereSDF;
    sphereSDF.SetFromString(
       "<sdf version='1.4'>\
          <model name='sphere'>\
            <pose>1 0 0 0 00</pose>\
            <link name='link'>\
              <pose>0 0 .5 0 00</pose>\
              <collision name='collision'>\
                <geometry>\
                 <sphere><radius>0.5</radius></sphere>\
                </geometry>\
              </collision>\
              <visual name='visual'>\
                <geometry>\
                  <sphere><radius>0.5</radius></sphere>\
                </geometry>\
              </visual>\
            </link>\
          </model>\
        </sdf>");
    // Demonstrate using a custommodel name.
    sdf::ElementPtr model =sphereSDF.Root()->GetElement("model");
   model->GetAttribute("name")->SetFromString("unique_sphere");
   _parent->InsertModelSDF(sphereSDF);

第三种方法使用消息传递机制来插入模型。这种方法对于单独的应用程序来说是最有用的，因为它与Gazebo在网络连接上进行通信。

    // Option 3: Insert model fromfile via message passing.
    {
      // Create a new transport node
      transport::NodePtr node(newtransport::Node());
 
      // Initialize the node with theworld name
     node->Init(_parent->GetName());
 
      // Create a publisher on the~/factory topic
      transport::PublisherPtrfactoryPub =
     node->Advertise<msgs::Factory>("~/factory");
 
      // Create the message
      msgs::Factory msg;
 
      // Model file to load
     msg.set_sdf_filename("model://cylinder");
 
      // Pose to initialize the modelto
      msgs::Set(msg.mutable_pose(),
          ignition::math::Pose3d(
           ignition::math::Vector3d(1, -2, 0),
           ignition::math::Quaterniond(0, 0, 0)));
 
      // Send the message
      factoryPub->Publish(msg);

编译

假设读者已经阅读了插件概述教程，那么需要做的就是把上面的代码保存为~/gazebo_plugin_tutorial/factory.cc。将以下代码添加到~/gazebo_plugin_tutorial/CMakeLists.txt

add_library(factorySHARED factory.cc)

target_link_libraries(factory ${GAZEBO_LIBRARIES})

编译这段代码将导致一个共享库，~/gazebo_plugin_tutorial/build/libfactory.so。因此，这可以插入到Gazebo仿真中。

$ mkdir~/gazebo_plugin_tutorial/build

$ cd~/gazebo_plugin_tutorial/build

$ cmake ../

$ make

制作形状

用一个方框和一个圆柱体来制作一个模型目录

$ mkdir~/gazebo_plugin_tutorial/models

$ cd~/gazebo_plugin_tutorial/models

$ mkdir boxcylinder

创建一个盒子模型

$ cd box

$ geditmodel.sdf

将以下内容复制粘贴到box/model.sdf中

<?xmlversion='1.0'?>
<sdf version='1.6'>
  <model name ='box'>
    <pose>1 2 0 0 0 0</pose>
    <link name ='link'>
      <pose>0 0 .5 0 0 0</pose>
      <collision name ='collision'>
        <geometry>
          <box><size>1 11</size></box>
        </geometry>
      </collision>
      <visual name ='visual'>
        <geometry>
          <box><size>1 11</size></box>
        </geometry>
      </visual>
    </link>
  </model>
</sdf>

创建一个model.config文件

$ gedit model.config

将以下内容复制到model.config

<?xmlversion='1.0'?>
 
<model>
  <name>box</name>
  <version>1.0</version>
  <sdf >model.sdf</sdf>
 
  <author>
    <name>me</name>
   <email>somebody@somewhere.com</email>
  </author>
 
  <description>
    A simple Box.
  </description>
</model>

转到圆柱体目录，并创建一个新的model.sdf文件

$ cd~/gazebo_plugin_tutorial/models/cylinder

$ geditmodel.sdf

将以下内容复制到model.sdf中

<?xmlversion='1.0'?>
<sdf version='1.6'>
  <model name ='cylinder'>
    <pose>1 2 0 0 0 0</pose>
    <link name ='link'>
      <pose>0 0 .5 0 0 0</pose>
      <collision name ='collision'>
        <geometry>
         <cylinder><radius>0.5</radius><length>1</length></cylinder>
        </geometry>
      </collision>
      <visual name='visual'>
        <geometry>
          <cylinder><radius>0.5</radius><length>1</length></cylinder>
        </geometry>
      </visual>
    </link>
  </model>
</sdf>

创建一个model.config文件

$ geditmodel.config

将以下内容复制到model.config

<?xmlversion='1.0'?>
 
<model>
  <name>cylinder</name>
  <version>1.0</version>
  <sdf>model.sdf</sdf>
 
  <author>
    <name>me</name>
   <email>somebody@somewhere.com</email>
  </author>
 
  <description>
    A simple cylinder.
  </description>
</model>

运行代码

确保您的$GAZEBO_MODEL_PATH引用您的新模型目录

$ export GAZEBO_MODEL_PATH=$HOME/gazebo_plugin_tutorial/models:$GAZEBO_MODEL_PATH

将您的库路径添加到GAZEBO_PLUGIN_PATH：

$ exportGAZEBO_PLUGIN_PATH=$HOME/gazebo_plugin_tutorial/build:$GAZEBO_PLUGIN_PATH

创建一个名为~/gazebo_plugin_tutorial/factory.world的世界SDF文件。

$ cd ~/gazebo_plugin_tutorial

$ geditfactory.world

把下面的内容复制到世界

<?xmlversion="1.0"?>
<sdfversion="1.4">
  <world name="default">
    <include>
     <uri>model://ground_plane</uri>
    </include>
 
    <include>
      <uri>model://sun</uri>
    </include>
 
    <plugin name="factory"filename="libfactory.so"/>
  </world>
</sdf>

运行Gazebo

$ gazebo~/gazebo_plugin_tutorial/factory.world

Gazebo窗口应该显示一个环境，其中包含一个球体、一个立方体和一个圆柱体。