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

Isaac SimにはLidarやCameraといったものが用意されているのですが、IMUは用意されていません。ここではIMUを実現してROSトピックとして出力できるようにします。

IMUとは

IMUとはInertial Measurement Unit（慣性計測装置）の略で、加速度センサとジャイロで構成されています。一般的にはIMUで観測された情報を使用して、ロボットが今どのような姿勢でどこに位置するかを推定する自己位置推定などに使用されます。

omni.isaac.imu_sensor

Isaac SimではIMUは用意されていませんが、IMUのようなものは用意されています。これはpythonから使用でき、対象オブジェクトの加速度などが取得できるという機能です。そのためIMUを搭載したい対象ロボットのbody部分のオブジェクトの加速度を取得することでIMUのようなものを実現することができます。
ただし、この機能で得られるIMUの値は真値であるため、現実のIMUのようなノイズは含まれないことに留意する必要があります。

コード

以下のコードは前回作成したROSロボットのbase_linkの加速度と角速度を取得して/imuトピックとして配信しています。

imu_msg_robo_publish.py

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

備考

上記の例では/imuトピックに角速度と加速度をセットして定期的に配信しています。