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

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

\qquad

前言：在上一篇博客中，我们首先了解了控制器插件的具体使用方法和配置流程，采用多个实例了解了模型插件和世界插件等的具体使用方法，本节博客将继续深入体会插件的功能效用，以两个实例重点介绍系统插件和传感器插件的配置方法，其中传感器插件的配置是重点，与之前的教程一致，可统一学习，最后通过创造API实现动态调整。

一、系统插件

• 目标：给gzclient设计系统插件，将图像存储到/tmp/gazebo_frames目录下方；

• 创建并编写插件文件：

1. 创建插件文件：

cd ~/gazebo_plugin_tutorial
gedit system_gui.cc 

2. 编写插件文件：

#include <functional>
#include <gazebo/gui/GuiIface.hh>
#include <gazebo/rendering/rendering.hh>
#include <gazebo/gazebo.hh>

namespace gazebo
{
 class SystemGUI : public SystemPlugin
 {
   // Destructor
   public: virtual ~SystemGUI()
   {
     this->connections.clear();
     if (this->userCam)
       this->userCam->EnableSaveFrame(false);
     this->userCam.reset();
   }

   // 在启动时，Gazebo加载之前，Load和Init会在插件被构造后调用，且二者必须不能阻塞(must not block) 
   public: void Load(int /*_argc*/, char ** /*_argv*/)
   {
     this->connections.push_back(
         event::Events::ConnectPreRender(
           std::bind(&SystemGUI::Update, this)));
   }

   // 只在`Load`后调用一次
   private: void Init()
   {
   }

   // 每次PreRender事件都会调用，看`Load`函数
   private: void Update()
   {
     if (!this->userCam)
     {
       // 得到一个激活用户相机的指针
       this->userCam = gui::get_active_camera();

       // 开启帧的保存
       this->userCam->EnableSaveFrame(true);

       // 指定要保存帧的路径
       this->userCam->SetSaveFramePathname("/tmp/gazebo_frames");
     }

     // 得到scene的指针
     rendering::ScenePtr scene = rendering::get_scene();

     // 等待，直到scene被初始化了
     if (!scene || !scene->Initialized())
       return;

     // 通过名字寻找一个特定的图像
     if (scene->GetVisual("ground_plane"))
       std::cout << "Has ground plane visual\n";
   }

   // 申明用户相机的指针
   private: rendering::UserCameraPtr userCam;

   // 申明存储所有事件连接的vector
   private: std::vector<event::ConnectionPtr> connections;
 };

 // 和模拟器上注册插件
 GZ_REGISTER_SYSTEM_PLUGIN(SystemGUI)
}

• 增加编译规则并进行编译：

1. 修改编译规则（底部添加）：

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

2. 编译插件：

cd ~/gazebo_plugin_tutorial/build
cmake ..
make

• 启动并运行服务器、客户端和插件：

gzserver & 
gzclient -g libsystem_gui.so

• 使用效果：

1. 在tmp/gazebo_frames目录下，应该会出现一些照片

2. 在同一个终端输入如下代码终止后台运行的程序fg
3. 按Ctrl+C终止进程

二、Velodyne传感器插件

1. 基本插件文件创建

• 创建工作区：

mkdir ~/velodyne_plugin
cd ~/velodyne_plugin

• 创建插件源文件：

gedit velodyne_plugin.cc

• 编写插件源文件：

#ifndef _VELODYNE_PLUGIN_HH_
#define _VELODYNE_PLUGIN_HH_

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>

namespace gazebo
{
  /// \brief A plugin to control a Velodyne sensor.
  class VelodynePlugin : public ModelPlugin
  {
    /// \brief Constructor
    public: VelodynePlugin() {}

    /// \brief The load function is called by Gazebo when the plugin is
    /// inserted into simulation
    /// \param[in] _model A pointer to the model that this plugin is
    /// attached to.
    /// \param[in] _sdf A pointer to the plugin's SDF element.
    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
    {
      // Just output a message for now
      std::cerr << "\nThe velodyne plugin is attach to model[" <<
        _model->GetName() << "]\n";
    }
  };

  // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
  GZ_REGISTER_MODEL_PLUGIN(VelodynePlugin)
}
#endif

• 创建编译规则文件构建脚本：

gedit CMakeLists.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

# Find Gazebo
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")

# Build our plugin
add_library(velodyne_plugin SHARED velodyne_plugin.cc)
target_link_libraries(velodyne_plugin ${GAZEBO_LIBRARIES})

2. 插件连接与测试

• 将插件附加到SDF文件中，连接到传感器，并通过include功能进行测试：

1. 创建世界文件：

gedit velodyne.world 

2. 编写世界文件：

<?xml version="1.0" ?>
<sdf version="1.5">
 <world name="default">
   <!-- A global light source -->
   <include>
     <uri>model://sun</uri>
   </include>
   <!-- A ground plane -->
   <include>
     <uri>model://ground_plane</uri>
   </include>

   <!-- A testing model that includes the Velodyne sensor model -->
   <model name="my_velodyne">
     <include>
       <uri>model://velodyne_hdl32</uri>
     </include>

     <!-- Attach the plugin to this model -->
     <plugin name="velodyne_control" >filename="libvelodyne_plugin.so"/>
   </model>

 </world>
</sdf>

3. 构造目录并编译文件：

cd ~/gazebo_plugin_tutorial/build/
cmake ..
make

4. 添加库路径并从build目录中运行gazebo：

cd ~/velodyne_plugin/build
cexport LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:~/velodyne_plugin/build
cgazebo ../velodyne.world

5. 效果展示：

The velodyne plugin is attached to model[my_velodyne]

3. 插件配置

• 插件配置：使用PID控制传感器关节速度

1. 修改插件源文件

gedit ~/velodyne_plugin/velodyne_plugin.cc

2. 修改Load函数，代码如下：

public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
 // Safety check
 if (_model->GetJointCount() == 0)
 {
   std::cerr << "Invalid joint count, Velodyne plugin not loaded\n";
   return;
 }

 // Store the model pointer for convenience.
 this->model = _model;

 // Get the first joint. We are making an assumption about the model
 // having one joint that is the rotational joint.
 this->joint = _model->GetJoints()[0];

 // Setup a P-controller, with a gain of 0.1.
 this->pid = common::PID(0.1, 0, 0);

 // Apply the P-controller to the joint.
 this->model->GetJointController()->SetVelocityPID(
     this->joint->GetScopedName(), this->pid);

 // Set the joint's target velocity. This target velocity is just
 // for demonstration purposes.
 this->model->GetJointController()->SetVelocityTarget(
     this->joint->GetScopedName(), 10.0);
}

3. 将如下私有成员添加到对应类中：

/// \brief Pointer to the model.
private: physics::ModelPtr model;

/// \brief Pointer to the joint.
private: physics::JointPtr joint;

/// \brief A PID controller for the joint.
private: common::PID pid;

4. 在世界文件中配置插件，读取自定义SDF参数：

gedit ~/gazebo_plugin_tutorial/velodyne.world

5. 修改标签来包含一个元素：

<plugin name="velodyne_control" filename="libvelodyne_plugin.so">
 <velocity>25</velocity>
</plugin>

6. 重新修改插件文件中的Load函数的底部，使用 sdf::ElementPtr参数来读取：

// 默认速度为0
double velocity = 0;

// 检查是否<velocity>元素存在，然后读取数值
if (_sdf->HasElement("velocity"))
 velocity = _sdf->Get<double>("velocity");

// 设置关节的目标速度
this->model->GetJointController()->>SetVelocityTarget(this->joint->GetScopedName(), >velocity); 

7. 效果： 重新编译并运行gazebo，修改，传感器应进行旋转

cd ~/velodyne_plugin/build
cmake ..
make
gazebo --verbose ../velodyne.world

三、创造API

1. 基本概念

• 目的：动态调整目标速度，无需手动修改SDF文件
• 分类：此处可以实现两种API类型：消息传递，和函数【此处我们可以同时实现】

1. 消息传递：
   依赖于Gazebo的传输机制，它将涉及创建一个命名的主题，发布者可以在该主题上发送double值。这样插件将接受到这些消息，并正确地设置速度值。对于进程间通信，消息传递是很方便的。
2. 函数法：
   新建一个公共函数来调整速度值。一个新的插件将继承我们当前的插件。子级插件将被实例化(而不是我们当前的插件)，通过调用函数，我们可以控制速度。当Gazebo与ROS交互时，这一方法最常用。

2. 方法具体实现

• 2.1 打开插件文件：

gedit ~/gazebo_plugin_tutorial/velodyne_plugin.cc

• 2.2 新建一个可以设置目标速度的公共函数：

/// \brief Set the velocity of the Velodyne
/// \param[in] _vel New target velocity
public: void SetVelocity(const double &_vel)
{
  // Set the joint's target velocity.
  this->model->GetJointController()->SetVelocityTarget(this->joint->GetScopedName(), _vel);
}

• 2.3 在插件中添加一个节点和订阅者，设置消息结构：

/// \brief A node used for transport
private: transport::NodePtr node;

/// \brief A subscriber to a named topic.
private: transport::SubscriberPtr sub;

• 2.4 在Load函数底部实例化节点和订阅者，node和 subscriber：

// 创造节点
this->node = transport::NodePtr(new transport::Node());
#if GAZEBO_MAJOR_VERSION < 8
this->node->Init(this->model->GetWorld()->GetName());
#else
this->node->Init(this->model->GetWorld()->Name());
#endif

// 创造一个主题名
std::string topicName = "~/" + this->model->GetName() + "/vel_cmd";

// 订阅这个主题，并且注册一个回调
this->sub = this->node->Subscribe(topicName, &VelodynePlugin::OnMsg, this); 

• 2.5 新建回调函数来处理接受到的信息：

/// \brief Handle incoming message
/// \param[in] _msg Repurpose a vector3 message. This function will
/// only use the x component.
private: void OnMsg(ConstVector3dPtr &_msg)
{
  this->SetVelocity(_msg->x());
}

• 2.6 给消息传递机制添加两个额外头文件：

#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

• 2.7 完整代码如下：

#ifndef _VELODYNE_PLUGIN_HH_
#define _VELODYNE_PLUGIN_HH_

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

namespace gazebo
{
  /// \brief A plugin to control a Velodyne sensor.
  class VelodynePlugin : public ModelPlugin
  {
    /// \brief Constructor
    public: VelodynePlugin() {}

    /// \brief The load function is called by Gazebo when the plugin is
    /// inserted into simulation
    /// \param[in] _model A pointer to the model that this plugin is
    /// attached to.
    /// \param[in] _sdf A pointer to the plugin's SDF element.
    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
    {
      // Safety check
      if (_model->GetJointCount() == 0)
      {
        std::cerr << "Invalid joint count, Velodyne plugin not loaded\n";
        return;
      }

      // Store the model pointer for convenience.
      this->model = _model;

      // Get the first joint. We are making an assumption about the model
      // having one joint that is the rotational joint.
      this->joint = _model->GetJoints()[0];

      // Setup a P-controller, with a gain of 0.1.
      this->pid = common::PID(0.1, 0, 0);

      // Apply the P-controller to the joint.
      this->model->GetJointController()->SetVelocityPID(
          this->joint->GetScopedName(), this->pid);

      // Default to zero velocity
      double velocity = 0;

      // Check that the velocity element exists, then read the value
      if (_sdf->HasElement("velocity"))
        velocity = _sdf->Get<double>("velocity");

      this->SetVelocity(velocity);

      // Create the node
      this->node = transport::NodePtr(new transport::Node());
      #if GAZEBO_MAJOR_VERSION < 8
      this->node->Init(this->model->GetWorld()->GetName());
      #else
      this->node->Init(this->model->GetWorld()->Name());
      #endif

      // Create a topic name
      std::string topicName = "~/" + this->model->GetName() + "/vel_cmd";

      // Subscribe to the topic, and register a callback
      this->sub = this->node->Subscribe(topicName,
         &VelodynePlugin::OnMsg, this);
    }

    /// \brief Set the velocity of the Velodyne
    /// \param[in] _vel New target velocity
    public: void SetVelocity(const double &_vel)
    {
      // Set the joint's target velocity.
      this->model->GetJointController()->SetVelocityTarget(
          this->joint->GetScopedName(), _vel);
    }

    /// \brief Handle incoming message
    /// \param[in] _msg Repurpose a vector3 message. This function will
    /// only use the x component.
    private: void OnMsg(ConstVector3dPtr &_msg)
    {
      this->SetVelocity(_msg->x());
    }

    /// \brief A node used for transport
    private: transport::NodePtr node;

    /// \brief A subscriber to a named topic.
    private: transport::SubscriberPtr sub;

    /// \brief Pointer to the model.
    private: physics::ModelPtr model;

    /// \brief Pointer to the joint.
    private: physics::JointPtr joint;

    /// \brief A PID controller for the joint.
    private: common::PID pid;
  };

  // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
  GZ_REGISTER_MODEL_PLUGIN(VelodynePlugin)
}
#endif

3. 测试API

• 在gazebo_plugin_tutorial目录下新建源文件：

gedit ~/velodyne_plugin/vel.cc

• 编写API文件：

#include <gazebo/gazebo_config.h>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

// Gazebo's API has changed between major releases. These changes are
// accounted for with #if..#endif blocks in this file.
#if GAZEBO_MAJOR_VERSION < 6
#include <gazebo/gazebo.hh>
#else
#include <gazebo/gazebo_client.hh>
#endif

int main(int _argc, char **_argv)
{
  // 将Gazebo加载为客户端
#if GAZEBO_MAJOR_VERSION < 6
  gazebo::setupClient(_argc, _argv);
#else
  gazebo::client::setup(_argc, _argv);
#endif

  // 为了通信，创建我们的节点
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  // 发布到velodyne传感器的主题
  gazebo::transport::PublisherPtr pub =
    node->Advertise<gazebo::msgs::Vector3d>("~/my_velodyne/vel_cmd");

  // 等待订阅者连接到发布者
  pub->WaitForConnection();

  // 创建一个vector3消息
  gazebo::msgs::Vector3d msg;

  // 设置x方向的速度
#if GAZEBO_MAJOR_VERSION < 6
  gazebo::msgs::Set(&msg, gazebo::math::Vector3(std::atof(_argv[1]), 0, 0));
#else
  gazebo::msgs::Set(&msg, ignition::math::Vector3d(std::atof(_argv[1]), 0, 0));
#endif

  // 发送消息
  pub->Publish(msg);

  // 确保所有都关闭了
#if GAZEBO_MAJOR_VERSION < 6
  gazebo::shutdown();
#else
  gazebo::client::shutdown();
#endif
}

• 添加编译规则（底部）：

# Build the stand-alone test program
add_executable(vel vel.cc)

if (${gazebo_VERSION_MAJOR} LESS 6)
  # These two
  include(FindBoost)
  find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem regex)
  target_link_libraries(vel ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
else()
  target_link_libraries(vel ${GAZEBO_LIBRARIES})
endif()

• 编译程序：

cd ~/gazebo_plugin_tutorial/build
cmake ..
make
gazebo --verbose ../velodyne.world

• 新建终端，进入文件所在目录并运行vel命令，确保设置数值，该数值被解释为目标速度值。

cd ~/gazebo_plugin_tutorial/build/
./vel 2

总结

• 内容分析：本篇博客主要介绍了Gazebo中系统插件的使用和配置方法，并且重点从头到尾分析研究了velodyne传感器插件的配置、设计、测试流程，最后针对于插件调整设计了两种API，完成了编程后的便捷使用，与上节博客一起完成了对于Gazebo仿真插件的使用教程介绍。

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