NVIDIA Omniverse和Isaac Sim笔记9：Standalone工作流介绍与IMU帧率设置

1.Standalone Python工作流初识
- 1.1 Standalone Python环境介绍
- 1.2 官方示例
2.Standalone Python基础功能代码
3.对IMU帧率的修改
4.相机和IMU不同帧率的设置
5.实际展示
6.参考资料

在上一篇笔记中，我们对相机的帧率进行了探讨，并通过修改SDGPipeline中的Isaac Simulation Gate节点实现了对于不同相机的帧率控制。在本篇笔记中，我们继续探讨帧率问题，但是IMU。这也就是上篇博客开头的第二个问题:IMU能否手动设置帧率。首先给出答案：是肯定的。不仅可以实现对于IMU帧率的设置，还可以同时实现对于相机和IMU帧率的同时控制。但要完成这个目标是有“代价”的，那就是，我们没有办法再用以前熟悉的GUI方式通过构造Action Graph的方式了，而是要通过Standalone编写代码的方式实现。在一开始的笔记中我们提到，Isaac Sim中主要有三种工作流，分别是GUI、Extensions、Standalone Python。之前我们主要接触的都是GUI，但到这篇博客，或者说为了实现上面提到的目标，就不得不用Standalone方式了，感兴趣可以阅读这个和这个网页。简单来说就是GUI方式已经到了它的天花板。所以本篇博客，我们首先会对Standalone模式进行进一步介绍，然后再说明如何使用它控制IMU帧率。最后，结合我们之前这篇笔记中搭建出来的机器人小车，实现对于相机和IMU帧率的同时控制与数据采集。

1.Standalone Python工作流初识

官方文档中对于Standalone Python工作流的介绍以及与其它两种方式的对比如下。简单来说，Standalone Python有两个主要特点，一是他可以手动精确地控制每一步渲染需要做什么(比如通过if语句更精确地控制一些逻辑)，二是他可以以headless模式(没有GUI界面)运行。

1.1 Standalone Python环境介绍

当你安装好Isaac Sim以后，它就会自带一个Python环境。所有的Standalone程序就都是通过这个Python环境运行的。这个Python环境位于Omniverse安装根目录/Libs/Isaac Sim安装目录下，比如我电脑上就是/home/xuhui/Softwares/Omniverse/Libs/isaac_sim-2022.1.1/python.sh。另外需要注意的是，在Windows下，Python环境的名字叫做python.bat。运行这个Python环境如下。可以看到，就是正常的Python。但不同之处在于，其内置了Isaac Sim的各种Python API接口，基于它们可以开发想要的功能。这些包和API在其它Python环境里没有。

1.2 官方示例

在Isaac Sim安装目录下打开终端，并输入如下命令，即可打开官方示例。比如我的路径是/home/xuhui/Softwares/Omniverse/Libs/isaac_sim-2022.1.1/。

./python.sh standalone_examples/api/omni.isaac.franka/follow_target_with_rmpflow.py

这是一个机械臂跟随的例子。我们移动这个TargetCube，机械臂会自动跟随。当跟随完成后，TargetCube颜色变成绿色，否则为红色，如下。

2.Standalone Python基础功能代码

Isaac Sim官方在这个网页中给出了一些常用功能的代码片段。我参考其它资料进行了进一步整理，归纳出以下内容。

2.1 Isaac Sim的启动与关闭

在Standalone Python模式中，我们通过如下代码开启或关闭Isaac Sim。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 编写代码，做想做的事情
print("simulation started")

# 关闭程序
simulation_app.close()

利用Isaac Sim的Python环境运行上述代码，你应该会看到Isaac Sim被启动，然后又自动退出，如下所示。可以看到，以输出的语句为界限，前面是启动，后面是退出。当然，其实你可以不在代码中关闭Isaac Sim，这样的效果就是，程序运行以后，Isaac Sim被启动。如果要结束仿真，那就在终端中按Ctrl+C强行结束即可(如果按一次没反应，多按几次就好了)。根据自己的需要自行判断是否需要关闭。根据目前的经验，其实大部分场景可能都不需要在代码中主动关闭Isaac Sim，把关闭程序的权力交给用户即可，除非是有特殊需求，比如渲染指定帧数或时间之后关闭等。

2.2 添加世界

上面启动Isaac Sim以后什么都没有，我们至少要添加一个World。在一个Isaac Sim仿真环境中，有且仅有一个World，代码如下。

from omni.isaac.core import World
world = World()

当然了，World()函数里有一些参数可以设置，比如世界的单位等等。

2.3 添加地面

在Isaac Sim中添加地面的代码如下。

world.scene.add_default_ground_plane()

通过world.scene提供的函数即可添加一个默认的地面，如下所示。

2.4 开启渲染

前面也说了，Standalone模式的一个特点就是可以手动控制渲染。截至目前，我们编写的代码如下所示。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 编写代码，做想做的事情
print("simulation started")

# 添加世界
from omni.isaac.core import World
world = World()

# 添加地面
world.scene.add_default_ground_plane()

如果直接运行的话，会报下面的错误。可以看到，程序启动成功，但是就退出了。这是因为没有渲染。具体而言，可以通过world.step()实现，所以我们需要修改代码，如下。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 编写代码，做想做的事情
print("simulation started")

# 添加世界
from omni.isaac.core import World
world = World()

# 添加地面
world.scene.add_default_ground_plane()

# 开启渲染
while True:
	world.step(render=True)

这里我们写了个死循环，这样程序就会一直渲染，直到手动Ctrl+C结束。当然你也可以更精准地控制，比如渲染100帧，然后退出，代码如下。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 编写代码，做想做的事情
print("simulation started")

# 添加世界
from omni.isaac.core import World
world = World()

# 添加地面
world.scene.add_default_ground_plane()

# 渲染100帧
for i in range(100):
	print(i+1)
	world.step(render=True)

# 关闭程序
simulation_app.close()

运行效果如下。可以看到，程序渲染了100次以后自动退出了。这里有个稍微需要注意的地方，如果渲染了100帧以后没有关闭程序，就会报和上面类似的错误(因为没有渲染了)。所以如果要手动控制渲染，渲染循环结束就要紧跟关闭程序的命令。

2.5 添加物体

和GUI方式类似的，我们也可以通过代码向场景中添加各种物体。以立方体为例，代码如下。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 编写代码，做想做的事情
print("simulation started")

# 添加世界
from omni.isaac.core import World
world = World()

# 添加地面
world.scene.add_default_ground_plane()

# 添加一个立方体
from omni.isaac.core.objects import DynamicCuboid
import numpy as np
my_cube = world.scene.add(
	DynamicCuboid(
		prim_path = "/World/my_cube",
		name = "my_cube",
		position = np.array([0,0,0.3]),
		scale = np.array([5,5,5]),
		color = np.array([0.7,0.5,1.0])))

# 根据官方文档建议，添加完物体之后，最好重置刷新一下世界
world.reset()

# 开启渲染
while True:
	world.step(render=True)

简单解释一下代码，为了添加立方体，我们需要引入DynamicCuboid这个包。然后通过world.scene.add()函数添加物体，并且手动设置如prim_path、位置、尺度、颜色等属性。启动脚本，就可以看到一个浅紫色的立方体了，效果如下。除了立方体，还有Capsule、Cone、Cylinder、Sphere。Dynamic开头表示物体受到物理引擎作用，而Visual开头则表示物体仅用于外观，不受物理引擎约束。比如下面的代码，我们添加两个物体，一个受物理引擎约束，一个不受。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 编写代码，做想做的事情
print("simulation started")

# 添加世界
from omni.isaac.core import World
world = World()

# 添加地面
world.scene.add_default_ground_plane()

# 添加两个立方体，一个受物理引擎约束，一个不受
from omni.isaac.core.objects import DynamicCuboid, VisualCuboid
import numpy as np
my_cube = world.scene.add(
	DynamicCuboid(
		prim_path = "/World/my_cube",
		name = "my_cube",
		position = np.array([0,-0.1,0.3]),
		scale = np.array([5,5,5]),
		color = np.array([0.7,0.5,1.0])))

my_cube2 = world.scene.add(
	VisualCuboid(
		prim_path = "/World/my_cube2",
		name = "my_cube2",
		position = np.array([0,0.4,0.3]),
		scale = np.array([5,5,5]),
		color = np.array([0.8,0.1,0.3])))

# 根据官方文档建议，添加完物体之后，最好重置刷新一下世界
world.reset()

# 开启渲染
while True:
	world.step(render=True)

启动后，点击播放按钮开启仿真，可以看到，一个落到地面上，一个漂浮在空中。

2.6 获得物体的属性

在Isaac Sim中，提供了一些预置好的函数，可以方便获取物体的位姿、速度等属性，如下。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 编写代码，做想做的事情
print("simulation started")

# 添加世界
from omni.isaac.core import World
world = World()

# 添加地面
world.scene.add_default_ground_plane()

# 添加一个立方体
from omni.isaac.core.objects import DynamicCuboid
import numpy as np
my_cube = world.scene.add(
	DynamicCuboid(
		prim_path = "/World/my_cube",
		name = "my_cube",
		position = np.array([0,0,0.3]),
		scale = np.array([5,5,5]),
		color = np.array([0.7,0.5,1.0])))

# 根据官方文档建议，添加完物体之后，最好重置刷新一下世界
world.reset()

# 开启渲染
while True:
	# 获取位姿、速度属性
	position, oritentation = my_cube.get_world_pose()
	linear_velocity = my_cube.get_linear_velocity()
	angular_velocity = my_cube.get_angular_velocity()

	print("Position is:", position)
	print("Orientation is:", oritentation)
	print("Linear velocity is:", linear_velocity)
	print("Angular velocity is:", angular_velocity)
	print("\n")

	world.step(render=True)

运行脚本后，如下所示，就可以看到输出的相关属性了。当然，既然是Python的代码，除了终端输出，自然也可以保存到文件，这里不再赘述。

2.7 加载已有场景

前面我们介绍了如何通过代码新建一个场景并向其中插入物体。但很多时候，我们可能会有现成的场景需要加载。Isaac Sim为我们提供了相关函数，如下。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 编写代码，做想做的事情
print("simulation started")

# 加载已有场景
from omni.isaac.core.utils.stage import open_stage
file_path = "/home/xuhui/Omniverse-Projects/our_carV6.usd"
open_stage(usd_path=file_path)

# 添加世界
from omni.isaac.core import World
world = World()

# 根据官方文档建议，添加完物体之后，最好重置刷新一下世界
world.reset()

# 开启渲染
while True:
	world.step(render=True)

执行上述代码，程序就会自动加载指定的.usd文件，如下图所示。这里的our_carV6.usd见这个Github项目。这里唯一需要注意的是，加载场景必须要在新建世界之前，不然就会报错。关于这一点的讨论可以参考这个网页。

2.8 查找指定Prim

在某些情况下，我们加载了一些已有的场景，然后想通过代码获取到一些已有的Prim，就需要用get_prim_at_path()函数。比如在我们的our_carV6.usd中，IMU的Prim Path是/our_car/camera_body/Imu_Sensor，如下所示。获取这个Prim的代码如下。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 编写代码，做想做的事情
print("simulation started")

# 加载已有场景
from omni.isaac.core.utils.stage import open_stage
file_path = "/home/xuhui/Omniverse-Projects/our_carV6.usd"
open_stage(usd_path=file_path)

# 按路径获取指定Prim
from omni.isaac.core.utils.prims import get_prim_at_path
prim = get_prim_at_path("/our_car/camera_body/Imu_Sensor")
if not prim.IsValid():
    print("Invalid prim")
else:
    print("Valid prim")

# 添加世界
from omni.isaac.core import World
world = World()

# 根据官方文档建议，添加完物体之后，最好重置刷新一下世界
world.reset()

# 开启渲染
while True:
	world.step(render=True)

运行效果如下。可以看到，在终端中输出了“Valid prim”，表示获取对象成功。

2.9 获取Prim的指定属性值

在查找到我们想要的Prim之后，自然想要获取到它相关属性的值。在Isaac Sim中提供了两种方式，一种是基于Prim类的成员函数GetAttribute()，另一种是直接调用函数get_prim_property()。以我们刚刚获取的IMU为例，尝试读取它的sensorPeriod属性，代码如下。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 编写代码，做想做的事情
print("simulation started")

# 加载已有场景
from omni.isaac.core.utils.stage import open_stage
file_path = "/home/xuhui/Omniverse-Projects/our_carV6.usd"
open_stage(usd_path=file_path)

# 按路径获取指定Prim
from omni.isaac.core.utils.prims import get_prim_at_path
prim = get_prim_at_path("/our_car/camera_body/Imu_Sensor")
if not prim.IsValid():
    print("Invalid prim")
else:
    print("Valid prim")

# 获取Prim的特定属性值
# 方法1: 调用GetAttribute()函数
frequency = prim.GetAttribute("sensorPeriod").Get()
print("IMU freq.(GetAttribute):", frequency)

# 方法2: 调用get_prim_property()函数，第一个参数是Prim路径，第二个参数是属性名称
from omni.isaac.core.utils.prims import get_prim_property
frequency = get_prim_property("/our_car/camera_body/Imu_Sensor", "sensorPeriod")
print("IMU freq.(get_prim_property):", frequency)


# 添加世界
from omni.isaac.core import World
world = World()

# 根据官方文档建议，添加完物体之后，最好重置刷新一下世界
world.reset()

# 开启渲染
while True:
	world.step(render=True)

运行结果如下，可以看到，两种方式获得的属性值是相同的。

2.10 设置Prim的指定属性值

前面我们获取了想要查看的属性值，但比如当前的值我不满意，需要重新设置，Isaac Sim也提供了相应的函数。在官方文档中也展示了两种方式。一种是调用USD API的Usd.Attribute.Set()函数，另一种则是通过ChangeProperty命令实现，这两种方式效果是一样的。还是以前面的IMU对象为例，设置它的sensorPeriod属性，代码如下。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 编写代码，做想做的事情
print("simulation started")

# 加载已有场景
from omni.isaac.core.utils.stage import open_stage
file_path = "/home/xuhui/Omniverse-Projects/our_carV6.usd"
open_stage(usd_path=file_path)

# 按路径获取指定Prim
from omni.isaac.core.utils.prims import get_prim_at_path
prim = get_prim_at_path("/our_car/camera_body/Imu_Sensor")
if not prim.IsValid():
    print("Invalid prim")
else:
    print("Valid prim")

# 打印原始属性值
frequency = prim.GetAttribute("sensorPeriod").Get()
print("IMU freq. ori:", frequency)

# 设置Prim的特定属性值
# 方法1: 调用USD API Usd.Attribute.Set()
prim.GetAttribute("sensorPeriod").Set(1/400)

# 打印第一次设置后的属性值
frequency = prim.GetAttribute("sensorPeriod").Get()
print("IMU freq. new1:", frequency)

# 方法2: 调用ChangeProperty命令
import omni.kit.commands
omni.kit.commands.execute("ChangeProperty", 
							prop_path="/our_car/camera_body/Imu_Sensor.sensorPeriod", 
							value=1/1000,
							prev=frequency)

# 打印第二次设置后的属性值
frequency = prim.GetAttribute("sensorPeriod").Get()
print("IMU freq. new2:", frequency)

# 添加世界
from omni.isaac.core import World
world = World()

# 根据官方文档建议，添加完物体之后，最好重置刷新一下世界
world.reset()

# 开启渲染
while True:
	world.step(render=True)

运行结果如下。可以看到原始数值是-1，第一次设为400Hz，对应0.025s，第二次设置为1000Hz，对应0.001s。

2.11 启用Extension

在前面的这篇笔记中我们提到，要想实现对于ROS消息的发布，需要先启动ros_bridge这个扩展，在Standalone模式中也是相同的。启用方式也很简单，如下。

# 启用ros_bridge扩展
from omni.isaac.core.utils import extensions
extensions.enable_extension("omni.isaac.ros_bridge")

我们可以在启动后的Isaac Sim中看到ros_bridge被开启了。

2.12 创建Action Graph

之前，我们都是在GUI中通过拖动的方式手动创建Action Graph，但Isaac Sim也支持代码的方式创建Action Graph。比如如下代码。如果发布的是相机渲染数据，那么需要增加"pipeline_stage": og.GraphPipelineStage.GRAPH_PIPELINE_STAGE_ONDEMAND属性。比如，这里我们以左相机为例发布Topic，代码如下。

# 一些参数
prim_cam_left_path = "/our_car/body/Camera_left"    # 左相机的Prim路径
topic_name_left_image = "/rgb_left"     # 发布的左影像Topic名称
# ------------------------------------------------------------

# standalone模式规定代码
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": False})

# 启用ROS Bridge扩展，否则Isaac Sim无法发布ROS Topic
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="/home/xuhui/Omniverse-Projects/our_carV6.usd")

# 新建世界
from omni.isaac.core import World
world = World()

# 构造Action Graph，发布相机数据
import omni.graph.core as og
keys = og.Controller.Keys
(ros_camera_graph, _, _, _) = og.Controller.edit(
    {
        "graph_path": "/publish_camera",    # 注意Graph的名称必须以/开头
        "evaluator_name": "push",
        "pipeline_stage": og.GraphPipelineStage.GRAPH_PIPELINE_STAGE_ONDEMAND,
    },
    {
        keys.CREATE_NODES: [
            ("OnPlaybackTick", "omni.graph.action.OnPlaybackTick"),
            ("createViewportLeft", "omni.isaac.core_nodes.IsaacCreateViewport"),
            ("setActiveCameraLeft", "omni.graph.ui.SetActiveViewportCamera"),
            ("cameraHelperLeft", "omni.isaac.ros_bridge.ROS1CameraHelper"),
        ],
        keys.CONNECT: [
            ("OnPlaybackTick.outputs:tick", "createViewportLeft.inputs:execIn"),
            ("createViewportLeft.outputs:viewport", "setActiveCameraLeft.inputs:execIn"),
            ("createViewportLeft.outputs:viewport", "cameraHelperLeft.inputs:viewport"),
            ("setActiveCameraLeft.outputs:execOut", "cameraHelperLeft.inputs:execIn"),
        ],
        keys.SET_VALUES: [
            ("createViewportLeft.inputs:viewportId", 1),
            ("setActiveCameraLeft.inputs:primPath", prim_cam_left_path),

            ("cameraHelperLeft.inputs:topicName", topic_name_left_image),
            ("cameraHelperLeft.inputs:type", "rgb"),
        ],
    },
)

# 运行一次构造的Graph，生成SDGPipeline
og.Controller.evaluate_sync(ros_camera_graph)
simulation_app.update()

print("start to simulate")
world.reset()

# 开始循环
while True:
    world.step(render=True)

首先打开一个新的终端，启动roscore，然后运行脚本。可以看到，Isaac Sim发布了左相机的Topic。

3.对IMU帧率的修改

基于前面提到的各种基础知识，我们在这里就可以对IMU的帧率进行修改了。

3.1 手动插入IMU并对帧率进行修改

先介绍第一种相对简单的情况，就是我们在代码中手动新建并插入一个IMU传感器，在新建的时候就对帧率进行设置，代码如下。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

# 添加世界
from omni.isaac.core import World
world = World()

# 添加地面
world.scene.add_default_ground_plane()

# 添加一个立方体
from omni.isaac.core.objects import DynamicCuboid
import numpy as np
my_cube = world.scene.add(
	DynamicCuboid(
		prim_path = "/World/my_cube",
		name = "my_cube",
		position = np.array([0,0,0.3]),
		scale = np.array([5,5,5]),
		color = np.array([0.7,0.5,1.0])))

# 新建一个IMU传感器并绑定到cube上
import omni.kit.commands
from pxr import Gf
result, sensor = omni.kit.commands.execute(
            "IsaacSensorCreateImuSensor",
            path="/imu_sensor",
            parent="/World/my_cube",
            sensor_period=1 / 1000,
            translation=Gf.Vec3d(0, 0, 0),
            orientation=Gf.Quatd(1, 0, 0, 0),
            visualize=True,
        )

# 根据官方文档建议，添加完物体之后，最好重置刷新一下世界
world.reset()

# 构造接口，获取IMU数据
from omni.isaac.isaac_sensor import _isaac_sensor
imu_data_loader = _isaac_sensor.acquire_imu_sensor_interface()

# 新建文件用于输出
fout = open("/home/xuhui/imu_data.txt", "w")

# 开始仿真
while True:
    # 读取IMU传感器数据
    imu_reading = imu_data_loader.get_sensor_readings("/World/my_cube/imu_sensor")

    # 解析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]

            print(data_timestamp, data_acc_x, data_acc_y, data_acc_z, data_ang_x, data_ang_y, data_ang_z, data_ori_x, data_ori_y, data_ori_z, data_ori_w)
            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")

    world.step(render=True)

在代码中，我们首先通过IsaacSensorCreateImuSensor命令新建了一个IMU传感器，并且在新建的时候就设置好了它的帧率。然后通过acquire_imu_sensor_interface()接口提供的get_sensor_readings()函数获取IMU数据。最后，将IMU数据保存到文件同时打印到终端。运行效果如下。可以看到，每两个IMU观测之间的时间间隔为0.001秒，也就是1ms，对应1000Hz。

3.2 加载已有IMU并对帧率进行修改

另一种情况则是，IMU传感器已经创建好了，我们只需要加载相关场景以后，读取IMU的sensor_period属性并修改即可，这主要用到2.9和2.10部分的知识。代码如下。

# 启动Isaac Sim，对于Standalone方式而言是必须的
from omni.isaac.kit import SimulationApp
# 可以选择是否以headless模式运行
simulation_app = SimulationApp({"headless": False})

scene_path = "/home/xuhui/Omniverse-Projects/our_carV6.usd" # 场景+小车所在的usd文件路径
imu_outpath = "/home/xuhui/omni-record/imu_data.txt"        # IMU数据输出文件路径
frequency_imu = 500                     # IMU的观测频率

# 加载环境与机器人(要在新建世界之前，不然会报错)
from omni.isaac.core.utils.stage import open_stage
open_stage(usd_path=scene_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("/our_car/camera_body/Imu_Sensor")
prim_imu.GetAttribute("sensorPeriod").Set(1/frequency_imu)

# 根据官方文档建议，添加完物体之后，最好重置刷新一下世界
world.reset()

# 构造接口，获取IMU数据
from omni.isaac.isaac_sensor import _isaac_sensor
imu_data_loader = _isaac_sensor.acquire_imu_sensor_interface()

# 新建文件用于输出
fout = open("/home/xuhui/imu_data.txt", "w")

# 开始仿真
while True:
    # 读取IMU传感器数据
    imu_reading = imu_data_loader.get_sensor_readings("/our_car/camera_body/Imu_Sensor")

    # 解析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]

            print(data_timestamp, data_acc_x, data_acc_y, data_acc_z, data_ang_x, data_ang_y, data_ang_z, data_ori_x, data_ori_y, data_ori_z, data_ori_w)
            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")

    world.step(render=True)

运行以后，效果如下。相邻观测的时间间隔为0.002秒，也即500Hz。

3.3 小结

至此，我们便完成了对于IMU数据的帧率设置以及对数据的保存，也就顺利回答了上篇博客的第二个问题：IMU能否设置帧率。对于IMU数据的保存，当然你可能会有疑问，为什么这里对于IMU的数据不通过ROS发布。其实我试过，但是或多或少会有问题。第一种方案是，在usd文件中写好IMU数据发布的Action Graph，然后在脚本中修改IMU帧率。这种做法经过实验，发现帧率修改没有作用。第二种方案是，在代码中构造Action Graph，然后再发布，效果和方案一一样，帧率没有改变。第三种方案是在代码中获取IMU数据以后，直接利用ROS的Python接口进行发布。这种方案理论上是可行的。Isaac Sim官方给出的VINS示例也是这样做的。但这种方案在我电脑上有个bug(可能是因为我的环境没配置好导致的)，那就是运行脚本是在Isaac Sim提供的Python环境里，但这个环境里并没有ROS的Pyhton接口，比如rospy、rospkg等包，没办法直接发布ROS Topic。所以才有了第四种方案，也就是读取IMU数据以后，直接保存成文本文件，再经过后期脚本，变成ROS Bag。所以小结一下就是：对于IMU数据，不建议使用Action Graph方式发布(无论是GUI方式构建的Graph还是Standalone编写代码构建的)，这种方式发布的数据最多只能达到渲染上限，而且没办法改变帧率，即使通过代码修改了IMU的sensorPeriod属性。所以如果要保存或者发布IMU数据，要么通过ROS的Python接口，要么直接保存为文件，后期再处理。

4.相机和IMU不同帧率的设置

接着，让我们回答上篇博客的第三个问题，不同传感器如何设置不同帧率。对于多个相机设置不同帧率的问题，在上篇博客已经探讨过了。而如何同时设置相机和IMU的帧率呢？这也就是这一部分的内容。

4.1 Standalone方式设置相机帧率

尽管在上篇笔记，我们通过修改SDGPipeline的Isaac Simulation Gate的Step属性完成了对相机帧率的修改，但这是在GUI方式下完成的。正如前面说的，如果要对IMU帧率进行修改，只能通过Standalone的方式。所以这里我们首先就先要了解，如何在Standalone模式下构造Action Graph并修改相机帧率。从本质上来说，思路是相同的，只是把我们之前手动的方式用代码实现。代码如下。

# 已知bug：在启动仿真环境后，需要手动修改Viewport:
# Viewport1 -> Overall_camera
# Viewport2 -> Camera_left
# Viewport3 -> Camera_right

# 代码主要实现对于双目数据的帧率设置与发布

# 一些参数
scene_path = "/home/xuhui/Omniverse-Projects/our_carV6.usd" # 场景+小车所在的usd文件路径

prim_cam_left_path = "/our_car/body/Camera_left"    # 左相机的Prim路径
prim_cam_right_path = "/our_car/body/Camera_right"  # 右相机的Prim路径

rgb_interval = 10                       # 每隔N个渲染帧发布一帧
topic_name_left_image = "/rgb_left"     # 发布的左影像Topic名称
topic_name_right_image = "/rgb_right"   # 发布的右影像Topic名称

# ------------------------------------------------------------

# standalone模式规定代码
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": False})

# 启用ROS Bridge扩展，否则Isaac Sim无法发布ROS Topic
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=scene_path)

# 新建世界
from omni.isaac.core import World
world = World()

# 构造Action Graph，发布相机数据
import omni.graph.core as og
keys = og.Controller.Keys
(ros_camera_graph, _, _, _) = og.Controller.edit(
    {
        "graph_path": "/publish_camera",    # 注意Graph的名称必须以/开头
        "evaluator_name": "push",
        "pipeline_stage": og.GraphPipelineStage.GRAPH_PIPELINE_STAGE_ONDEMAND,
    },
    {
        keys.CREATE_NODES: [
            ("OnPlaybackTick", "omni.graph.action.OnPlaybackTick"),

            ("createViewportLeft", "omni.isaac.core_nodes.IsaacCreateViewport"),
            ("createViewportRight", "omni.isaac.core_nodes.IsaacCreateViewport"),

            ("setActiveCameraLeft", "omni.graph.ui.SetActiveViewportCamera"),
            ("setActiveCameraRight", "omni.graph.ui.SetActiveViewportCamera"),

            ("cameraHelperLeft", "omni.isaac.ros_bridge.ROS1CameraHelper"),
            ("cameraHelperRight", "omni.isaac.ros_bridge.ROS1CameraHelper"),
        ],
        keys.CONNECT: [
            ("OnPlaybackTick.outputs:tick", "createViewportLeft.inputs:execIn"),
            ("createViewportLeft.outputs:viewport", "setActiveCameraLeft.inputs:execIn"),
            ("createViewportLeft.outputs:viewport", "cameraHelperLeft.inputs:viewport"),
            ("setActiveCameraLeft.outputs:execOut", "cameraHelperLeft.inputs:execIn"),

            ("OnPlaybackTick.outputs:tick", "createViewportRight.inputs:execIn"),
            ("createViewportRight.outputs:viewport", "setActiveCameraRight.inputs:viewport"),
            ("createViewportRight.outputs:viewport", "cameraHelperRight.inputs:viewport"),
            ("setActiveCameraRight.outputs:execOut", "cameraHelperRight.inputs:execIn"),
        ],
        keys.SET_VALUES: [
            ("createViewportLeft.inputs:viewportId", 1),
            ("createViewportRight.inputs:viewportId", 2),
            ("setActiveCameraLeft.inputs:primPath", prim_cam_left_path),
            ("setActiveCameraRight.inputs:primPath", prim_cam_right_path),

            ("cameraHelperLeft.inputs:topicName", topic_name_left_image),
            ("cameraHelperLeft.inputs:type", "rgb"),

            ("cameraHelperRight.inputs:topicName", topic_name_right_image),
            ("cameraHelperRight.inputs:type", "rgb"),
        ],
    },
)

# 运行一次构造的Graph，生成SDGPipeline
og.Controller.evaluate_sync(ros_camera_graph)
simulation_app.update()

# 修改SDGPipeline对应的节点，调整帧率
left_camera_path = "/Render/PostProcess/SDGPipeline/RenderProduct_Viewport_2_LdrColorSDIsaacSimulationGate"
right_camera_path = "/Render/PostProcess/SDGPipeline/RenderProduct_Viewport_3_LdrColorSDIsaacSimulationGate"
import omni.graph.core as og
og.Controller.attribute(left_camera_path + ".inputs:step").set(rgb_interval)
og.Controller.attribute(right_camera_path + ".inputs:step").set(rgb_interval)

print("start to simulate")
world.reset()

# 开始循环渲染
while True:
    world.step(render=True)

运行上述代码，如下。可以看到，相机帧率按照我们的设定降下来了。在我电脑上跑满渲染帧率大约在60FPS左右，每隔10帧发布一次，就是大约6FPS。当然，正如同代码一开始写的，目前还有个潜在的bug，就是运行脚本之后，Viewport的内容需要手动设置一下，不然不太对。目前不知道如何解决，欢迎大佬指正。官方也提供了相应的代码，感兴趣可以看看，思路是一样的。

4.2 相机与IMU联合

在实现了利用代码控制帧率并发布相机数据以后，接着就可以利用3.2部分的知识对IMU帧率进行控制。代码如下。

# 已知bug：在启动仿真环境后，需要手动修改Viewport:
# Viewport1 -> Overall_camera
# Viewport2 -> Camera_left
# Viewport3 -> Camera_right

# 代码主要实现对于双目数据的帧率设置与发布、IMU的帧率设置与发布

# 一些参数
scene_path = "/home/xuhui/Omniverse-Projects/our_carV6.usd" # 场景+小车所在的usd文件路径
imu_outpath = "/home/xuhui/omni-record/imu_data.txt"        # IMU数据输出文件路径

prim_imu_path = "/our_car/camera_body/Imu_Sensor"   # IMU的Prim路径
prim_cam_left_path = "/our_car/body/Camera_left"    # 左相机的Prim路径
prim_cam_right_path = "/our_car/body/Camera_right"  # 右相机的Prim路径

frequency_imu = 500                     # IMU的观测频率
rgb_interval = 10                       # 每隔N个渲染帧发布一帧
topic_name_left_image = "/rgb_left"     # 发布的左影像Topic名称
topic_name_right_image = "/rgb_right"   # 发布的右影像Topic名称

# ------------------------------------------------------------

# standalone模式规定代码
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": False})

# 启用ROS Bridge扩展，否则Isaac Sim无法发布ROS Topic
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=scene_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(prim_imu_path)
prim_imu.GetAttribute("sensorPeriod").Set(1/frequency_imu)

# 构造Action Graph，发布相机数据
import omni.graph.core as og
keys = og.Controller.Keys
(ros_camera_graph, _, _, _) = og.Controller.edit(
    {
        "graph_path": "/publish_camera",    # 注意Graph的名称必须以/开头
        "evaluator_name": "push",
        "pipeline_stage": og.GraphPipelineStage.GRAPH_PIPELINE_STAGE_ONDEMAND,
    },
    {
        keys.CREATE_NODES: [
            ("OnPlaybackTick", "omni.graph.action.OnPlaybackTick"),

            ("createViewportLeft", "omni.isaac.core_nodes.IsaacCreateViewport"),
            ("createViewportRight", "omni.isaac.core_nodes.IsaacCreateViewport"),

            ("setActiveCameraLeft", "omni.graph.ui.SetActiveViewportCamera"),
            ("setActiveCameraRight", "omni.graph.ui.SetActiveViewportCamera"),

            ("cameraHelperLeft", "omni.isaac.ros_bridge.ROS1CameraHelper"),
            ("cameraHelperRight", "omni.isaac.ros_bridge.ROS1CameraHelper"),

            ("computeOdometry","omni.isaac.core_nodes.IsaacComputeOdometry"),
            ("getTime","omni.isaac.core_nodes.IsaacReadSimulationTime"),
            ("publishOdometry","omni.isaac.ros_bridge.ROS1PublishOdometry"),
        ],
        keys.CONNECT: [
            ("OnPlaybackTick.outputs:tick", "createViewportLeft.inputs:execIn"),
            ("createViewportLeft.outputs:viewport", "setActiveCameraLeft.inputs:execIn"),
            ("createViewportLeft.outputs:viewport", "cameraHelperLeft.inputs:viewport"),
            ("setActiveCameraLeft.outputs:execOut", "cameraHelperLeft.inputs:execIn"),

            ("OnPlaybackTick.outputs:tick", "createViewportRight.inputs:execIn"),
            ("createViewportRight.outputs:viewport", "setActiveCameraRight.inputs:viewport"),
            ("createViewportRight.outputs:viewport", "cameraHelperRight.inputs:viewport"),
            ("setActiveCameraRight.outputs:execOut", "cameraHelperRight.inputs:execIn"),
        ],
        keys.SET_VALUES: [
            ("createViewportLeft.inputs:viewportId", 1),
            ("createViewportRight.inputs:viewportId", 2),
            ("setActiveCameraLeft.inputs:primPath", prim_cam_left_path),
            ("setActiveCameraRight.inputs:primPath", prim_cam_right_path),

            ("cameraHelperLeft.inputs:topicName", topic_name_left_image),
            ("cameraHelperLeft.inputs:type", "rgb"),

            ("cameraHelperRight.inputs:topicName", topic_name_right_image),
            ("cameraHelperRight.inputs:type", "rgb"),
        ],
    },
)

# 运行一次构造的Graph，生成SDGPipeline
og.Controller.evaluate_sync(ros_camera_graph)
simulation_app.update()

# 修改SDGPipeline对应的节点，调整帧率
left_camera_path = "/Render/PostProcess/SDGPipeline/RenderProduct_Viewport_2_LdrColorSDIsaacSimulationGate"
right_camera_path = "/Render/PostProcess/SDGPipeline/RenderProduct_Viewport_3_LdrColorSDIsaacSimulationGate"
import omni.graph.core as og
og.Controller.attribute(left_camera_path + ".inputs:step").set(rgb_interval)
og.Controller.attribute(right_camera_path + ".inputs:step").set(rgb_interval)

# 获取IMU传感器对象
from omni.isaac.isaac_sensor import _isaac_sensor
_is = _isaac_sensor.acquire_imu_sensor_interface()

print("start to simulate")
frame_counter = 0
imu_out = open(imu_outpath,"w")
world.reset()

# 开始循环，在循环过程中采集IMU数据并输出至指定文件
while True:
    # 读取IMU传感器数据
    imu_reading = _is.get_sensor_readings(prim_imu_path)

    # 解析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]

            imu_out.write(str(data_timestamp)+"\t"+
                str(data_acc_x)+"\t"+str(data_acc_y)+"\t"+str(data_acc_z)+"\t"+
                str(data_ang_x)+"\t"+str(data_ang_y)+"\t"+str(data_ang_z)+"\t"+
                str(data_ori_x)+"\t"+str(data_ori_y)+"\t"+str(data_ori_z)+"\t"+str(data_ori_w)+"\n")
            # print(data_timestamp, data_acc_x, data_acc_y, data_acc_z, data_ang_x, data_ang_y, data_ang_z, data_ori_x, data_ori_y, data_ori_z, data_ori_w)

    world.step(render=True)
    
    if frame_counter % 5000 == 0:
        print("IMU Recoding in progress")
    frame_counter += 1

运行上述脚本，首先可以看到，相机帧率被修改了，如下。然后，打开保存的IMU数据，如下。可以看到，IMU帧率也和设置的是相同的。

4.3 小结

至此，我们真正意义上回答了上篇博客的第三个问题。通过Standalone方式，同时修改了相机和IMU的帧率。

5.实际展示

最后一部分，我们展示一下利用Isaac Sim进行数据采集的全过程，包括环境和机器人的搭建、Standalone脚本编写、数据采集、数据后处理步骤。为了方便，把每一步骤的相关文件都上传到了Github，点击查看，欢迎Star或Fork。

5.1 环境和机器人的搭建

机器人和环境的搭建在之前这篇博客中就已经完成了。但我们需要对其稍作修改。

对于小车的控制，以GUI方式搭建Action Graph完成
对于小车相机数据发布和帧率修改，以Standalone方式构建Action Graph并修改SDGPipeline
对于小车IMU数据的发布与帧率修改，以Standalone方式修改，结果保存到文件中
对于小车位姿真值发布，以GUI方式搭建Action Graph完成

所以，我们需要把之前的那个很大的Aciton Graph删除一部分，主要是双目相机的流程和IMU数据发布的流程，只保留位姿真值发布流程。删除完以后如下所示。修改后的场景文件见Github项目中的ourCarV6.usd文件。

5.2 Standalone脚本编写

我们脚本的核心目的是构建Action Graph发布双目数据、修改SDGPipeline以改变帧率、修改IMU传感器的sensor_period以修改IMU帧率、将IMU数据保存到文件。完整代码如下。

# 已知bug：在启动仿真环境后，需要手动修改Viewport:
# Viewport1 -> Overall_camera
# Viewport2 -> Camera_left
# Viewport3 -> Camera_right

# 小车的控制、位姿真值的发布由GUI Actin Graph完成(真值Topic名称为/odom)
# 代码主要实现对于双目数据的帧率设置与发布、IMU的帧率设置与发布

# 一些参数
scene_path = "/home/xuhui/Omniverse-Projects/our_carV6.usd" # 场景+小车所在的usd文件路径
imu_outpath = "/home/xuhui/omni-record/imu_data.txt"        # IMU数据输出文件路径

prim_imu_path = "/our_car/camera_body/Imu_Sensor"   # IMU的Prim路径
prim_cam_left_path = "/our_car/body/Camera_left"    # 左相机的Prim路径
prim_cam_right_path = "/our_car/body/Camera_right"  # 右相机的Prim路径

frequency_imu = 500                     # IMU的观测频率
rgb_interval = 10                       # 每隔N个渲染帧发布一帧
topic_name_left_image = "/rgb_left"     # 发布的左影像Topic名称
topic_name_right_image = "/rgb_right"   # 发布的右影像Topic名称

# ------------------------------------------------------------

# standalone模式规定代码
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": False})

# 启用ROS Bridge扩展，否则Isaac Sim无法发布ROS Topic
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=scene_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(prim_imu_path)
prim_imu.GetAttribute("sensorPeriod").Set(1/frequency_imu)

# 构造Action Graph，发布相机数据
import omni.graph.core as og
keys = og.Controller.Keys
(ros_camera_graph, _, _, _) = og.Controller.edit(
    {
        "graph_path": "/publish_camera",    # 注意Graph的名称必须以/开头
        "evaluator_name": "push",
        "pipeline_stage": og.GraphPipelineStage.GRAPH_PIPELINE_STAGE_ONDEMAND,
    },
    {
        keys.CREATE_NODES: [
            ("OnPlaybackTick", "omni.graph.action.OnPlaybackTick"),

            ("createViewportLeft", "omni.isaac.core_nodes.IsaacCreateViewport"),
            ("createViewportRight", "omni.isaac.core_nodes.IsaacCreateViewport"),

            ("setActiveCameraLeft", "omni.graph.ui.SetActiveViewportCamera"),
            ("setActiveCameraRight", "omni.graph.ui.SetActiveViewportCamera"),

            ("cameraHelperLeft", "omni.isaac.ros_bridge.ROS1CameraHelper"),
            ("cameraHelperRight", "omni.isaac.ros_bridge.ROS1CameraHelper"),
        ],
        keys.CONNECT: [
            ("OnPlaybackTick.outputs:tick", "createViewportLeft.inputs:execIn"),
            ("createViewportLeft.outputs:viewport", "setActiveCameraLeft.inputs:execIn"),
            ("createViewportLeft.outputs:viewport", "cameraHelperLeft.inputs:viewport"),
            ("setActiveCameraLeft.outputs:execOut", "cameraHelperLeft.inputs:execIn"),

            ("OnPlaybackTick.outputs:tick", "createViewportRight.inputs:execIn"),
            ("createViewportRight.outputs:viewport", "setActiveCameraRight.inputs:viewport"),
            ("createViewportRight.outputs:viewport", "cameraHelperRight.inputs:viewport"),
            ("setActiveCameraRight.outputs:execOut", "cameraHelperRight.inputs:execIn"),
        ],
        keys.SET_VALUES: [
            ("createViewportLeft.inputs:viewportId", 1),
            ("createViewportRight.inputs:viewportId", 2),
            ("setActiveCameraLeft.inputs:primPath", prim_cam_left_path),
            ("setActiveCameraRight.inputs:primPath", prim_cam_right_path),

            ("cameraHelperLeft.inputs:topicName", topic_name_left_image),
            ("cameraHelperLeft.inputs:type", "rgb"),

            ("cameraHelperRight.inputs:topicName", topic_name_right_image),
            ("cameraHelperRight.inputs:type", "rgb"),
        ],
    },
)

# 运行一次构造的Graph，生成SDGPipeline
og.Controller.evaluate_sync(ros_camera_graph)
simulation_app.update()

# 修改SDGPipeline对应的节点，调整帧率
left_camera_path = "/Render/PostProcess/SDGPipeline/RenderProduct_Viewport_2_LdrColorSDIsaacSimulationGate"
right_camera_path = "/Render/PostProcess/SDGPipeline/RenderProduct_Viewport_3_LdrColorSDIsaacSimulationGate"
import omni.graph.core as og
og.Controller.attribute(left_camera_path + ".inputs:step").set(rgb_interval)
og.Controller.attribute(right_camera_path + ".inputs:step").set(rgb_interval)

# 获取IMU传感器对象
from omni.isaac.isaac_sensor import _isaac_sensor
_is = _isaac_sensor.acquire_imu_sensor_interface()

print("start to simulate")
frame_counter = 0
imu_out = open(imu_outpath,"w")
world.reset()

# 开始循环，在循环过程中采集IMU数据并输出至指定文件
while True:
    # 读取IMU传感器数据
    imu_reading = _is.get_sensor_readings(prim_imu_path)

    # 解析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]

            imu_out.write(str(data_timestamp)+"\t"+
                str(data_acc_x)+"\t"+str(data_acc_y)+"\t"+str(data_acc_z)+"\t"+
                str(data_ang_x)+"\t"+str(data_ang_y)+"\t"+str(data_ang_z)+"\t"+
                str(data_ori_x)+"\t"+str(data_ori_y)+"\t"+str(data_ori_z)+"\t"+str(data_ori_w)+"\n")
            # print(data_timestamp, data_acc_x, data_acc_y, data_acc_z, data_ang_x, data_ang_y, data_ang_z, data_ori_x, data_ori_y, data_ori_z, data_ori_w)

    world.step(render=True)
    
    if frame_counter % 5000 == 0:
        print("IMU Recoding in progress")
    frame_counter += 1

运行脚本之后，在Isaac Sim中就可以通过键盘控制小车。同时打开rqt监控工具，可以看到同时发布了相关Topic，如下。这里需要说明的是1.Isaac Sim支持同时运行多个Action Graph，这也就是为什么可以利用键盘控制小车，并且发布数据的原因。2.对于IMU数据的录制从启动程序的那一刻就开始了，初始时间戳为0。相机和位姿真值同样是从启动就会发布Topic。但如果运行脚本以后，点击了结束按钮后，再重新启动仿真环境。相机和位姿真值的时间戳仍然会继续累加，但IMU时间戳会从零重新开始。完整的代码文件见Github项目中的runSimulation.py文件。

5.3 数据录制

对于IMU数据，无需操心，程序自动将其记录在文本文件中，如下。而对于位姿真值、双目相机数据，则需要通过ROS工具进行录制。打开一个新终端，输入如下内容即可开启录制。

rosbag record /odom /rgb_left /rgb_right

这里稍微需要注意的是，因为我们前面编写的代码文件有个需要手动切换Viewport内容的bug。所以这里并不建议一运行脚本就立刻开启录制。而是等手动切换完成，确保没什么问题了以后再开启录制。录制完成后的Bag信息如下。下一步要做的就是，把文本文件形式的IMU数据和现有的ROS Bag文件整合起来。

5.4 数据后处理

将文本文件形式保存的IMU数据转换到ROS Bag文件，其实在之前的这篇笔记中已经介绍了。这里直接基于它进行简单修改即可。核心思路就是，对于已有的Bag文件，利用函数依次读取里面的数据，然后直接“转存”到新的Bag文件中即可。对于IMU数据，则按照ROS要求，构建ROS的IMU数据格式，最后将它们写入新的Bag文件。完整代码如下。

# 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 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("\t")
        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/omni-record/imu_data.txt"
    bag_path = "/home/xuhui/omni-record/2023-02-19-23-16-29.bag"
    out_path = "/home/xuhui/omni-record/2023-02-19-23-16-29-out.bag"
    left_img_name = "/rgb_left"
    right_img_name = "/rgb_right"
    pose_name = "/odom"
    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)
    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(poses)):
        bag_out.write(pose_name, poses[i], pose_timestamps[i])
        print("save pose gt",i+1,"/",len(poses))

    bag_out.close()

运行结束以后，会生成新的ROS Bag文件，查看相关信息如下。利用rqt_bag工具查看如下。可以看到，已经成功将这几个需要的Topic放到了同一个ROS Bag里面。完整的代码文件见Github项目中的joinBag.py文件。

5.5 数据可视化

如下所示，我们播放整合好的ROS Bag文件，利用RViz可视化位姿真值、影像数据，如下。可以看到，一切正常。以上所有代码均在Isaac Sim 2022.1.1版本测试通过。至此，我们便算是真正意义上回答了上篇博客中的所有三个问题。并且把利用Isaac Sim进行数据采集的流程全部走了一遍，依靠这个流程采集的数据终于达到“真正可用”的状态，而不再只是看起来能用的数据，暂时没发现新的问题。这期间解决了很多问题，也确实遇到了太多太多的坑和挫折，不过也学到了很多。Isaac Sim的相关笔记就暂时告一段落了。之后如果遇到了新的需求、新的问题会再做记录。

6.参考资料

[1] https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_intro_workflows.html#isaac-sim-app-tutorial-intro-workflows
[2] https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_core_hello_world.html#isaac-sim-app-tutorial-core-hello-world
[3] https://zhuanlan.zhihu.com/p/546773212
[4] https://forums.developer.nvidia.com/t/loading-usd-scene-in-standalone-application/203052
[5] https://docs.omniverse.nvidia.com/prod_usd/prod_usd/python-snippets/properties/get-attribute-value.html
[6] https://docs.omniverse.nvidia.com/prod_usd/prod_usd/python-snippets/properties/set-attribute.html
[7] https://forums.developer.nvidia.com/t/can-i-set-imu-frequency-in-isaac-sim/226055
[8] https://forums.developer.nvidia.com/t/imu-sensor-hz-setting-problem-using-ros2-in-isaac-simulator/237944
[9] https://forums.developer.nvidia.com/t/imu-sensor-readings-false/214533
[10] https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/reference_python_snippets.html
[11] https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_ros_python.html#isaac-sim-app-tutorial-ros-python
[12] https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_ros_quadruped_vio.html

Menu