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

编程语言 2023-04-08 00:15:45 阅读次数: 0

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

在这里插入图片描述

\qquad

文章目录

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


一、系统插件

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

  • 创建并编写插件文件：

  1. 创建插件文件：
cd ~/gazebo_plugin_tutorial
gedit system_gui.cc
  1. 编写插件文件：
#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})
  1. 编译插件：
cd ~/gazebo_plugin_tutorial/build
cmake ..
make
  • 启动并运行服务器、客户端和插件：
gzserver & 
gzclient -g libsystem_gui.so
  • 使用效果：
  1. 在tmp/gazebo_frames目录下，应该会出现一些照片

在这里插入图片描述

  1. 在同一个终端输入如下代码终止后台运行的程序fg
  2. 按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
  1. 编写世界文件：
<?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>
  1. 构造目录并编译文件：
cd ~/gazebo_plugin_tutorial/build/
cmake ..
make
  1. 添加库路径并从build目录中运行gazebo：
cd ~/velodyne_plugin/build
cexport LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:~/velodyne_plugin/build
cgazebo ../velodyne.world
  1. 效果展示：
The velodyne plugin is attached to model[my_velodyne]

3. 插件配置

  • 插件配置：使用PID控制传感器关节速度
  1. 修改插件源文件
gedit ~/velodyne_plugin/velodyne_plugin.cc
  1. 修改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);
}
  1. 将如下私有成员添加到对应类中：
/// \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;
  1. 在世界文件中配置插件，读取自定义SDF参数：
gedit ~/gazebo_plugin_tutorial/velodyne.world
  1. 修改标签来包含一个元素：
<plugin name="velodyne_control" filename="libvelodyne_plugin.so">
 <velocity>25</velocity>
</plugin>
  1. 重新修改插件文件中的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);
  1. 效果： 重新编译并运行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的教程等，主要目的是方便自行查询知识，巩固学习经验，无任何商业用途。

猜你喜欢

转载自blog.csdn.net/lc1852109/article/details/126478874
今日推荐
《美国对全球网络空间安全与发展的威胁和破坏》报告发布
火速冲上 GitHub 热榜 —— 开源编程语言、框架哪有这么可爱？
北京人形机器人创新中心发布全球首个纯电驱拟人奔跑的全尺寸人形机器人“天工”
LFOSSA 源来如此公开课 | 掌握云原生未来：CNCF 认证全面攻略与备考秘籍
国产云输入法——仅华为无云端数据上传安全问题
周排行
Python环境安装与基础语法（1）——计算机基础知识
IMU预积分
ADAS中的LDW、FCW、BSD、LCA、ACC、AEB、APA、DMS代表的含义
B站笔试两道题
skyeye arm 硬件虚拟机环境的搭建
Web前端静态页面示例
数组-合并排序数组 II-简单
springcloud之版本问题启动报错
面向对象-------------匿名对象(六)
输入URL到页面呈现中间发生了什么？
每日归档
更多
2024-04-30(1)
2024-04-29(40)
2024-04-28(0)
2024-04-27(56)
2024-04-26(39)
2024-04-25(22)
2024-04-24(36)
2024-04-23(26)
2024-04-22(39)
2024-04-21(0)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022