ROS 2を用いたロボットの実機とシミュレーションによるデジタルツイン連動の実践：ROSの進化とデジタルツインの可能性（後編）（3/5 ページ）

[富士ソフト，MONOist]

3．シミュレーションと実機に発生する同期ズレの調整

　ここまで、Isaac Sim上でTurtlebot3を動かす方法について説明した。実際にシミュレーション上のロボットと実機ロボットを同期させる時には、物理演算などでズレが発生する場合がある。高度な物理演算機能を持つIsaac Simには、オブジェクト同士の摩擦係数やタイヤ回転のステータスなど設定が必要な多くのパラメータがあり、ズレを調整することは容易ではない。

　そこで今回、実機から送信される/odomトピックを使って、Isaac Sim上のTurtlebot3の位置を実機のTurtlebot3とリアルタイムで同期させてみよう。

　Omni Graphには/odomを受信するノードがないため、Isaac Sim APIを使用して、Pythonから/odomトピックを受信し、ロボットの位置を調整する手法を用いる。Isaac SimはGUIから起動する方法の他に、Pythonから起動してその中でAPIを使いながらシミュレーションを動かす機能を有している（図18）。

図18　Isaac Sim APIを使用して、Pythonから/odomトピックを受信し、ロボットの位置を調整する［クリックで拡大］

　ここでは、Turtlebot3実機ではROS 2によるセットアップ（ROBOTISのe-Mnualの「3．Quick Start Guide」まで）が完了しているものとする。また、ROS 2通信のため、Isaac SimのPCとTurtlebot3実機は同一ネットワーク上に置く。

（1）/odomを受信し、APIを用いてロボットの位置を調整するコード（リスト6）を作成する。

from omni.isaac.kit import SimulationApp
import argparse
#ROS2 Bridgeの有効化設定
parser = argparse.ArgumentParser(description="Generate Occluded and Unoccluded data")
parser.add_argument("--test", action="store_true")
parser.add_argument(
    "--ros2_bridge",
    default="omni.isaac.ros2_bridge",
    nargs="?",
    choices=["omni.isaac.ros2_bridge", "omni.isaac.ros2_bridge-humble"],
)
args, unknown = parser.parse_known_args()
# Isaac Simの初期設定
simulation_app = SimulationApp({"renderer": "RayTracedLighting", "headless": False})
import omni
from omni.isaac.core import SimulationContext
from pxr import Gf
#ROS2 Bridgeの有効化
from omni.isaac.core.utils.extensions import enable_extension
enable_extension(args.ros2_bridge)
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
# /odomを受信し、位置同期を行うノード
class Subscriber(Node):
    def __init__(self):
        super().__init__("move_robot_subscriber")
        #Isaac Simの起動処理
        usd_path =  "〇〇.usd”※「2.2.3 Isaac SimとROSの連携」で作成したUSDステージのファイルパス
        omni.usd.get_context().open_stage(usd_path, None)
        self.stage = omni.usd.get_context().get_stage()
        simulation_app.update()
        print("Loading stage...")
        from omni.isaac.core.utils.stage import is_stage_loading
        while is_stage_loading():
            simulation_app.update()
        print("Loading Complete")
        self.simulation_context = SimulationContext(physics_prim_path ="/Environment/physicsScene")
        self.simulation_context.play()
        self.simulation_context.step()
        for frame in range(60):
            self.simulation_context.step()
        
        #変数の初期化
        self.x_initial=0.0
        self.y_initial=0.0
        self._robot_position = (0, 0, 0)
        self._robot_orientation = Gf.Quatd(0.0, 0.0, 0.0, 1.0)
        self._topic_sub_flg= False
        self.ros_sub = self.create_subscription(Odometry, "odom", self.move_robot_callback, 10)
    def move_robot_callback(self, data):
        if self.simulation_context.is_playing():
            topic_pos=data.pose.pose.position
            topic_orientation= data.pose.pose.orientation
            #/odomの初期値をIsaacSimの(0,0)とする
            if self.x_initial == 0.0:
                self.x_initial = topic_pos.x
                self.y_initial = topic_pos.y
            #位置,角度情報の格納
            self._robot_position = (topic_pos.x-self.x_initial , topic_pos.y-self.y_initial, topic_pos.z)
            self._robot_orientation = Gf.Quatd(topic_orientation.w, topic_orientation.x, topic_orientation.y, topic_orientation.z)
            self._topic_sub_flg= True
    def run_simulation(self):
        #Isaac Simの実行処理
        self.simulation_context.play()
        while simulation_app.is_running():
            self.simulation_context.step(render=True)
            rclpy.spin_once(self, timeout_sec=0.0)
            if self.simulation_context.is_playing():
                if self.simulation_context.current_time_step_index == 0:
                    self.simulation_context.reset()
                #/odomを受信したら位置を同期
                if self._topic_sub_flg:
                    obj = self.stage.GetPrimAtPath("/World/turtlebot3_burger/base_link")
                    obj.GetAttribute("xformOp:translate").Set(self._robot_position)
                    print(self._robot_orientation)
                    obj.GetAttribute("xformOp:orient").Set(self._robot_orientation)
                    self._topic_sub_flg= False
        self.simulation_context.stop()
        self.destroy_node()
        simulation_app.close()
if __name__ == "__main__":
    rclpy.init()
    subscriber = Subscriber()
    subscriber.run_simulation()

リスト6　/odomを受信し、APIを用いてロボットの位置を調整するコード

（2）作成したコードを任意の場所に保存する。

　今回は“~/.local/share/ov/pkg/isaac_sim-2022.2.1/ standalone_examples/api/omni.isaac.ros2_bridge/”に“turtlebot3_move.py”で保存する。

（3）Isaac Sim起動のため、ROSの環境変数設定を行う（リスト7）。

$ unset LD_LIBRARY_PATH
$ export FASTRTPS_DEFAULT_PROFILES_FILE=~/.ros/fastdds.xml

リスト7　ROSの環境変数設定

（4）~/.local/share/ov/pkg/isaac_sim-2022.2.1上でリスト8のコマンドを用いて、保存したPythonコードを実行する。Isaac Simが起動し、Turtlebot3が表示される。

$ cd ~/.local/share/ov/pkg/isaac_sim-2022.2.1
$ ./python.sh standalone_examples/api/omni.isaac.ros2_bridge/turtlebot3_move.py

リスト8　Isaac Simが起動し、Turtlebot3を表示するコマンド

（5）実機側のターミナルで、Turtlebot3で用意されているROS 2での実機制御プログラムを起動する（リスト9）^※3）。

※3）Turtlebot3の起動

$ ros2 launch turtlebot3_bringup robot.launch.py

リスト9　ROS 2での実機制御プログラムを起動するコマンド

　/cmd_velでTurtlebot3を操作すると、Isaac Sim上のTurtlebot3と実機のTurtlebot3の動きが同期していることが分かる（図19）^※4）。

※4）キーボードによるTurtlebot3の操作

図19　Isaac Sim上のTurtlebot3と実機のTurtlebot3の動きが同期

　プログラムの仕組みとしては、Isaac Simの起動処理後、/odomトピックを受信するノードによって、Isaac Sim内のオブジェクト位置を変更するAPIを用いて位置を同期している。

　Turtlebot3の/odomにはタイヤの回転角とIMUによるロボットのスタートからの進行距離が格納されているため、それをIsaac Simの位置と同期するという仕組みだ。