Gazebo之Sensors简介

1. 源由

本章将学习如何将传感器添加到机器人仿真世界中。

• 典型的三种不同传感器：IMU传感器、接触传感器和激光雷达传感器。
• 同时，还将学习如何使用 gz launch 通过一个文件启动多个任务。

2. 初步准备

默认插件不会加载。因此，在添加传感器之前，首先需要添加一些逻辑上的默认设置，以便使用 GZ GUI：

<sdf version='1.9'>
  <world name='demo'>
    <plugin
        filename="gz-sim-physics-system"
        name="gz::sim::systems::Physics">
    </plugin>
    <plugin
        filename="gz-sim-scene-broadcaster-system"
        name="gz::sim::systems::SceneBroadcaster">
    </plugin>

    <!-- ... -->

测试资料：SnapLearnGazebo/lesson_02_sensor

3. 传感器

使用 moving_robot.sdf 作为基础SDF文件，创建一个新的文件 sensor_tutorial.sdf，在上面逐步添加上述传感器。

3.1 IMU 传感器

惯性测量单元 (IMU) 输出三个轴 (X, Y, Z) 的角速度和三个轴上的线性加速度。

Step 1: 引用 IMU 传感器

在 标签下添加以下代码：

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

Step 2: 添加 IMU 传感器

添加到机器人世界中：

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

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

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

Step 3: 读取 IMU 传感数据

1. 在一个终端中运行世界并按下播放按钮：

$ gz sim sensor_tutorial.sdf

2. 在另一个终端中执行以下命令：

$ gz topic -e -t /imu

3. 使用键盘的向上键移动你的机器人前进，可以看到传感器值在变化。

$ gz topic -e -t /imu
header {
  stamp {
    nsec: 1000000
  }
  data {
    key: "frame_id"
    value: "vehicle_blue::chassis::imu_sensor"
  }
  data {
    key: "seq"
    value: "0"
  }
}
entity_name: "vehicle_blue::chassis::imu_sensor"
orientation {
  x: 2.1215887269564552e-22
  y: 5.4613557001741309e-23
  z: 4.2036654388084205e-24
  w: 1
}
orientation_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}
angular_velocity {
  x: 4.24317745391291e-19
  y: 1.0922711400348261e-19
  z: 8.4073308776168415e-21
}
angular_velocity_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}
linear_acceleration {
  x: 3.5127917569011972e-18
  y: -2.8126787673456782e-17
}
linear_acceleration_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}

header {
  stamp {
    sec: 1
  }
  data {
    key: "frame_id"
    value: "vehicle_blue::chassis::imu_sensor"
  }
  data {
    key: "seq"
    value: "1"
  }
}
entity_name: "vehicle_blue::chassis::imu_sensor"
orientation {
  x: -3.1711735761273735e-06
  y: 5.1765558419587438e-06
  z: 0.0077879738067922165
  w: 0.99996967325371
}
orientation_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}
angular_velocity {
  x: 6.3239137476902e-05
  y: -9.01573735557911e-05
  z: 6.8026775775727753e-10
}
angular_velocity_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}
linear_acceleration {
  x: 0.00023964047644863543
  y: 0.00019414434201520824
  z: 9.7991386189340943
}
linear_acceleration_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}

header {
  stamp {
    sec: 2
  }
  data {
    key: "frame_id"
    value: "vehicle_blue::chassis::imu_sensor"
  }
  data {
    key: "seq"
    value: "2"
  }
}
entity_name: "vehicle_blue::chassis::imu_sensor"
orientation {
  x: -2.3117893552150796e-08
  y: 2.9654079887270204e-06
  z: 0.0077879344431916247
  w: 0.99996967357431121
}
orientation_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}
angular_velocity {
  x: -2.643416560507971e-11
  y: 5.5508724142631941e-06
  z: 2.61864093623602e-13
}
angular_velocity_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}
linear_acceleration {
  x: -7.0848758728578524e-05
  y: -1.1039375494702606e-09
  z: 9.799992568914794
}
linear_acceleration_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}

header {
  stamp {
    sec: 3
  }
  data {
    key: "frame_id"
    value: "vehicle_blue::chassis::imu_sensor"
  }
  data {
    key: "seq"
    value: "3"
  }
}
entity_name: "vehicle_blue::chassis::imu_sensor"
orientation {
  x: -2.3722197175311586e-08
  y: 3.0429764124486936e-06
  z: 0.0077879344081707427
  w: 0.99996967357435085
}
orientation_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}
angular_velocity {
  x: 8.7327529922574356e-12
  y: -5.277354213423571e-06
  z: -2.817821160315417e-11
}
angular_velocity_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}
linear_acceleration {
  x: -5.4889792669568007e-05
  y: -8.05116325338902e-10
  z: 9.7999923870500609
}
linear_acceleration_covariance {
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
  data: 0
}

3.2 Contact 传感器

接触传感器，这个传感器会在接触到其他物体时发出指示。

我们将构建一个障碍物（墙）并在其上添加接触传感器。如果机器人碰到障碍物，它将停止，从而防止机器人损坏自身。

Step 1: 构建障碍物

添加一堵墙：

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

Step 2: 引入 contact 传感器

通过添加以下代码来定义接触传感器：

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

Step 3: 将传感器链接到墙模型

将接触传感器添加到墙模型的箱体链接中：

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

的定义很简单，我们只需定义传感器的名称和类型。
在 collision 中，我们定义箱体链接的碰撞名称为 collision。

Step 4: 添加 TouchPlugin事件

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

• ：将与我们的墙接触的对象，在我们的例子中是 vehicle_blue。
• ：设置我们话题的命名空间，因此当机器人碰到墙时，它会向 /wall/touched 话题发送一条消息。

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

Step 5: 简单测试

1. 在一个终端中运行世界并按下播放按钮：

$ gz sim sensor_tutorial.sdf

2. 在另一个终端中执行以下命令：

$ gz topic -e -t /wall/touched

3. 使用键盘箭头键将你的机器人向前驱动到墙壁，可以看到数据输出。

$ gz topic -e -t /wall/touched
data: true

Step 6: 添加 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>

当我们在 /wall/touched 话题上接收到数据 true 时，我们发布 linear: {x: 0.0}, angular: {z: 0.0}，使我们的机器人停止。

3.3 Lidar 传感器

我们不希望机器人碰到墙壁，因为这可能会造成一些损坏，所以可以使用激光雷达（Lidar）代替接触传感器。Lidar 是“光探测和测距”的缩写。这个传感器可以帮助我们检测机器人周围的障碍物。我们将使用它来测量机器人和墙壁之间的距离。

Step 1: 固定激光雷达

首先，让我们创建一个框架来固定我们的激光雷达。

这应该添加到 vehicle_blue 的 标签内，因为激光雷达框架是固定在机器人的底盘上的：

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

Step 2: 引入 Lidar 传感器

然后在 标签下添加这个插件，以便能够使用激光雷达传感器：

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

Step 3: 添加 Lidar 传感器

在底盘链接下，我们可以按如下方式添加激光雷达传感器：

• 首先，我们定义了传感器的名称和类型
• 然后，定义了它相对于 lidar_frame 的 。
• 在 中，我们定义了激光雷达数据将被发布的话题。
• <update_rate> 是激光雷达数据生成的频率，在我们的例子中是 10 Hz，相当于 0.1 秒。
• 在 和 标签下，我们定义了水平和垂直激光束的属性。
  • 是每个完整激光扫描周期生成的模拟激光雷达射线的数量。
  • ：这个数字乘以样本数来确定范围数据点的数量。
  • <min_angle> 和 <max_angle> 是生成的射线的角度范围。
• 在 下，我们定义了每个模拟射线的范围属性：
  • 和 定义了每个激光雷达射线的最小和最大距离。
  • 这里的 标签定义了每个激光雷达射线的线性分辨率。
• <always_on>：如果为 true，传感器将始终根据 <update_rate> 进行更新。
• ：如果为 true，传感器将在 GUI 中可视化。

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

Step 4: 简单测试

1. 在一个终端中运行世界并按下播放按钮：

$ gz sim sensor_tutorial.sdf

2. 在另一个终端中执行以下命令

$ gz topic -e -t /lidar

3. 可以看到数据输出。

header {
  stamp {
    sec: 1
    nsec: 900000000
  }
  data {
    key: "frame_id"
    value: "vehicle_blue::chassis::gpu_lidar"
  }
  data {
    key: "seq"
    value: "18"
  }
}
frame: "vehicle_blue::chassis::gpu_lidar"
world_pose {
  position {
    x: 0.8
    z: 0.5
  }
  orientation {
    w: 1
  }
}
angle_min: -1.396263
angle_max: 1.396263
angle_step: 0.0043701502347417839
range_min: 0.08
range_max: 10
count: 640
vertical_angle_step: nan
vertical_count: 1
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: 6.0434856414794922
ranges: 6.0434856414794922
ranges: 6.0434856414794922
ranges: 5.9542717933654785
ranges: 5.9542717933654785
ranges: 5.8695230484008789
ranges: 5.8695230484008789
ranges: 5.8695230484008789
ranges: 5.7889442443847656
ranges: 5.7889442443847656
ranges: 5.7122631072998047
ranges: 5.7122631072998047
ranges: 5.6392321586608887
ranges: 5.6392321586608887
ranges: 5.5696215629577637
ranges: 5.5696215629577637
ranges: 5.5696215629577637
ranges: 5.5032200813293457
ranges: 5.5032200813293457
ranges: 5.4398350715637207
ranges: 5.4398350715637207
ranges: 5.3792848587036133
ranges: 5.3792848587036133
ranges: 5.3214030265808105
ranges: 5.3214030265808105
ranges: 5.2660360336303711
ranges: 5.2660360336303711
ranges: 5.2130403518676758
ranges: 5.2130403518676758
ranges: 5.1622824668884277
ranges: 5.1622824668884277
ranges: 5.1136398315429688
ranges: 5.1136398315429688
ranges: 5.0669965744018555
ranges: 5.0669965744018555
ranges: 5.022244930267334
ranges: 5.022244930267334
ranges: 4.9792847633361816
ranges: 4.9792847633361816
ranges: 4.938023567199707
ranges: 4.8983726501464844
ranges: 4.8983726501464844
ranges: 4.8600902557373047
ranges: 4.8600902557373047
ranges: 4.8222751617431641
ranges: 4.8222751617431641
ranges: 4.7847685813903809
ranges: 4.7847685813903809
ranges: 4.74757719039917
ranges: 4.74757719039917
ranges: 4.7107095718383789
ranges: 4.7107095718383789
ranges: 4.6741724014282227
ranges: 4.6741724014282227
ranges: 4.637974739074707
ranges: 4.637974739074707
ranges: 4.6021232604980469
ranges: 4.6021232604980469
ranges: 4.56662654876709
ranges: 4.56662654876709
ranges: 4.5314931869506836
ranges: 4.5314931869506836
ranges: 4.4967317581176758
ranges: 4.4967317581176758
ranges: 4.4623508453369141
ranges: 4.4623508453369141
ranges: 4.4283585548400879
ranges: 4.4283585548400879
ranges: 4.3947649002075195
ranges: 4.3947649002075195
ranges: 4.3615789413452148
ranges: 4.3615789413452148
ranges: 4.32880973815918
ranges: 4.32880973815918
ranges: 4.32880973815918
ranges: 4.2964673042297363
ranges: 4.2964673042297363
ranges: 4.2645606994628906
ranges: 4.2645606994628906
ranges: 4.2330999374389648
ranges: 4.2330999374389648
ranges: 4.2330999374389648
ranges: 4.2020955085754395
ranges: 4.2020955085754395
ranges: 4.1715569496154785
ranges: 4.1715569496154785
ranges: 4.1715569496154785
ranges: 4.1414952278137207
ranges: 4.1414952278137207
ranges: 4.1119203567504883
ranges: 4.1119203567504883
ranges: 4.1119203567504883
ranges: 4.0828428268432617
ranges: 4.0828428268432617
ranges: 4.05427360534668
ranges: 4.05427360534668
ranges: 4.05427360534668
ranges: 4.0262236595153809
ranges: 4.0262236595153809
ranges: 3.9987032413482666
ranges: 3.9987032413482666
ranges: 3.9987032413482666
ranges: 3.971724271774292
ranges: 3.971724271774292
ranges: 3.971724271774292
ranges: 3.9452977180480957
ranges: 3.9452977180480957
ranges: 3.9194338321685791
ranges: 3.9194338321685791
ranges: 3.9194338321685791
ranges: 3.8941445350646973
ranges: 3.8941445350646973
ranges: 3.8941445350646973
ranges: 3.8694407939910889
ranges: 3.8694407939910889
ranges: 3.8694407939910889
ranges: 3.845334529876709
ranges: 3.845334529876709
ranges: 3.845334529876709
ranges: 3.8218362331390381
ranges: 3.8218362331390381
ranges: 3.8218362331390381
ranges: 3.7989575862884521
ranges: 3.7989575862884521
ranges: 3.7989575862884521
ranges: 3.7767097949981689
ranges: 3.7767097949981689
ranges: 3.7767097949981689
ranges: 3.7551043033599854
ranges: 3.7551043033599854
ranges: 3.7551043033599854
ranges: 3.73415207862854
ranges: 3.73415207862854
ranges: 3.73415207862854
ranges: 3.7138638496398926
ranges: 3.7138638496398926
ranges: 3.7138638496398926
ranges: 3.69425106048584
ranges: 3.69425106048584
ranges: 3.69425106048584
ranges: 3.6753242015838623
ranges: 3.6753242015838623
ranges: 3.6753242015838623
ranges: 3.6570942401885986
ranges: 3.6570942401885986
ranges: 3.6570942401885986
ranges: 3.6395711898803711
ranges: 3.6395711898803711
ranges: 3.6395711898803711
ranges: 3.6227657794952393
ranges: 3.6227657794952393
ranges: 3.6227657794952393
ranges: 3.6227657794952393
ranges: 3.6066875457763672
ranges: 3.6066875457763672
ranges: 3.6066875457763672
ranges: 3.5913469791412354
ranges: 3.5913469791412354
ranges: 3.5913469791412354
ranges: 3.5767526626586914
ranges: 3.5767526626586914
ranges: 3.5767526626586914
ranges: 3.5767526626586914
ranges: 3.5629146099090576
ranges: 3.5629146099090576
ranges: 3.5629146099090576
ranges: 3.5498411655426025
ranges: 3.5498411655426025
ranges: 3.5498411655426025
ranges: 3.5375409126281738
ranges: 3.5375409126281738
ranges: 3.5375409126281738
ranges: 3.5375409126281738
ranges: 3.5260219573974609
ranges: 3.5260219573974609
ranges: 3.5260219573974609
ranges: 3.5152919292449951
ranges: 3.5152919292449951
ranges: 3.5152919292449951
ranges: 3.5152919292449951
ranges: 3.5053582191467285
ranges: 3.5053582191467285
ranges: 3.5053582191467285
ranges: 3.496227502822876
ranges: 3.496227502822876
ranges: 3.496227502822876
ranges: 3.496227502822876
ranges: 3.4879059791564941
ranges: 3.4879059791564941
ranges: 3.4879059791564941
ranges: 3.4804000854492188
ranges: 3.4804000854492188
ranges: 3.4804000854492188
ranges: 3.4804000854492188
ranges: 3.4737141132354736
ranges: 3.4737141132354736
ranges: 3.4737141132354736
ranges: 3.4678535461425781
ranges: 3.4678535461425781
ranges: 3.4678535461425781
ranges: 3.4678535461425781
ranges: 3.4628221988677979
ranges: 3.4628221988677979
ranges: 3.4628221988677979
ranges: 3.4586238861083984
ranges: 3.4586238861083984
ranges: 3.4586238861083984
ranges: 3.4586238861083984
ranges: 3.4552614688873291
ranges: 3.4552614688873291
ranges: 3.4552614688873291
ranges: 3.45273756980896
ranges: 3.45273756980896
ranges: 3.45273756980896
ranges: 3.45273756980896
ranges: 3.4510538578033447
ranges: 3.4510538578033447
ranges: 3.4510538578033447
ranges: 3.450211763381958
ranges: 3.450211763381958
ranges: 3.450211763381958
ranges: 3.450211763381958
ranges: 3.450211763381958
ranges: 3.450211763381958
ranges: 3.450211763381958
ranges: 3.450211763381958
ranges: 3.4510538578033447
ranges: 3.4510538578033447
ranges: 3.4510538578033447
ranges: 3.45273756980896
ranges: 3.45273756980896
ranges: 3.45273756980896
ranges: 3.45273756980896
ranges: 3.4552614688873291
ranges: 3.4552614688873291
ranges: 3.4552614688873291
ranges: 3.4586238861083984
ranges: 3.4586238861083984
ranges: 3.4586238861083984
ranges: 3.4586238861083984
ranges: 3.4628221988677979
ranges: 3.4628221988677979
ranges: 3.4628221988677979
ranges: 3.4678535461425781
ranges: 3.4678535461425781
ranges: 3.4678535461425781
ranges: 3.4678535461425781
ranges: 3.4737143516540527
ranges: 3.4737143516540527
ranges: 3.4737143516540527
ranges: 3.4804000854492188
ranges: 3.4804000854492188
ranges: 3.4804000854492188
ranges: 3.4804000854492188
ranges: 3.4879062175750732
ranges: 3.4879062175750732
ranges: 3.4879062175750732
ranges: 3.4962277412414551
ranges: 3.4962277412414551
ranges: 3.4962277412414551
ranges: 3.4962277412414551
ranges: 3.5053584575653076
ranges: 3.5053584575653076
ranges: 3.5053584575653076
ranges: 3.5152924060821533
ranges: 3.5152924060821533
ranges: 3.5152924060821533
ranges: 3.5152924060821533
ranges: 3.52602219581604
ranges: 3.52602219581604
ranges: 3.52602219581604
ranges: 3.5375411510467529
ranges: 3.5375411510467529
ranges: 3.5375411510467529
ranges: 3.5375411510467529
ranges: 3.5498414039611816
ranges: 3.5498414039611816
ranges: 3.5498414039611816
ranges: 3.5629148483276367
ranges: 3.5629148483276367
ranges: 3.5629148483276367
ranges: 3.5767529010772705
ranges: 3.5767529010772705
ranges: 3.5767529010772705
ranges: 3.5767529010772705
ranges: 3.5913472175598145
ranges: 3.5913472175598145
ranges: 3.5913472175598145
ranges: 3.6066877841949463
ranges: 3.6066877841949463
ranges: 3.6066877841949463
ranges: 3.6227660179138184
ranges: 3.6227660179138184
ranges: 3.6227660179138184
ranges: 3.6227660179138184
ranges: 3.6395716667175293
ranges: 3.6395716667175293
ranges: 3.6395716667175293
ranges: 3.6570944786071777
ranges: 3.6570944786071777
ranges: 3.6570944786071777
ranges: 3.6753244400024414
ranges: 3.6753244400024414
ranges: 3.6753244400024414
ranges: 3.6942512989044189
ranges: 3.6942512989044189
ranges: 3.6942512989044189
ranges: 3.7138640880584717
ranges: 3.7138640880584717
ranges: 3.7138640880584717
ranges: 3.7341523170471191
ranges: 3.7341523170471191
ranges: 3.7341523170471191
ranges: 3.7551045417785645
ranges: 3.7551045417785645
ranges: 3.7551045417785645
ranges: 3.7767102718353271
ranges: 3.7767102718353271
ranges: 3.7767102718353271
ranges: 3.7989580631256104
ranges: 3.7989580631256104
ranges: 3.7989580631256104
ranges: 3.8218367099761963
ranges: 3.8218367099761963
ranges: 3.8218367099761963
ranges: 3.8453347682952881
ranges: 3.8453347682952881
ranges: 3.8453347682952881
ranges: 3.8694415092468262
ranges: 3.8694415092468262
ranges: 3.8694415092468262
ranges: 3.8941450119018555
ranges: 3.8941450119018555
ranges: 3.8941450119018555
ranges: 3.9194343090057373
ranges: 3.9194343090057373
ranges: 3.9194343090057373
ranges: 3.9452981948852539
ranges: 3.9452981948852539
ranges: 3.9717252254486084
ranges: 3.9717252254486084
ranges: 3.9717252254486084
ranges: 3.9987044334411621
ranges: 3.9987044334411621
ranges: 3.9987044334411621
ranges: 4.0262246131896973
ranges: 4.0262246131896973
ranges: 4.0542745590209961
ranges: 4.0542745590209961
ranges: 4.0542745590209961
ranges: 4.0828437805175781
ranges: 4.0828437805175781
ranges: 4.1119213104248047
ranges: 4.1119213104248047
ranges: 4.1119213104248047
ranges: 4.1414961814880371
ranges: 4.1414961814880371
ranges: 4.1715579032897949
ranges: 4.1715579032897949
ranges: 4.1715579032897949
ranges: 4.2020969390869141
ranges: 4.2020969390869141
ranges: 4.2331013679504395
ranges: 4.2331013679504395
ranges: 4.2331013679504395
ranges: 4.2645621299743652
ranges: 4.2645621299743652
ranges: 4.2964687347412109
ranges: 4.2964687347412109
ranges: 4.3288111686706543
ranges: 4.3288111686706543
ranges: 4.3288111686706543
ranges: 4.3615803718566895
ranges: 4.3615803718566895
ranges: 4.3947663307189941
ranges: 4.3947663307189941
ranges: 4.4283599853515625
ranges: 4.4283599853515625
ranges: 4.4623517990112305
ranges: 4.4623517990112305
ranges: 4.49673318862915
ranges: 4.49673318862915
ranges: 4.5314946174621582
ranges: 4.5314946174621582
ranges: 4.5666279792785645
ranges: 4.5666279792785645
ranges: 4.6021246910095215
ranges: 4.6021246910095215
ranges: 4.6379756927490234
ranges: 4.6379756927490234
ranges: 4.6741738319396973
ranges: 4.6741738319396973
ranges: 4.7107110023498535
ranges: 4.7107110023498535
ranges: 4.7475786209106445
ranges: 4.7475786209106445
ranges: 4.7847700119018555
ranges: 4.7847700119018555
ranges: 4.8222765922546387
ranges: 4.8222765922546387
ranges: 4.8600921630859375
ranges: 4.8600921630859375
ranges: 4.89837121963501
ranges: 4.89837121963501
ranges: 4.9380226135253906
ranges: 4.9792838096618652
ranges: 4.9792838096618652
ranges: 5.0222439765930176
ranges: 5.0222439765930176
ranges: 5.0669951438903809
ranges: 5.0669951438903809
ranges: 5.1136388778686523
ranges: 5.1136388778686523
ranges: 5.1622810363769531
ranges: 5.1622810363769531
ranges: 5.2130389213562012
ranges: 5.2130389213562012
ranges: 5.2660350799560547
ranges: 5.2660350799560547
ranges: 5.3214025497436523
ranges: 5.3214025497436523
ranges: 5.3792843818664551
ranges: 5.3792843818664551
ranges: 5.4398350715637207
ranges: 5.4398350715637207
ranges: 5.5032196044921875
ranges: 5.5032196044921875
ranges: 5.5696210861206055
ranges: 5.5696210861206055
ranges: 5.5696210861206055
ranges: 5.6392321586608887
ranges: 5.6392321586608887
ranges: 5.7122621536254883
ranges: 5.7122621536254883
ranges: 5.7889442443847656
ranges: 5.7889442443847656
ranges: 5.8695225715637207
ranges: 5.8695225715637207
ranges: 5.8695225715637207
ranges: 5.9542708396911621
ranges: 5.9542708396911621
ranges: 6.0434846878051758
ranges: 6.0434846878051758
ranges: 6.0434846878051758
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
ranges: inf
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0
intensities: 0

4. Lidar 避障

在机器人上安装了激光雷达（LIDAR），可以使用其测距数据让机器人避免撞到墙上。

为此，编写一个简短的 C++ 程序，该程序监听传感器数据并向机器人发送速度命令。这个程序被称为节点。

构建这个节点，订阅 /lidar 主题并读取其数据。

Step 1: 声明一个节点

该节点将发布到 cmd_vel 主题，并定义了消息类型 Twist。然后发布我们的节点。

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

Step 2: 回调检查

在回调函数中，检查所有光束的范围是否大于1.0。如果是这样，我们发布消息让汽车前进。否则，我们让机器人旋转。

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

Step 3: 订阅数据源

在主函数中，订阅了 lidar 主题，并等待节点关闭。

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

Step 4: 编译节点

$ mkdir build
$ cd build
$ cmake ..
$ make lidar_node

Step 5: 简单测试

$ ./build/lidar_node

$ gz sim sensor_tutorial.sdf

现在,可以看到机器人向前移动，当它接近墙壁时，它会开始向左转，直到前方没有障碍物后再次向前移动（确保按下左下角的播放按钮以启动机器人）。

5. Gazebo 启动

我们可以创建一个启动文件来同时运行 sensor_world 和 lidar_node，而不是从两个不同的终端运行两个不同的任务。

Step 1:

打开文本编辑器并添加以下代码。

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

Step 2:

启动文件是一个 XML 文件。我们只需在 <executable> 标签下定义将运行哪些命令。

• 第一个命令是 gz sim sensor_tutorial.sdf，它启动世界。
• 第二个命令是 ./build/lidar_node，它运行 lidar_node。

将文件保存为 sensor_launch.gzlaunch，然后使用以下命令运行它：

$ gz launch sensor_launch.gzlaunch

按下左下角的播放按钮以启动机器人，就会看到与章节4一样的效果。

6. 参考资料

【1】ArduPilot开源代码之ROS2Humble+CartographerSLAM+SITL+Gazebo
【2】ArduPilot飞控之Gazebo + SITL + MP的Jetson Orin环境搭建
【3】ArduPilot飞控之ubuntu22.04-Gazebo模拟
【4】PX4模块设计之七：Ubuntu 20.04搭建Gazebo模拟器