Isaac SimでROSロボットにIMUを搭載する

import numpy as np
import carb
import omni
import asyncio

from omni.isaac.kit import SimulationApp

simulation_app = SimulationApp({"headless": False})

from omni.isaac.core import World
from omni.isaac.core.utils.nucleus import find_nucleus_server
from omni.isaac.core.utils.stage import add_reference_to_stage

from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.imu_sensor import _imu_sensor

my_world = World(stage_units_in_meters=0.01)

enable_extension("omni.isaac.ros_bridge")

_, nucleus_server = find_nucleus_server()
asset_path = nucleus_server + "/Users/hardbake/Office/office.usd"
add_reference_to_stage(asset_path, "/World/Office")
asset_path = nucleus_server + "/Users/hardbake/vehicle_robo.usd"
add_reference_to_stage(asset_path, "/World/Office/vehicle_robo")

_is = _imu_sensor.acquire_imu_sensor_interface()

props = _imu_sensor.SensorProperties()
props.position = carb.Float3(0, 0, 0)
props.orientation = carb.Float4(0, 0, 0, 1)
props.sensorPeriod = 1 / 500

sensor_handle = _is.add_sensor_on_body("/World/Office/vehicle_robo/base_link", props)

simulation_app.update()
result, check = omni.kit.commands.execute("RosBridgeRosMasterCheck")
if not check:
    carb.log_error("Please run roscore before executing this script")
    simulation_app.close()
    exit()

import rospy
rospy.init_node("imu_publisher", anonymous=True, disable_signals=True, log_level=rospy.ERROR)

async def my_task():
    from sensor_msgs.msg import Imu
    pub = rospy.Publisher("/imu", Imu, queue_size=10)

    while True:
        msg = Imu()
        readings = _is.get_sensor_readings(sensor_handle)
        msg.header.stamp = rospy.Time.from_seconds(readings[0][0])
        msg.header.frame_id = "base_link"
        msg.angular_velocity.x = readings[0][4]
        msg.angular_velocity.y = readings[0][5]
        msg.angular_velocity.z = readings[0][6]
        msg.linear_acceleration.x = readings[0][1]
        msg.linear_acceleration.y = readings[0][2]
        msg.linear_acceleration.z = readings[0][3]
        pub.publish(msg)
        await asyncio.sleep(0.1)
    pub.unregister()
    pub = None

asyncio.ensure_future(my_task())

my_world.play()

while simulation_app.is_running():
    my_world.step(render=True)

my_world.stop()
simulation_app.close()