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 传感数据
- 在一个终端中运行世界并按下播放按钮:
$ gz sim sensor_tutorial.sdf
- 在另一个终端中执行以下命令:
$ gz topic -e -t /imu
- 使用键盘的向上键移动你的机器人前进,可以看到传感器值在变化。
$ 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: 简单测试
- 在一个终端中运行世界并按下播放按钮:
$ gz sim sensor_tutorial.sdf
- 在另一个终端中执行以下命令:
$ gz topic -e -t /wall/touched
- 使用键盘箭头键将你的机器人向前驱动到墙壁,可以看到数据输出。
$ 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: 简单测试
- 在一个终端中运行世界并按下播放按钮:
$ gz sim sensor_tutorial.sdf
- 在另一个终端中执行以下命令
$ gz topic -e -t /lidar
- 可以看到数据输出。
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模拟器