【Gazebo入门教程】第五讲 控制器插件的编写与配置（上）

【Gazebo入门教程】第五讲 控制器插件的编写与配置（上）

前言：在先前的博客中，我们不仅完成了对机器人模型的建立和仿真，并且创建了机器人的工作空间，即仿真环境的设置，那么想要通过控制机器人传感器来完成对于机器人的控制就需要进一步研究，学会如何使用控制插件，通过编写代码在Gazebo中加载C++库完成对于机器人的实际控制。

一、控制插件的使用方法

1. 插件简介

• 目的：用于访问Gazebo的API，通过API执行目标任务，例如移动、添加对象、获取传感器数据等；
• 优点：在后期的Gazebo仿真开发中必不可少的部分，可以大大简化对于机器人的控制实现；

1. 能让开发者控制几乎所有的Gazebo功能
2. 独立的例行程序，易于分享
3. 可以插入正在运行的系统或被其移除

• 使用场景：控制插件的使用场景较为广泛，我们应尽量通过插件简化仿真和控制流程；

1. 以编程的方式来更改一个仿真，例如移动模型，对事件进行响应，插入有预先条件的模型
2. 想要有一个到Gazebo的快速接口，不必经过传输层(transport layer)，例如没有对消息进行序列化和反序列化
3. 代码分享需求

• 插件类别：目前的插件主要可分为World，Model，Sensor，System，Visual，GUI六种，每个插件类型都由Gazebo的不同部分管理，系统插件在命令行中被指定，在Gazebo启动时首先加载。基于想实现的功能，我们应当选择不同的插件类型，比如我们想调整物理引擎，更改环境光，则需要世界插件。又或者想控制关节，和模型的状态，则需要模型插件。

2. 插件编写流程

• 2.1 环境配置：安装Gazebo开发文件

sudo apt-get install libgazebo11-dev

• 2.2 创建工作目录：

mkdir ~/gazebo_plugin_tutorial
cd ~/gazebo_plugin_tutorial

• 2.3 编写插件代码：内容紧随其后，附逐行解析

gedit hello_world.cc 

#include <gazebo/gazebo.hh>
// gazebo/gazebo.hh包含了一组核心的基本Gazebo函数，所有的插件都必须在gazebo的命名空间内。
namespace gazebo
{
  //每个插件都必须继承自某个插件类型，在这里是WorldPlugin类
  class WorldPluginTutorial : public WorldPlugin
  {
    public: WorldPluginTutorial() : WorldPlugin()
            {
              printf("Hello World!\n");
            }
	//强制函数Load，接受被加载SDF文件内的元素和属性。
    public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
            {
            }
  };
  // 插件必须使用宏GZ_REGISTER_WORLD_PLUGIN在模拟器中注册。这个宏的唯一参数就是插件的类。
  GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial)
}

• 2.4 编译插件：

1. 创建CMakeLists：

gedit ~/gazebo_plugin_tutorial/CMakeLists.txt

2. 编写编译文件：

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS >"${GAZEBO_CXX_FLAGS}")

add_library(hello_world SHARED hello_world.cc)
target_link_libraries(hello_world ${GAZEBO_LIBRARIES})

\qquad 【注：从gazebo6的版本开始都需要c++11的flag，对应上述第六行代码】

• 2.5 创建build目录并编译代码：

mkdir ~/gazebo_plugin_tutorial/build
cd ~/gazebo_plugin_tutorial/build
cmake ../
make

• 2.6 添加共享库路径：在编译后，会产生~/gazebo_plugin_tutorial/build/libhello_world.so共享库，在仿真时插入，添加路径的方法如下：

export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/gazebo_plugin_tutorial/build

【编译完成后，就可以把它附加到SDF文件内的一个世界或一个模型。启动Gazebo会解析SDF文件，并定位插件，加载代码。所以需要指定插件完全路径，或让插件存在于GAZEBO_PLUGIN_PATH环境遍历路径中】

• 2.7 创建并编写世界文件：

1. 创建世界文件：

gedit ~/gazebo_plugin_tutorial/hello.world

2. 编写世界文件：

<?xml version="1.0"?>
<sdf version="1.4">
 <world name="default">
   <plugin name="hello_world" filename="libhello_world.so"/>
 </world>
</sdf>

• 2.8 在gzserver打开世界文件：

gzserver ~/gazebo_plugin_tutorial/hello.world --verbose

• 2.9 输出如下：

Gazebo multi-robot simulator, version 9.16.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: xxx.xxx.xxx.xxx
[Msg] Loading world file [/home/qqc/gazebo_plugin_tutorial/hello.world]
Hello World!

二、模型插件与世界插件

1. 模型插件

• 创建并编写插件文件：

1. 创建插件文件：

cd ~/gazebo_plugin_tutorial
gedit model_push.cc

2. 编写插件文件：

#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>

namespace gazebo
{
 class ModelPush : public ModelPlugin
 {
   public: void Load(physics::ModelPtr _parent, >sdf::ElementPtr /*_sdf*/)
   {
     // Store the pointer to the model
     this->model = _parent;

     // Listen to the update event. This event is >broadcast every
     // simulation iteration.
     this->updateConnection = >event::Events::ConnectWorldUpdateBegin(
         std::bind(&ModelPush::OnUpdate, this));
   }

   // Called by the world update start event
   public: void OnUpdate()
   {
     // Apply a small linear velocity to the model.
     // 每次迭代设置_parent速度(.3，0，0)
     this->model->SetLinearVel(ignition::math::Vector3d(.3, 0, 0));
   }

   // Pointer to the model
   private: physics::ModelPtr model;

   // Pointer to the update event connection
   private: event::ConnectionPtr updateConnection;
 };

 // Register this plugin with the simulator
 GZ_REGISTER_MODEL_PLUGIN(ModelPush)
}

• 修改编译规则文件：

1. 打开原有文件：

gedit ~/gazebo_plugin_tutorial/CMakeLists.txt

2. 在文件底部添加如下代码：

add_library(model_push SHARED model_push.cc)
target_link_libraries(model_push >${GAZEBO_LIBRARIES})

• 编译：共享库~/gazebo_plugin_tutorial/build/libmodel_push.so

cd ~/gazebo_plugin_tutorial/build
cmake ../
make

• 修改世界文件：

1. 打开文件：

cd ~/gazebo_plugin_tutorial
gedit model_push.world

2. 编写世界文件：

<?xml version="1.0"?> 
<sdf version="1.4">
 <world name="default">

   <!-- Ground Plane -->
   <include>
     <uri>model://ground_plane</uri>
   </include>

   <include>
     <uri>model://sun</uri>
   </include>

   <model name="box">
    <pose>0 0 0.5 0 0 0</pose>
     <link name="link">
       <collision name="collision">
         <geometry>
           <box>
             <size>1 1 1</size>
           </box>
         </geometry>
       </collision>

       <visual name="visual">
         <geometry>
           <box>
             <size>1 1 1</size>
           </box>
         </geometry>
       </visual>
     </link>

     <plugin name="model_push" filename="libmodel_push.so"/>
   </model>        
 </world>
</sdf>

• 添加库路径：

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

• 运行仿真：

cd ~/gazebo_plugin_tutorial/
gzserver -u model_push.world

• 新建终端启动GUI：

gzclient

• 效果：方盒沿X轴进行匀速运动，图示如下：

2. 世界插件

1）自动添加模型的世界插件

• 创建并编写插件文件：

1. 创建插件文件：

cd ~/gazebo_plugin_tutorial
gedit factory.cc

2. 编写插件文件：

#include <ignition/math/Pose3.hh>
#include "gazebo/physics/physics.hh"
#include "gazebo/common/common.hh"
#include "gazebo/gazebo.hh"

namespace gazebo
{
class Factory : public WorldPlugin
{
 public: void Load(physics::WorldPtr _parent, >sdf::ElementPtr /*_sdf*/)
 {
   // 方法1: 基于在资源路径内的文件来加载模型，通过调用函数，从文件中插入模型
   // Option 1: Insert model from file via function call.
   // 文件名必须要在环境变量`GAZEBO_MODEL_PATH`中
   // The filename must be in the >GAZEBO_MODEL_PATH environment variable.
   _parent->InsertModelFile("model://box");

   // 方法2:通过调用函数，从字符串中插入模型
   // Option 2: Insert model from string via function call.
   // 从字符串插入一个球形模型
   // Insert a sphere model from string
   sdf::SDF sphereSDF;
   sphereSDF.SetFromString(
      "<sdf version ='1.4'>\
         <model name ='sphere'>\
           <pose>1 0 0 0 0 0</pose>\
           <link name ='link'>\
             <pose>0 0 .5 0 0 0</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);

   // 方法3: 通过消息传递，从文件插入模型
   // Option 3: Insert model from file via message passing.
   {
     // 创造一个新的传输节点 Create a new transport node
     transport::NodePtr node(new transport::Node());

     // 用世界名初始化节点 Initialize the node with the world name
     node->Init(_parent->Name());

     // 在~/factory创建一个发布者publisher Create a publisher on the ~/factory topic
     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);
   }
 }
};

// Register this plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(Factory)
}

• 修改编译文件：底部添加代码

add_library(factory SHARED factory.cc)
target_link_libraries(factory
  ${GAZEBO_LIBRARIES}
) 

• 编译文件：

cd ~/gazebo_plugin_tutorial/build
cmake ../
make

• 创建模型目录：

mkdir ~/gazebo_plugin_tutorial/models
cd ~/gazebo_plugin_tutorial/models
mkdir box cylinder

• 创建盒子模型并编写SDF文件：

1. 创建盒子模型：

cd box
gedit model.sdf

2. 编写SDF文件：

<?xml version='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 1 1</size></box>
       </geometry>
     </collision>
     <visual name ='visual'>
       <geometry>
         <box><size>1 1 1</size></box>
       </geometry>
     </visual>
   </link>
 </model>
</sdf> 

• 创建并编写配置文件：

1. 创建配置文件：

gedit model.config

2. 编写配置文件：

<?xml version='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>

• 对圆柱体重复操作：

1. SDF文件：

<?xml version='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>

2. Config文件：

<?xml version='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>

• 添加库路径：

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

• 创建世界文件：

cd ~/gazebo_plugin_tutorial
gedit factory.world

<?xml version="1.0"?>
<sdf version="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

2）可编程的世界控制

• 创建世界文件：

cd ~/gazebo_plugin_tutorial
gedit world_edit.world

<?xml version ='1.0'?>
<sdf version ='1.4'>
  <world name='default'>
    <include>
      <uri>model://ground_plane</uri>
    </include>

    <include>
      <uri>model://sun</uri>
    </include>

    <plugin filename="libworld_edit.so" name="world_edit"/>
  </world>
</sdf>

• 创建插件文件：

gedit world_edit.cc

#include <sdf/sdf.hh>
#include <ignition/math/Pose3.hh>
#include "gazebo/gazebo.hh"
#include "gazebo/common/Plugin.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"

/// \example examples/plugins/world_edit.cc
/// This example creates a WorldPlugin, initializes the Transport system by
/// creating a new Node, and publishes messages to alter gravity.
namespace gazebo
{
  class WorldEdit : public WorldPlugin
  {
    public: void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
    {
      // 新建一个运输节点 Create a new transport node
      transport::NodePtr node(new transport::Node());

      // 用世界名称初始化节点 Initialize the node with the world name
      node->Init(_parent->Name());

      // 在~/physics主题上创建一个发布者 Create a publisher on the ~/physics topic
      transport::PublisherPtr physicsPub = node->Advertise<msgs::Physics>("~/physics");

      msgs::Physics physicsMsg;
      physicsMsg.set_type(msgs::Physics::ODE);

      // 设置步长时间 Set the step time
      physicsMsg.set_max_step_size(0.01);

      // 改变重力 Change gravity
      msgs::Set(physicsMsg.mutable_gravity(), ignition::math::Vector3d(0.01, 0, 0.1));
      physicsPub->Publish(physicsMsg);
    }
  };

  // Register this plugin with the simulator
  GZ_REGISTER_WORLD_PLUGIN(WorldEdit)
}

• 创建编译文件：底部添加代码

add_library(world_edit SHARED world_edit.cc)
target_link_libraries(world_edit ${GAZEBO_LIBRARIES}) 

• 编译文件：

cd ~/gazebo_plugin_tutorial/build
cmake ..
make

• 添加路径：

export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/gazebo_plugin_tutorial/build/

• 运行世界文件仿真：

cd ~/gazebo_plugin_tutorial
gazebo world_edit.world

总结

• 内容分析：本篇博客主要介绍了Gazebo中如何编写并完成插件的配置，分别用一个helloworld的简易插件讲述了编写插件的流程，并紧接着使用模型插件和世界插件的例子完成对于插件编写和配置的深入研究，内容更多关注于代码和配置流程，故可读性一般，理解需认真钻研。

• 注意：本文参考了Gazebo官方网站以及古月居中的Gazebo有关教程，主要目的是方便自行查询知识，巩固学习经验，无任何商业用途。