Gazebo中的传感器配置

传感器

注意： 本教程是在Gazebo Garden中使创建好的机器人动起来教程的延续。
在本教程中，我们将学习如何向我们的机器人和世界中的其他模型添加传感器。我们将使用三种不同的传感器：IMU 传感器、接触传感器和Lidar传感器。我们还将学习如何使用 gz launch 仅用一个文件启动多个任务。

IMU传感器

惯性测量单元（IMU）以四元数的形式输出机器人的方向，以及三个轴（X、Y、Z）的角速度和线加速度。我们使用 moving_robot.sdf 世界并对其进行修改。创建一个新文件 sensor_tutorial.sdf，并将 moving_robot.sdf 中的代码添加到其中。要定义 IMU 传感器，请在<world>标签下添加以下代码：

<plugin filename="gz-sim-imu-system"
        name="gz::sim::systems::Imu">
</plugin>

这段代码定义了将在我们的世界中使用的 IMU 传感器插件。现在我们可以按以下方式将 IMU 传感器添加到我们的机器人中：

<sensor name="imu_sensor" type="imu">
    <always_on>1</always_on>
    <update_rate>1</update_rate>
    <visualize>true</visualize>
    <topic>imu</topic>
</sensor>

传感器通常添加到我们模型的某个链接中；我们将其添加到chassis链接下。
让我们描述一下这些标签：

• <always_on>如果为 true，则传感器将始终根据更新率进行更新。
• <update_rate>生成传感器数据的频率。
• <visualize>如果为 true，则传感器将在 GUI 中可视化。
• <topic>发布数据的主题名称。

注意： 并非任意标签都支持所有传感器。

从IMU读取数据

要从 IMU 读取数据，请在一个终端中运行世界并按下播放按钮：

gz sim sensor_tutorial.sdf

在另一个终端中，运行：

gz topic -e -t /imu

最后一个命令监听通过 /imu 主题发送的消息。IMU 数据是如上所述的方向、角速度和线加速度。它看起来应该像这样：

使用键盘上的向上键向前移动机器人。你应该能看到传感器值发生变化。

接触传感器

让我们介绍另一种类型的传感器，接触传感器。从名称中你可以猜出，当它接触（接触）其他东西时，这个传感器会发出指示。我们将建造一个障碍物（墙）并将接触传感器添加到其中。如果机器人撞到障碍物，它将停止，以防止机器人损坏。让我们首先按照以下方式建造障碍物：

<model name='wall'>
    <static>true</static>
    <pose>5 0 0 0 0 0</pose><!--pose relative to the world-->
    <link name='box'>
        <visual name='visual'>
            <geometry>
                <box>
                    <size>0.5 10.0 2.0</size>
                </box>
            </geometry>
            <!--let's add color to our link-->
            <material>
                <ambient>0.0 0.0 1.0 1</ambient>
                <diffuse>0.0 0.0 1.0 1</diffuse>
                <specular>0.0 0.0 1.0 1</specular>
            </material>
        </visual>
        <collision name='collision'>
            <geometry>
                <box>
                    <size>0.5 10.0 2.0</size>
                </box>
            </geometry>
        </collision>
    </link>
</model>

这只是一个简单的模型，只有一个盒状链接。你可以查看“在Gazebo Garden中创建一个机器人”教程来学习如何构建模型。
现在运行世界，并确保墙壁像这样出现在模拟中：

让我们将接触传感器添加到墙上。与 IMU 传感器一样，我们首先应该通过添加以下代码来定义接触传感器：

<plugin filename="gz-sim-contact-system"
        name="gz::sim::systems::Contact">
</plugin>

现在我们可以将contact传感器添加到wall模型的box链接中：

<sensor name='sensor_contact' type='contact'>
    <contact>
        <collision>collision</collision>
    </contact>
</sensor>

<sensor>的定义很简单，我们只定义了传感器的名称和类型。在collision内部，我们定义了box链接的碰撞的名称，即collision。
我们还需要在墙壁模型下添加 TouchPlugin，如下所示：

<plugin filename="gz-sim-touchplugin-system"
        name="gz::sim::systems::TouchPlugin">
    <target>vehicle_blue</target>
    <namespace>wall</namespace>
    <time>0.001</time>
    <enabled>true</enabled>
</plugin>

TouchPlugin 将在墙壁被碰触时发布（发送）一条消息。插件的标签如下：

• <target>将与我们的墙壁接触，在我们的例子中是 vehicle_blue。
• <namespace>采用我们的主题的命名空间，因此当我们的机器人撞到墙壁时，它将向 /wall/touched 话题发送一条消息。
  在一个终端中运行仿真世界：

gz sim sensor_tutorial.sdf

在另一个终端中，监听 /wall/touched 主题：

gz topic -e -t /wall/touched

使用键盘箭头键将机器人向前驱向墙壁。确保通过点击播放按钮开始仿真，并通过单击插件下拉列表（垂直省略号）启用 Key Publisher 插件。
当机器人撞到障碍物时，你应该在运行 gz topic -e -t /wall/touched 的终端上看到一条消息 data: true。
现在我们可以使用 TriggeredPublisher 插件让我们的机器人在撞到墙壁时停止，如下所示：

<plugin filename="gz-sim-triggered-publisher-system"
        name="gz::sim::systems::TriggeredPublisher">
    <input type="gz.msgs.Boolean" topic="/wall/touched">
        <match>data: true</match>
    </input>
    <output type="gz.msgs.Twist" topic="/cmd_vel">
        linear: {x: 0.0}, angular: {z: 0.0}
    </output>
</plugin>

如在Gazebo Garden中使创建好的机器人动起来中所述，我们可以根据接收到的输入发布输出。因此，当我们在 /wall/touched 主题上接收到 data: true 时，我们发布 linear: {x: 0.0}, angular: {z: 0.0} 以使我们的机器人停止。

Lidar传感器

我们不希望机器人碰到墙壁，因为这可能会造成一些损坏，所以我们可以使用激光雷达而不是接触传感器。Lidar是“light detection and ranging（光探测和测距）”的缩写。该传感器可以帮助我们检测机器人周围的障碍物。我们将使用它来测量机器人与墙壁之间的距离。
首先，我们创建一个Frame来固定Lidar。这应该添加到 vehicle_blue 的<model>标记内，因为lidar frame附加在机器人的底盘（chassis）上：

<frame name="lidar_frame" attached_to='chassis'>
    <pose>0.8 0 0.5 0 0 0</pose>
</frame>

然后在<world>标签下添加此插件，以便能够使用lidar传感器：

    <plugin
      filename="gz-sim-sensors-system"
      name="gz::sim::systems::Sensors">
      <render_engine>ogre2</render_engine>
    </plugin>

在chassis链接下，我们可以添加lidar传感器，如下所示：

<sensor name='gpu_lidar' type='gpu_lidar'>"
    <pose relative_to='lidar_frame'>0 0 0 0 0 0</pose>
    <topic>lidar</topic>
    <update_rate>10</update_rate>
    <ray>
        <scan>
            <horizontal>
                <samples>640</samples>
                <resolution>1</resolution>
                <min_angle>-1.396263</min_angle>
                <max_angle>1.396263</max_angle>
            </horizontal>
            <vertical>
                <samples>1</samples>
                <resolution>0.01</resolution>
                <min_angle>0</min_angle>
                <max_angle>0</max_angle>
            </vertical>
        </scan>
        <range>
            <min>0.08</min>
            <max>10.0</max>
            <resolution>0.01</resolution>
        </range>
    </ray>
    <always_on>1</always_on>
    <visualize>true</visualize>
</sensor>

• 首先，我们定义了传感器的名称和类型，然后定义了它相对于 lidar_frame 的<pose>。
• 在<topic>中，我们定义了 lidar 数据将发布的话题。
• <update_rate>是 lidar 数据生成的频率，在我们的例子中是 10 Hz，即 0.1 秒。
• 在<horizontal>和<vertical>标签下，我们定义了水平和垂直激光射线的属性。
• <samples>是每个完整激光扫射周期生成的模拟 lidar 射线的数量。
• <resolution>：该数字乘以 samples 来确定数据点的数量范围。
• <min_angle>和<max_angle>是生成射线的角度范围。
• 在<range>下，我们定义了每个模拟射线的范围属性
  • <min>和<max>定义了每条 lidar 射线的最小和最大距离。
  • <resolution>标签在此处定义了每条 lidar 射线的线性分辨率。
• <always_on>：如果为 true，则传感器将根据<update_rate>始终更新。
• <visualize>：如果为 true，则传感器将在 GUI 中可视化。
  现在运行世界并按左下角的播放按钮：

gz sim sensor_tutorial.sdf

查看 lidar 主题上的 lidar 消息，特别是范围数据：

gz topic -e -t /lidar

lidar 消息具有以下属性：

message LaserScan
{
  Header header              = 1;

  string frame               = 2;
  Pose world_pose            = 3;
  double angle_min           = 4;
  double angle_max           = 5;
  double angle_step          = 6;
  double range_min           = 7;
  double range_max           = 8;
  uint32 count               = 9;
  double vertical_angle_min  = 10;
  double vertical_angle_max  = 11;
  double vertical_angle_step = 12;
  uint32 vertical_count      = 13;

  repeated double ranges              = 14;
  repeated double intensities         = 15;
}

避开墙壁

现在我们的机器人有了激光雷达，我们可以使用范围数据来让我们的机器人避免撞到墙上。为此，我们将编写一个简短的 C++程序，该程序监听传感器数据并向机器人发送速度命令。这个程序被称为节点。我们将构建一个节点，订阅 lidar 话题并读取其数据。看看这个教程，学习如何构建发布者和订阅者节点。你可以从这里下载这个演示的完成节点。

lidar_node节点

gz::transport::Node node;
std::string topic_pub = "/cmd_vel";
gz::msgs::Twist data;
auto pub = node.Advertise<gz::msgs::Twist>(topic_pub);

我们声明一个将发布到 cmd_vel 话题的节点，并定义了消息类型 Twist。然后宣传我们的节点。

void cb(const gz::msgs::LaserScan &_msg)
{
  bool allMore = true;
  for (int i = 0; i < _msg.ranges_size(); i++)
  {
    if (_msg.ranges(i) < 1.0)
    {
      allMore = false;
      break;
    }
  }
  if (allMore) //if all bigger than one
  {
    data.mutable_linear()->set_x(0.5);
    data.mutable_angular()->set_z(0.0);
  }
  else
  {
    data.mutable_linear()->set_x(0.0);
    data.mutable_angular()->set_z(0.5);
  }
  pub.Publish(data);
}

在回调函数内部，我们检查所有射线的范围是否大于 1.0。如果是这样，我们将向机器人发布一个前进的消息。否则，让机器人旋转。

int main(int argc, char **argv)
{
    std::string topic = "/lidar";
    // Subscribe to a topic by registering a callback.
    if (!node.Subscribe(topic, cb))
    {
        std::cerr << "Error subscribing to topic [" << topic << "]" << std::endl;
        return -1;
    }

    // Zzzzzz.
    gz::transport::waitForShutdown();

    return 0;
}

在 main 内部，我们订阅 lidar 话题，并等待节点关闭。

构建节点

下载 CMakeLists.txt，并在 lidar_node 所在的同一文件夹中创建 build/ 目录：

mkdir build
cd build

执行cmka命令并构建：

cmake ..
make lidar_node

运行代码

在终端1运行节点：

./build/lidar_node

在终端2执行：

gz sim sensor_tutorial.sdf

现在你可以看到机器人向前移动，当它接近墙壁时，它开始向左转动，直到它清除并再次向前移动（确保按下左下角的播放按钮以使机器人开始移动）。

启动Gazebo

与其从两个不同的终端运行两个不同的任务，我们可以制作一个启动文件，同时运行 sensor_world 和 lidar_node。打开你的文本编辑器并添加以下代码。

<?xml version='1.0'?>
<gz version='1.0'>
  <executable name='sensor-world'>
    <command>gz sim sensor_tutorial.sdf</command>
  </executable>

  <executable name='lidar_node'>
    <command>./build/lidar_node</command>
  </executable>

</gz>

启动文件是一个 XML 文件。我们只需在<executable>标签下定义将运行的命令。第一个命令是 gz sim sensor_tutorial.sdf，它启动仿真世界。第二个命令是 ./build/lidar_node，它运行 lidar_node。将文件保存为 sensor_launch.gzlaunch，然后通过以下命令运行它：

gz launch sensor_launch.gzlaunch

按播放按钮开始模拟。我们的机器人现在正在移动并避开墙壁。
要使仿真更加复杂，请在下一个教程中学习如何向仿真世界添加演员。