传感器噪音

  本教程将通过添加噪声来改善传感器的输出。几乎每个传感器的输出端都有噪声。例如相机可能具有色差，声纳多径效应以及激光不正确的距离读数。我们必须将噪声添加到模拟生成的数据中，以便更紧密地匹配真实传感器生成的数据类型。   gazebo有一个内置的噪声模型，可以将高斯噪声应用于各种传感器。尽管高斯噪声有时不太现实，但它总比没有好。尽管如此，在很多情况下它可以很好地对噪声值进行近似估计，并且高斯噪声也相对容易应用于数据流。  

Step1 可视化传感器数据

  让我们从查看当前Velodyne输出开始，然后我们可以添加噪声。   1、打开凉亭，然后插入Velodyne传感器。

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

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

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

 
  3、我们可以通过Gazebo的主题可视化器仔细查看传感器数据。按Ctrl-t，打开主题选择器。找到/gazebo/default/velodyne/top/sensor/scan主题。  
  选择/gazebo/default/velodyne/top/sensor/scan，然后按确定以打开激光可视化仪。我们可以看到激光线条十分平滑。  
 

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。这是后可以发现输出看起来已经没有刚才那么平滑了，我们可以通过修改mean 和 stddev，来修改噪声幅度。  
 

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

一、介绍

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