gazebo教程(五)添加传感器噪声& 简单控制插件

204
0
2020年12月30日 09时08分

文章目录

 

  • 传感器噪音
  • Step1 可视化传感器数据
  • Step2 添加噪声
  • 一、介绍
    • 1、插件概述
    • 2、安装依赖
  • 二、编写插件
    • 步骤1:创建工作区
    • 步骤2:创建插件源文件
    • 步骤3:创建CMake构建脚本
    • 步骤4:将插件连接到Velodyne传感器
    • 步骤5:构建和测试
  • 四、移动Velodyne

 

传感器噪音

 

本教程将通过添加噪声来改善传感器的输出。几乎每个传感器的输出端都有噪声。例如相机可能具有色差,声纳多径效应以及激光不正确的距离读数。我们必须将噪声添加到模拟生成的数据中,以便更紧密地匹配真实传感器生成的数据类型。

 

gazebo有一个内置的噪声模型,可以将高斯噪声应用于各种传感器。尽管高斯噪声有时不太现实,但它总比没有好。尽管如此,在很多情况下它可以很好地对噪声值进行近似估计,并且高斯噪声也相对容易应用于数据流。

 

Step1 可视化传感器数据

 

让我们从查看当前Velodyne输出开始,然后我们可以添加噪声。

 

1、打开凉亭,然后插入Velodyne传感器。

  • gazebo
  • 选择左上方附近的“插入”选项卡。
  • 向下滚动并选择Velodyne HDL-32型号。
  • 单击左键选定传感器放置位置。

 

2、在激光束前面添加一个方框,以便获得有用的数据。

  • 在渲染窗口上方的工具栏中选择Box图标。
  • 单击左键选定箱子放置位置。

 

微信图片_20201222223956

 

3、我们可以通过Gazebo的主题可视化器仔细查看传感器数据。按Ctrl-t,打开主题选择器。找到/gazebo/default/velodyne/top/sensor/scan主题。

 

微信图片_20201222223958

 

选择/gazebo/default/velodyne/top/sensor/scan,然后按确定以打开激光可视化仪。我们可以看到激光线条十分平滑。

 

微信图片_20201222224001

 

Step2 添加噪声

 

gazebo的噪声模型可以使用标签访问。

 

1、打开Velodyne模型。

 

gedit ~/.gazebo/models/velodyne_hdl32/model.sdf

 

2、将<noise>元素添加为<ray>元素的子元素。首先,我们将施加大量噪声,以便可以轻松看到效果。

 

<sensor type="ray" name="sensor">
  <pose>0 0 -0.004645 1.5707 0 0</pose>
  <visualize>true</visualize>
  <ray>
    <noise>
      <!-- Use gaussian noise -->
      <type>gaussian</type>
      <mean>0.0</mean>
      <stddev>0.1</stddev>
    </noise>

 

3、重新打开可视化器(Ctrl-t),然后选择 Velodyne laser scan。这是后可以发现输出看起来已经没有刚才那么平滑了,我们可以通过修改meanstddev,来修改噪声幅度。

 

微信图片_20201222224121

 


到目前为止,我们有一个功能齐全的传感器。模型组件已准备就绪,并且已应用了高斯噪声模型。最后要添加的组件是控制传感器的一个自由度的插件。该插件将控制传感器上部的旋转。

 

一、介绍

 

1、插件概述

 

插件是一个由Gazebo在运行时加载的C ++库。插件可以访问Gazebo的API,该API允许插件执行各种各样的任务,包括移动对象,添加/删除对象以及访问传感器数据。

 

2、安装依赖

 

在某些操作系统上,必须在构建插件之前安装开发包。

 

# Ubuntu or Debian
sudo apt install libgazebo8-dev
# Fedora
sudo yum install gazebo-devel

 

二、编写插件

我们将在新目录中创建插件。该目录的内容将包括插件源代码和CMake构建脚本。
为了使本教程简短,我们不会详细介绍插件的各个组件。查看这些教程以获取更多信息。

ps:后续会补上插件相关教程,如果实在有需要的小伙伴可以去查看官方教程
步骤1:创建工作区

 

mkdir ~/velodyne_plugin
cd ~/velodyne_plugin

 

步骤2:创建插件源文件

 

我们将从一个简单的插件开始,在工作空间中,创建源文件。

 

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

 

步骤3:创建CMake构建脚本

 

在工作空间中创建一个文件。CMakeLists.txt

 

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})

 

步骤4:将插件连接到Velodyne传感器

 

我们将利用SDF的<include>功能来测试我们的插件,而无需改变Velodyne SDF主体内容。

在工作空间内,创建一个新的世界文件。

 

gedit velodyne.world

 

复制以下SDF内容:

 

<?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>

 

步骤5:构建和测试

 

现在,我们准备编译插件并对其进行测试。在您的工作空间中,创建一个构建目录。

 

mkdir build

 

编译插件。

 

cd build
cmake ..
make

 

在gazebo中运行.world文件。注意:从build目录中运行gazebo很重要,这样Gazebo才能找到插件库。

 

  • gazebo版本<6

 

cd ~/velodyne_plugin/build
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:~/velodyne_plugin/build
gazebo ../velodyne.world

 

  • gazebo版本> = 6

 

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

 

检查您的终端,您应该看到:

 

The velodyne plugin is attached to model[my_velodyne]

 

四、移动Velodyne

 

现在,我们有了一个基本的插件,可以由Gazebo编译和运行。下一步是添加控制Velodyne关节的代码。我们将使用一个简单的PID控制器来控制Velodyne关节的速度。在工作空间中打开源文件。

 

gedit ~/velodyne_plugin/velodyne_plugin.cc

 

修改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);
}

 

并将以下私有成员添加到该类的正下方:

 

/// \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;

 

重新编译并运行gazebo,这时我们应该能看到传感器旋转了:

 

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

 

发表评论

后才能评论