- 1. 基于PhysX的LiDAR传感器
- 2. 基于RTX的LiDAR传感器
- 3. LVI多传感器数据融合采集
- 4. Isaac Sim采集并输出双目-IMU-LiDAR数据完整实例展示
- 5. 参考资料
在之前的这篇笔记中，我们了解了通过手动构造Action Graph来保存相机影像的方法，在这篇笔记中，则进一步介绍了通过Stand Alone方式进行保存的方法。在这篇笔记中，类似的，利用Action Graph进行IMU数据的录制与保存。本篇笔记我们主要学习Isaac Sim中LiDAR传感器的使用，包括传感器的添加、基本属性设置、数据保存等内容。
总体而言，在Isaac Sim中，有两类LiDAR，一类是普通的Lidar类型的LiDAR，另一类是Camera类型的RTX LiDAR。根据官方文档描述，RTX LiDAR支持通过JSON配置文件从而支持固态或者旋转机械雷达。每一个RTX LiDAR都必须要固联到一个Viewport对象，才可以正常仿真。在下面我们会分别进行介绍。
注意，本篇笔记所有内容均基于Isaac Sim 2022.2.0版本，测试通过。由于Isaac Sim版本更新快，API变化较大，因此不同版本可能存在不兼容情况。
1. 基于PhysX的LiDAR传感器
1.1 传感器添加
在Isaac Sim中使用普通的LiDAR传感器其实非常简单。打开Isaac Sim以后，依次点击Create > Isaac > Sensors > Lidar > Rotating即可创建一个LiDAR对象，如下所示。
这样我们便添加好了一个LiDAR传感器，为了能够比较好可视化，可以在Stage标签栏中选择刚刚新建的Lidar对象，然后找到下面Property面板中最下面的“Raw USD Properties”，可以看到“drawLines”选项，如下所示。
勾选以后，仿真运行的时候就可以看到激光线了，如下所示。
这样其实LiDAR传感器就添加好了，十分简单。
1.2 更好的激光可视化
为了更好的可视化，我们可以在场景里添加一些东西，比如一个Cube。为了能够让激光线和Cube相遇时更加突出，我们可以给这个Cube添加Collider碰撞属性。方法是选中新添加的“Cube”，在Property中选择Add > Physics > Collider，如下图所示。
添加完成以后，再次运行仿真环境，就可以看到和Cube相交的激光线变成红色了，如下所示。
1.3 将LiDAR传感器固联到实体上
上面我们添加了LiDAR传感器，但它是一个看不见摸不着的东西，为了使用方便，我们可以将其固联到某个Prim上。这样，当我们移动某个实体的时候，LiDAR也就会跟着一起动了。可以有两种方案。
第一种是我们向场景中添加任意一个实体，比如一个Cylinder，然后直接把LiDAR传感器拖拽到Cylinder上面即可，如下所示。
运行仿真环境，可以看到，LiDAR点云是可以360度正常运转的。但这里有个潜在风险需要注意一下。根据我们之前的习惯，对于一个实体对它赋予物理属性都喜欢用“Rigid Body with Colliders Preset”。但如果你这样做了，而LiDAR传感器又恰巧被你放在了这个实体的内部，激光线就会被这个实体遮住。本来好好的水平360度的激光，只变成了很小的角度，如下所示。
这个问题的核心就在于“Colliders”这个物理属性，所以，我们把这个物理属性删掉即可。原因也非常简单，如果某个实体一旦有了这个碰撞属性，激光就无法穿透它了。这个时候，你又把光源放到它的内部，自然就没有观测了。解决的办法也很简单，要么你把这个光源从实体内部拿出来，要么就把实体的这个物理属性删掉即可。
另一个办法那就是通过一个Xform来固联它们，如下图所示。新建一个Xform，然后把它们两个都放进去。这样当我们移动Xform的时候，Cylinder和LiDAR就会同时移动了。
1.4 LiDAR传感器的设置
其实，Isaac Sim中所有LiDAR传感器的属性都在上面提到的“Raw USD Properties”中。我们可以通过修改相应参数实现想要的功能，如下。
大部分参数一看就懂。比较重要的有：
- horizontalFov: 水平视角，默认是360度，也就是一周
- horizontalResolution: 水平分辨率，度为单位。比如0.4表示水平每0.4度有一个观测线
- maxRange: LiDAR传感器最远量程，单位是米
- mixRange: LiDAR传感器最近量程，单位是米
- rotationRate: 激光线的旋转速率，每秒多少圈。如果为0则表示一次性获得所有激光线
- verticalFov: 垂直视角，LiDAR传感器在竖直方向上的视角范围，单位为度
- verticalResolution: 竖直分辨率。两条激光线之间的竖直角度，单位为度
比如，下图展示了旋转速率为1时候的激光雷达效果。
1.5 LiDAR传感器数据输出
LiDAR传感器的数据输出比较好的方式就是利用Isaac Sim的ROS Bridge，通过构建Action Graph进行LiDAR点云发布。原理其实和前面发布IMU数据是类似的，这里直接给出构造好的Action Graph，如下图所示。
具体而言，我们利用Isaac Read Lidar Point Cloud Node进行点云数据读取(指定它的Lidar Prim属性为我们刚刚添加的Lidar对象)，然后利用ROS1 Publish Point Cloud进行点云数据发布。同时，为了能够包含时间戳，利用Isaac Read Simulation Time来获取时间戳。最后，利用On Playback Tick来触发整个Action Graph。
为了更好辨别发布的数据，我们随便向场景中添加一些物体，然后运行仿真环境，点云数据就被发布了。我们可以利用RViz实时可视化，或者利用rosbag工具进行录制，如下。
我们也可以使用rqt_bag工具查看录制的bag包，如下。
可以看到，20.7秒录制了415个messge，平均帧率是20.048。通过分布图也可以看出，总体还是比较均匀稳定的。至此，便简单实现了利用Isaac Sim采集LiDAR数据的需求。但是有以下几点需要注意：
- 目前，在Isaac Sim中，并不直接支持对于LiDAR帧率的设置，这点和相机类似。输出的帧率基本等于渲染帧率，因此如果对帧率有特殊需求，可能需要后期进行抽帧等操作。
- 通过上面介绍的这种方式获得的LiDAR数据其实是单线的。换句话说在竖直方向上只有一根线。尽管前面提到可以设置竖直FOV，但经过实测无效。
以上两点如果有解决办法欢迎在评论区留言。
2. 基于RTX的LiDAR传感器
根据官方文档描述，它是基于RTX硬件而开发的LiDAR传感器。它的使用必须要和Camera一起，或者换句话说，它本身就是一种Camera类型的传感器，它的结果是渲染出来的。相比于上面提到的传感器，它的最大优点就是，真正支持多线激光，而不只是单线。下面进行介绍。
2.1 RTX LiDAR添加
添加方式其实和上面的LiDAR传感器差不多，依次点击Create > Isaac > Sensors > RTX Lidar > Rotating即可创建，如下。
可以看到，尽管是LiDAR，但类型却是Camera。
然后你也可以按照之前的方法将其绑定到一些实体上，这里就暂时不绑定了。
2.2 RTX LiDAR数据输出
和普通LiDAR类似的，也是通过构建Action Graph进行录制。构造好的图如下所示。
在这个图中，核心就是Isaac Create Render Product和ROS1 RTX Lidar Helper这两个节点。前面说了，RTX LiDAR的结果是渲染出来的，所以这里我们通过Isaac Create Render Product来获得渲染之后的产品。具体而言，我们在Isaac Create Render Product节点的inputs:cameraPrim属性指定我们添加的RTX LiDAR即可。它会将结果通过outputs:renderProductPath输出。它的输出主要有两个，一个是Laser Scan类型的数据，另一个是Point Cloud类型的数据。如果需要，你可以像官方文档中一样两个都接收。或者像我们这里一样，只接收点云数据。然后，在ROS1 RTX Lidar Helper的inputs:type属性中，选择类型为point_cloud。对应修改一下Topic的名称。这样，输出的就是点云了(默认的是laser_scan)。运行以后，打开RViz，我们就可以看到消息了，如下所示。
如果我们用rosbag进行记录，就可以保存数据了。这样，其实我们就完成了利用RTX LiDAR进行数据采集的任务了。而且通过上面的可视化可以看到，点云是多线的。还记得前面说的，RTX LiDAR本质上是Camera类型，而且Action Graph里又用到了Helper节点，所以你应该会感觉和相机的ROS Topic发布是类似的。当我们运行过一次Action Graph以后，系统就自动会生成一个SDGpipeline，我们可以打开看看，如下图所示。如果对SDGpipeline不了解，可以参考之前的这篇笔记，详细介绍了相关过程。
2.3 LiDAR帧率设置
在上面我们提到普通LiDAR传感器是没有办法显式设置帧率的。那么RTX LiDAR可不可以呢？看了上面的介绍其实你心里应该已经有答案了。是可以的。因为RTX LiDAR本质上是Camera类型，而Camera类型渲染结果都是走SDGPipeline的。而正如同在之前博客里说的，我们可以通过控制SDGPipeline的Isaac Simulation Gate节点的Step属性来间接控制输出帧率。而正如上面的SDGPipeline所示，它这里又恰好有这个节点，所以就可以像控制相机帧率一样人工干预了。
首先，直接录制输出数据，计算帧率，在我机器上大约是110帧左右。假如我们的目标帧率是10，那么就差不多是每10帧输出一次。打开SDGPipeline，可以看到有两个Isaac Simulation Gate。
经过实际测试，这两个地方都要修改Step属性才会有效果，不然尽管帧率会下降，但并不是预期的帧率。修改Step属性完成以后，再次运行、录制数据，查看帧率，如下。
平均帧率差不多到达了预期目标10帧左右，算是完成了任务。
2.4 LiDAR参数设置
虽然上面利用RTX LiDAR我们完成了上面普通LiDAR难以做到的两点：第一是获得了多线激光雷达的数据，而不再是单线，第二是可以手动调整数据的输出功率而不是无法改变，但是我们还是有点在意一些事情。一个很关键的点就是，尽管我们用RTX LiDAR获得的是多线数据，但到底是多少线？最远量程是多少？我们都不知道。而且，在某些场景下，我们也许需要进行定制化修改。要找到这些参数其实也不难。还记得前面我们说的，LiDAR的相关属性都可以在Raw USD Properties里看到。我们打开这个面板，如下。
可以看到cameraSensorType属性为lidar(camera/radar/lidar三选一)，sensorModelConfig属性为Example_Rotary，sensorModelPluginName为omni.drivesim.sensors.nv.lidar.lidar_core.plugin。不要小看了sensorModelConfig和sensorModelPluginName属性。这两个属性决定了LiDAR的相关参数。根据官方文档描述，对于默认新建的RTX LiDAR传感器，其提供两种sensorModelConfig属性，分别是Example_Rotary和Example_Solid_State。字面意思就可以理解，一个是机械旋转式LiDAR，一个是固态LiDAR。这两个雷达的配置文件位于Isaac Sim的根目录下的exts/omni.drivesim.sensors.nv.lidar/data文件夹中。比如我电脑上它们的绝对路径就是/home/xuhui/Softwares/Omniverse/Libs/isaac_sim-2022.2.0/exts/omni.drivesim.sensors.nv.lidar/data/Example_Rotary.json。
打开它如下。
可以看到，在这里定义了LiDAR所有参数。而我们好奇的最大量程、多线的个数等在这里都能找到答案。默认的LiDAR是128线，最大量程是200m。当然，你可以根据你自己的需要修改这个文件中的对应部分，从而满足你的需求。
当然，如果这两种默认的配置还是满足不了你的需求。Isaac Sim还提供了多种预置的方案。这里我直接从这个官方文档截图，如下。
根据这个文档描述，Isaac Sim提供的LiDAR配置文件在/exts/omni.isaac.sensor/data/lidar_configs中。也如上图所示，有多种现实LiDAR传感器可以使用。比如说，我们想使用禾赛的激光雷达，那么我们就修改插入的RTX LiDAR传感器的sensorModelConfig属性为Hesai_XT32_SD10，运行效果如下。
最后，在这个官方文档里，详细解释了所有参数的意义，需要的话可以参考。
2.5 Stand Alone模式
除了像上面描述的通过软件界面的方式插入LiDAR传感器并发布数据，和之前相机和IMU类似，也可以通过Stand Alone脚本的方式启动。而关于Stand Alone的介绍见这篇笔记。
官方也给出了LiDAR相关的Stand Alone脚本，位于Isaac安装根目录/standalone_examples/api/omni.isaac.ros_bridge/rtx_lidar.py。把代码贴在下面：
# Copyright (c) 2020-2022, NVIDIA CORPORATION. All rights reserved.
#
# NVIDIA CORPORATION and its licensors retain all intellectual property
# and proprietary rights in and to this software, related documentation
# and any modifications thereto. Any use, reproduction, disclosure or
# distribution of this software and related documentation without an express
# license agreement from NVIDIA CORPORATION is strictly prohibited.
import carb
from omni.isaac.kit import SimulationApp
import sys
# Example for creating a RTX lidar sensor and publishing PCL data
simulation_app = SimulationApp({"headless": False})
import omni
from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.core import SimulationContext
from omni.isaac.core.utils import stage, nucleus
from omni.isaac.core.utils.render_product import create_hydra_texture
import omni.kit.viewport.utility
from pxr import Gf
import omni.replicator.core as rep
# enable ROS bridge extension
enable_extension("omni.isaac.ros_bridge")
simulation_app.update()
# check if rosmaster node is running
# this is to prevent this sample from waiting indefinetly if roscore is not running
# can be removed in regular usage
import rosgraph
if not rosgraph.is_master_online():
carb.log_error("Please run roscore before executing this script")
simulation_app.close()
exit()
# Locate Isaac Sim assets folder to load environment and robot stages
assets_root_path = nucleus.get_assets_root_path()
if assets_root_path is None:
carb.log_error("Could not find Isaac Sim assets folder")
simulation_app.close()
sys.exit()
simulation_app.update()
# Loading the simple_room environment
stage.add_reference_to_stage(
assets_root_path + "/Isaac/Environments/Simple_Warehouse/full_warehouse.usd", "/background"
)
simulation_app.update()
# Create the lidar sensor that generates data into "RtxSensorCpu"
# Sensor needs to be rotated 90 degrees about X so that its Z up
# Possible options are Example_Rotary and Example_Solid_State
# drive sim applies 0.5,-0.5,-0.5,w(-0.5), we have to apply the reverse
_, sensor = omni.kit.commands.execute(
"IsaacSensorCreateRtxLidar",
path="/sensor",
parent=None,
config="Example_Rotary",
translation=(0, 0, 1.0),
orientation=Gf.Quatd(0.5, 0.5, -0.5, -0.5), # Gf.Quatd is w,i,j,k
)
_, render_product_path = create_hydra_texture([1, 1], sensor.GetPath().pathString)
# Create Point cloud publisher pipeline in the post process graph
writer = rep.writers.get("RtxLidar" + "ROS1PublishPointCloud")
writer.initialize(topicName="point_cloud", frameId="sim_lidar")
writer.attach([render_product_path])
# Create the debug draw pipeline in the post process graph
writer = rep.writers.get("RtxLidar" + "DebugDrawPointCloud")
writer.attach([render_product_path])
# Create LaserScan publisher pipeline in the post process graph
writer = rep.writers.get("RtxLidar" + "ROS1PublishLaserScan")
writer.initialize(topicName="laser_scan", frameId="sim_lidar")
writer.attach([render_product_path])
simulation_app.update()
simulation_app.update()
simulation_context = SimulationContext(physics_dt=1.0 / 60.0, rendering_dt=1.0 / 60.0, stage_units_in_meters=1.0)
simulation_context.play()
while simulation_app.is_running():
simulation_app.update()
# cleanup and shutdown
simulation_context.stop()
simulation_app.close()
我们在Isaac Sim安装根目录下找到python.sh，然后运行这个脚本即可。如果运行这个脚本的话，就会打开一个世界，LiDAR已经插入，仿真环境已经运行，点云可视化也已经做好，相关ROS的Topic已经发布，如下图所示。
当然了，看起来这个LiDAR转的好像有点慢，那就修改对应参数文件中的参数即可。
当然可能有个潜在问题，就是如果你的网络不太好或者硬件性能差一些，那么可能会卡在加载世界环境的这个步骤(因为要从Amazon云上在线下载资源)。因为示例代码里加载的是整个仓库的场景比较大。如果等了很久都没有进展，有两种方案：一种是手动修改场景，换一个小一点的场景；第二种是代码中不加载场景，等启动以后再手动选择一些场景都可以。
这个脚本总体上是遵循Stand Alone流程的一贯做法的。下面简单一步步解释一下。首先，因为需要发布ROS消息，所以脚本首先开启了ROS Extention。然后，检查当前状态roscore是否在运行，如果没有就退出。第三步，还是异常检查，检查Isaac相关Asset是否就位。第四步，尝试加载一个环境，这里加载的是一个仓库的环境。第五步，核心步骤之一，创建RTX LiDAR传感器。可以看到，这里创建的是Example_Rotary类型的传感器。所以如果需要，可以修改对应的配置文件，可以改变相关参数。然后第六步，利用replicator创建了点云的发布节点，并和相关数据进行了绑定。第七步，同样是利用replicator创建了点云渲染节点。第八步，利用replicator创建了Laser Scan的发布节点。最后，通过一个while循环，不停更新渲染，直到关闭仿真环境。
以上便是一个通过Stand Alone方式进行LiDAR数据采集的流程。
2.6 激光线可视化设置
为了更好的采集数据，我们自然是希望可以实时看到激光线的。对于普通的LiDAR传感器来说，可以通过设置drawLines和drawPoints属性完成。而对于RTX LiDAR传感器，可以通过在Stand Alone脚本中指定debug pipeline实现。而对于RTX LiDAR如何直接在Isaac Sim界面中设置激光线暂时没有找到方法，如果你知道欢迎分享。
3. LVI多传感器数据融合采集
如同之前笔记介绍的，Isaac Sim中可以有两种方式发布数据，一种就是基于GUI手动编写Action Graph，另一种是基于Stand Alone模式写代码进行数据发布。但个人建议是，如果能用GUI解决的就尽量不用代码解决了，除非你特别喜欢挑战自己，喜欢写代码。Stand Alone模式只是在不得不用地时候使用一下。以下代码和文件均在Isaac Sim 2022.2.0版本测试通过。
3.1 无键盘控制的双目-IMU-单线传感器配置与数据发布
在前面的笔记中，我们介绍了使用GUI构造Action Graph发布数据的方法。这里，类似的，我们也首先使用这种方式进行数据发布。为了能够准确和简单演示，我们首先搭建一个非常抽象的平台，在它上面分别放两个相机(模拟双目)、IMU以及普通的LiDAR传感器(不是RTX LiDAR)。一个搭建好的效果如下所示。
可以看到，整个传感器附着于LVI_Sensor这个实体，里面包含了两个相机、一个IMU以及一个普通的单线LiDAR。由于这些传感器都在LVI_Sensor里面，所以，当选中并移动LVI_Sensor的时候，所有传感器都会跟着同时运动。添加好传感器以后，下面就是构造Action Graph分别发布左右相机数据、IMU数据、LiDAR数据，以及最后的传感器的位姿真值数据，Graph如下。
其实这个图看起来比较复杂，但其实都是由之前介绍过的各个部分组合而成的。在构造图的时候，记得一些属性别忘记修改，不然可能不会有想要的结果。
然后，我们启动这个运行环境，他就会不停发布ROS消息了。我们也可以利用RViz对相关数据进行可视化，如下。
可以看到，RViz可以正常接收数据。不过需要注意的是，通过RViz可以看到，输出的相机影像里也有激光线，这是因为我们在Isaac Sim中设置了让他显示。所以如果要输出数据里没有，记得把LiDAR的drawLines属性改一下。总体而言，我们可以说，基于上述配置，我们便可以完成LVI数据的初步采集了。当然了，这个搭建的传感器的外参是很好获得的，直接从文件中就可以算出来，而各传感器内参则通过传感器对象的属性也可以读取出来。最后，我们把这个文件上传到了Github，名字叫做sensor-lvi1.0.usd，点击查看。
3.2 无键盘控制的双目-IMU-多线传感器配置与数据发布
在3.1.1部分，我们使用的LiDAR是普通LiDAR传感器。前面我们也说了，它只是单线的，没有特别大实际价值。所以更进一步，我们用RTX LiDAR传感器替代LiDAR传感器。在3.1.1的基础上，其实我们只需要把现有LiDAR用RTX LiDAR替换，再简单按照上面介绍的，修改成RTX LiDAR数据的发布流程就可以了。首先，我们修改完以后传感器如下。
然后，我们修改完的Action Graph如下。
总体而言没什么特别好说的，记得修改一些对应属性即可。运行以后的效果如下所示。
当然了，这里有个小坑。如果你运行之后发现LiDAR Topic一切正常，但是在RViz中就是没有数据，那么请尝试挪动一下LiDAR的位置。因为有可能是传感器实体把LiDAR激光挡住了。多试一试就能找到一个比较好的位置了。我们把这个版本的文件上传到了Github，名字叫做sensor-lvi2.0.usd，点击查看。
3.3 带键盘控制的双目-IMU-多线传感器配置与数据发布
在3.1.2部分，尽管我们比较好地完成了LVI数据的采集任务。但是还有个小问题，就是这个传感器我们都是靠手拖动才运动的。我们希望能够像玩游戏一样通过键盘控制。要做到这个也是很简单的，在之前的笔记中也多次提到。不论是自己搭还是用别人的模型，本质上都是找个可以运动的小车，然后将配置好的传感器绑定到它上面，然后在Action Graph中添加节点来控制小车的运动。这里为了方便，我们就直接将上面搭建好的这个长条形的LVI传感器绑定到Isaac Sim自带的两轮小车JetBot上面。做法也很简单，找到JetBot拖拽到场景里，然后将整个LVI_Sensor这个实体和它绑定，这样就可以实现同步运动了，修改完成之后如下。
当然了，你可以根据需要对JetBot进行各种魔改，比如原版它有两个天线，对于我来说很多余，而且会阻挡LiDAR激光，所以我就把它们高度降低了。这样修改完成以后，LVI传感器就是可以和小车一起运动的。最后一步，就是参考这篇笔记中的方法，通过Action Graph添加手动控制，即可以实现操控，构造的图如下。
运行仿真环境，再通过键盘就可以实现控制，效果如下。
相比于3.1.2部分的效果，我们可以通过键盘实现对小车的自由控制，进一步达到了我们的目标。我们把这个版本的文件上传到了Github，名字叫做sensor-lvi3.0.usd，点击查看。
3.4 可修改渲染数据帧率且带键盘控制的双目-IMU-多线传感器配置与数据发布
3.1.3的结果没有问题吗？答案是不一定，看你怎么看。这里面存在的一个老生常谈的问题就是不同传感器的帧率问题。我们运行上面搭建好的仿真环境，然后进行ROS Bag录制，我们查看数据信息如下。
我们录制了58.7秒的数据，然后双目影像共有901帧，帧率约为15.3。同理可以算出IMU和轨迹真值基本都是15帧左右，点云大约是30帧每秒。所以存在这么两个问题：一是如何修改某个传感器的帧率，二是不同传感器如何设置不同帧率。关于这两个问题在这篇笔记中专门进行了讨论。简单来说就是，对于走渲染通道获得的数据，比如双目相机、LiDAR，在运行一次以后会生成SDGPipeline，我们修改SDGPipeline Graph中的Isaac Simulation Gate的Step属性就可以间接修改帧率。对于某个传感器，设置不同Step属性可以改变它的帧率；对于多个传感器，有多个对应的Step属性，分别设置就可以实现不同传感器的不同分辨率。这个并不陌生，因为前面已经说过了。比如，我们这里LiDAR帧率是30帧，太高了，我们只需要10帧就差不多，这样Step属性设为3，就可以显著降低帧率。这里，比如我们还是运行sensor-lvi3.0.usd，但是修改SDGPipeline，完成以后再次录制数据，并查看ROS Bag信息，如下。
可以看到，这里，我们将LiDAR帧率Step属性设为3、双目相机Step属性设为2，对应的输出频率也降低了(左边是降低后的，右边是Step全为1的)。这里还是需要例行提示的是SDGPipeline不是永久的，而是只针对当前打开的仿真环境有效，当关闭再打开环境，就是新的Pipeline了，需要重新设置。
不过这个方式有个小小的问题就是，这样修改帧率以后，输出的数据可能会出现数据帧率不稳定的情况(数据分布不均匀)，所以如果在你电脑上发现这个情况很严重，那么可以跳过这一步，不修改Step属性正常采集。采集完成以后再手动抽帧、改变帧率。
另外，还是需要说明的是，对于非渲染的数据，比如IMU或者位姿真值等，这种方式就无能为力了。因为这些传感器或者数据走的不是渲染通道，比如IMU的观测是通过CPU解算出来的，所以对它就无效。如何修改IMU的帧率，是进一步要讨论的问题。
3.5 可修改IMU传感器帧率且带键盘控制的双目-IMU-多线传感器配置与数据发布
在3.1.4部分基础上，如何修改/提高IMU帧率是一个关键问题(现在这个帧率几乎没法实际应用)。事实上，这个问题我们在之前的这篇笔记中进行了详细分析。简单概括就是，使用GUI构造Action Graph的方式已经“走到了尽头”，得要使用Stand Alone的方式才可以修改IMU数据的帧率。并且根据之前的实际测试，即使修改了IMU的帧率，如果是直接以ROS Topic形式发布，帧率还是没有改变。所以，遵循之前的解决方案，通过代码手动获取IMU数据，然后保存成文本文件，后期再转化一下。首先，因为我们现在不再需要通过Action Graph发布IMU数据，所以把相关节点删掉，如下。
我们把删掉IMU数据发布节点的文件叫做sensor-lvi4.0.usd，文件上传到了Github，点击查看。
这样，当我们再运行以后，就只会发布两个影像数据、一个位姿真值数据、一个点云数据。
而IMU数据则通过下面的Python代码进行读取和保存。
import time
# 运行相关参数
imu_outpath = "/home/xuhui/imu_data.txt" # IMU数据输出文件路径
imu_frequency = 500 # IMU的观测频率
env_path = "/home/xuhui/Omniverse-Projects/sensor-lvi4.0.usd" # 需要加载的场景和小车的usd文件
imu_prim_name = "/World/LVI_Sensor/Imu_Sensor" # 场景文件中IMU传感器的Prim路径
# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})
# 启用ros_bridge扩展
from omni.isaac.core.utils import extensions
extensions.enable_extension("omni.isaac.ros_bridge")
# 加载已有场景
from omni.isaac.core.utils.stage import open_stage
open_stage(usd_path=env_path)
# 添加世界
from omni.isaac.core import World
world = World()
# 根据参数设置IMU频率
from omni.isaac.core.utils.prims import get_prim_at_path
prim_imu = get_prim_at_path(imu_prim_name)
prim_imu.GetAttribute("sensorPeriod").Set(1/imu_frequency)
# 根据官方文档建议，添加完物体之后，最好重置刷新一下世界
world.reset()
# 构造接口，获取IMU数据
from omni.isaac.sensor import _sensor
_imu_sensor_interface = _sensor.acquire_imu_sensor_interface()
# 新建文件用于输出
fout = open(imu_outpath, "w")
flag = True
print("IMU Recoding in progress ...")
# 开启渲染
while simulation_app.is_running():
world.step(render=True)
if flag:
start_time = time.time()
flag = False
imu_reading = _imu_sensor_interface.get_sensor_readings(imu_prim_name)
# 解析IMU传感器数据
if(len(imu_reading) != 0):
for i in range(len(imu_reading)):
tmp_imu_reading = imu_reading[i]
data_timestamp = tmp_imu_reading[0]
data_acc_x = tmp_imu_reading[1]
data_acc_y = tmp_imu_reading[2]
data_acc_z = tmp_imu_reading[3]
data_ang_x = tmp_imu_reading[4]
data_ang_y = tmp_imu_reading[5]
data_ang_z = tmp_imu_reading[6]
data_ori_x = tmp_imu_reading[7][0]
data_ori_y = tmp_imu_reading[7][1]
data_ori_z = tmp_imu_reading[7][2]
data_ori_w = tmp_imu_reading[7][3]
fout.write(str(data_timestamp) + " " + str(data_acc_x) + " " + str(data_acc_y) + " " + str(data_acc_z) + " " + str(data_ang_x) + " " + str(data_ang_y) + " " + str(data_ang_z) + " " + str(data_ori_x) + " " + str(data_ori_y) + " " + str(data_ori_z) + " " + str(data_ori_w) + "\n")
# cleanup and shutdown
fout.write("start time:"+str(start_time))
fout.close()
simulation_app.close()
执行代码以后，我们就可以在指定的路径下找到输出的IMU数据文件，打开如下。
这里数据的格式是时间戳、线加速度x、线加速度y、线加速度z、角加速度x、角加速度y、角加速度z、四元数x、四元数y、四元数z、四元数w。这里的时间戳是相对于开始时间的时间戳，而开始时间存放于最后一行。所以也可以很方便地恢复各个观测地绝对时间戳。可以看到，观测间隔为0.002，对应500Hz。所以我们可以说成功修改了IMU帧率，并输出了观测数据。上面这个代码文件也上传到到了Github，叫做lvi-recording-v1.py，点击查看。
3.6 可修改所有传感器帧率且带键盘控制的双目-IMU-多线传感器配置与数据发布
在前面我们提到，我们主要通过GUI方式修改SDGPipeline属性来完成双目相机、LiDAR点云这种基于渲染数据帧率的修改。同时，我们利用Stand Alone方式修改了IMU属性并进行数据存储。但在实际中，我们自然是希望可以同时修改相机等数据帧率，同时又可以修改IMU帧率并发布。而要实现这一目标，正如同之前这篇笔记介绍的，就得以Stand Alone的方式启动Isaac Sim。一个最简单和方便的做法是，利用Stand Alone脚本启动Isaac Sim，然后手动修改SDGPipeline中的Step属性调整帧率(这个过程不需要停止仿真运行，修改实时生效)。然后利用ROS Bag工具录制双目相机、LiDAR、位姿真值，而IMU则是在启动环境以后，就由脚本在一直记录。然后键盘控制小车运动采集数据。完成以后，关闭Isaac Sim软件，停止ROS Bag录制即可。采用这样一套流程，其实和3.1.5部分用到的文件和代码是一模一样的(sensor-lvi4.0.usd和lvi-recording-v1.py)，只是多了一步启动以后手动修改SDGPipeline参数。当然，如果你愿意挑战自己，不嫌麻烦，你也可以完全把所有Action Graph都删掉，用代码全部重新实现，功能上也不会有差别，只是从偷懒的角度来说没必要这么做。
4. Isaac Sim采集并输出双目-IMU-LiDAR数据完整实例展示
上面我们介绍了那么多，这里再非常简洁地对完整步骤做个总结，方便查阅。我们用到的文件是sensor-lvi4.0.usd和lvi-recording-v1.py，简介如下。
sensor-lvi4.0.usd：包含了我们自己搭建的LVI传感器(可以根据需求和各类运动平台绑定)、需要采集的环境(可以根据需要替换成其它场景)、Action Graph(负责双目相机、LiDAR、位姿真值这四个ROS Topic的发布、小车运动键盘控制)。lvi-recording-v1.py：负责以Stand Alone模式启动Isaac Sim，并且以指定地频率记录IMU观测。
4.1 所需文件下载与参数确认
下载上面这两个文件，然后打开lvi-recording-v1.py，根据自己的情况修改相关参数。如果对于小车的运动控制、ROS Topic发布、场景等参数需要修改，则打开sensor-lvi4.0.usd，否则默认就好。lvi-recording-v1.py中包含的参数如下所示。
4.2 启动Isaac Sim
找到Isaac Sim本地安装根目录下的python.sh，比如我的路径是/home/xuhui/Softwares/Omniverse/Libs/isaac_sim-2022.2.0/python.sh。在该根目录下输入./python.sh再加要执行的脚本文件，比如我电脑上是./python.sh /home/xuhui/blog/isaac-sim-lidar/lvi-recording-v1.py，回车即可启动仿真，如下。
然后根据自己的习惯调整一下窗口布局，并且点击菜单栏Window > Visual Scripting > Action Graph，打开Action Graph界面，如下。
4.3 根据预期帧率预估消息发布间隔
打开一个新的系统终端，输入rqt，在新打开的界面中依次选择Plugins > Topics > Topic Monitor，打开工具，如下。
然后依次选择Topic前面的方框，查看当前实时帧率，根据当前帧率和对该Topic地预期帧率计算应该每多少帧发布一次。比如选择/point_cloud，可以看到当前帧率大约为60，我们预期是10，所以差不多每6帧发布一次。
经过实测，在我电脑上影像大约是30FPS，点云是60FPS，我预期影像15FPS，点云10FPS，所以间隔分别是2和6。
4.4 手动修改SDGPipeline的Step属性
在Action Graph面板中点击“Edit Action Graph”，选择SDGPipeline并打开。依次找到双目相机、LiDAR对应的Isaac Simulation Gate节点，并根据上一步算出的间隔修改Step属性。比如这里以影像为例，修改了Step属性为2。
4.5 利用ROS工具录制Bag包
打开一个新的系统终端，输入rosbag record以及你要录制的Topic名称。比如这里要录制所有发布的Topic，就是rosbag record /cam_left /cam_right /point_cloud /odom，然后回车即可开始录制，如下。
然后你就可以切换到Isaac Sim软件，利用W、S、A、D键控制小车移动了。当差不多以后，在终端中按Ctrl+C停止录制。然后关闭Isaac Sim软件，结束整个过程。最后，你会在指定目录下看到路只好的bag包和刚刚保存的IMU数据，如下。
至此，录制过程全部结束，剩下的就是后处理了。
4.6 数据后处理
观测数据的后处理核心就是把这个IMU数据文件与之前的Bag包整合到一起。可以参考之前的这篇笔记，已经实现了IMU和双目、位姿真值的合并。这里就是又多了一个LiDAR Topic，本质上是一样的。使用编写好的joinBag.py脚本即可。脚本也上传到了Github，点击查看。这里也展示一下。
# coding=utf-8
from ctypes import c_void_p
from re import I
import rosbag
import subprocess, yaml
import sys
import os
import numpy as np
import cv2
from sensor_msgs.msg import Image, Imu
from cv_bridge import CvBridge
import rospy
from geometry_msgs.msg import Vector3
from nav_msgs.msg import Odometry
import subprocess, yaml
def loadTopicImgs(bag_path, topic_name):
imgs = []
timestamps = []
counter = 0
with rosbag.Bag(bag_path, 'r') as bag:
for topic, msg, t in bag.read_messages():
if topic == topic_name:
try:
cur_time = msg.header.stamp.to_sec()
print(cur_time)
timestamps.append(msg.header.stamp)
imgs.append(msg)
counter += 1
print(topic_name,counter)
except CvBridgeError as e:
print(e)
return imgs, timestamps
def loadOdometry(bag_path, topic_name):
poses = []
timestamps = []
counter = 0
with rosbag.Bag(bag_path, 'r') as bag:
for topic, msg, t in bag.read_messages():
if topic == topic_name:
cur_time = msg.header.stamp.to_sec()
print(cur_time)
timestamps.append(msg.header.stamp)
poses.append(msg)
counter += 1
print(topic_name,counter)
return poses, timestamps
def loadLiDAR(bag_path, topic_name):
point_clouds = []
timestamps = []
counter = 0
with rosbag.Bag(bag_path, 'r') as bag:
for topic, msg, t in bag.read_messages():
if topic == topic_name:
cur_time = msg.header.stamp.to_sec()
print(cur_time)
timestamps.append(msg.header.stamp)
point_clouds.append(msg)
counter += 1
print(topic_name,counter)
return point_clouds, timestamps
def loadIMU(imu_path):
imu_msg = Imu()
angular_v = Vector3()
linear_a = Vector3()
imu_msgs = []
timestamps = []
counter = 0
fin = open(imu_path, 'r')
fin.readline()
line = fin.readline().strip()
while line:
parts = line.split(" ")
print(len(parts))
if len(parts) < 7:
line = fin.readline().strip()
continue
ts = float(parts[0])
ax = float(parts[1])
ay = float(parts[2])
az = float(parts[3])
wx = float(parts[4])
wy = float(parts[5])
wz = float(parts[6])
imu_ts_ros = rospy.rostime.Time.from_sec(ts)
timestamps.append(imu_ts_ros)
imu_msg.header.stamp = imu_ts_ros
angular_v.x = wx
angular_v.y = wy
angular_v.z = wz
linear_a.x = ax
linear_a.y = ay
linear_a.z = az
imu_msg.angular_velocity = angular_v
imu_msg.linear_acceleration = linear_a
imu_msgs.append(imu_msg)
print("imu",counter)
counter += 1
line = fin.readline().strip()
fin.close()
return imu_msgs, timestamps
def getSummaryInfo(bag_path):
info_strs = []
info_dict = yaml.load(
subprocess.Popen(['rosbag', 'info', '--yaml', bag_path], stdout=subprocess.PIPE).communicate()[0])
end_timestamp = float(info_dict['end'])
duration = float(info_dict['duration'])
start_timestamp = end_timestamp - duration
start_time_str = "Start timestamp:" + str(start_timestamp) + " s"
end_time_str = "End timestamp:" + str(end_timestamp) + " s"
duration_str = "Duration:" + str(duration) + " s"
print("-" * 100)
print("Summary Info:")
print(start_time_str)
print(end_time_str)
print(duration_str)
info_strs.append("-" * 100 + "\n")
info_strs.append("Summary Info:\n")
info_strs.append(start_time_str + "\n")
info_strs.append(end_time_str + "\n")
info_strs.append(duration_str + "\n")
return start_timestamp
if __name__ == '__main__':
imu_path = "/home/xuhui/imu_data.txt"
bag_path = "/home/xuhui/2023-11-02-23-46-39.bag"
out_path = "./out.bag"
left_img_name = "/cam_left"
right_img_name = "/cam_right"
pose_name = "/odom"
lidar_name = "/point_cloud"
imu_name = "/imu"
left_imgs, left_timestamps = loadTopicImgs(bag_path, left_img_name)
right_imgs, right_timestamps = loadTopicImgs(bag_path, right_img_name)
poses, pose_timestamps = loadOdometry(bag_path, pose_name)
point_clouds, pc_timestamps = loadLiDAR(bag_path, lidar_name)
imu_msgs, imu_timestamps = loadIMU(imu_path)
print(len(imu_msgs), len(imu_timestamps))
bag_out = rosbag.Bag(out_path,'w')
for i in range(len(imu_msgs)):
bag_out.write(imu_name,imu_msgs[i], imu_timestamps[i])
print('save imu:',i+1,'/',len(imu_msgs))
for i in range(len(left_imgs)):
bag_out.write(left_img_name, left_imgs[i], left_timestamps[i])
bag_out.write(right_img_name, right_imgs[i], right_timestamps[i])
print("save img",i+1,"/",len(left_imgs))
for i in range(len(point_clouds)):
bag_out.write(lidar_name, point_clouds[i], pc_timestamps[i])
print("save point cloud",i+1,"/",len(point_clouds))
for i in range(len(poses)):
bag_out.write(pose_name, poses[i], pose_timestamps[i])
print("save pose gt",i+1,"/",len(poses))
bag_out.close()
利用rqt_bag工具查看合成以后的包如下。
可以看到，双目相机、IMU、LiDAR数据、位姿真值都被采集下来了。这个Bag包文件就可以拿来跑SLAM了。至此，我们就完整完成了一开始的目标。当然了，由于IMU一启动就在录制，所以比其它Topic早很多。如果需要，也可以裁剪一下。
5. 参考资料
- [1] https://docs.omniverse.nvidia.com/isaacsim/latest/advanced_tutorials/tutorial_advanced_range_sensor_lidar.html#isaac-sim-app-tutorial-advanced-range-sensor-lidar
- [2] https://docs.omniverse.nvidia.com/isaacsim/latest/features/sensors_simulation/isaac_sim_sensors_rtx_based_lidar.html
- [3] https://docs.omniverse.nvidia.com/isaacsim/latest/ros_tutorials/tutorial_ros_rtx_lidar.html?highlight=rtx%20lidar#
- [4] https://docs.omniverse.nvidia.com/isaacsim/latest/features/sensors_simulation/ext_omni_isaac_range_sensor.html?highlight=lidar#lidar-example
- [5] https://docs.omniverse.nvidia.com/isaacsim/latest/features/sensors_simulation/isaac_sim_sensors_rtx_based_lidar/lidar_config.html#rtx-lidar-config-parameters
- [6] https://docs.omniverse.nvidia.com/isaacsim/latest/features/sensors_simulation/isaac_sim_sensors_rtx_based_lidar/node_descriptions.html
- [7] https://docs.omniverse.nvidia.com/isaacsim/latest/features/sensors_simulation/isaac_sim_sensors_rtx_based_lidar/node_overview.html
本文作者原创,未经许可不得转载,谢谢配合
