ROS2でxacro -> urdf -> robot_state_publisherの流れをやる方法

ROS2でxacroを展開してurdfを生成、robot_state_publisherでtfを出力する流れに関するドキュメントを探したのですが、なかなか参考になる情報が出てこなかったので備忘録を残しておきます。

xacro,urdf,robot_state_publisherとは？

まずURDFは、ROSにおけるロボットの幾何構造を記述するフォーマットです。
URDFの使い方、記述法はこちら等にまとまっています。
ROS講座13 URDFを記述する1
ロボットをURDFで記述する
移動型ロボットのURDF作成

xacroはURDFのために作成されたXMLのマクロです。
左右対称に配置されているセンサの間隔をいじったりするのにURDFだと何箇所も値を書き換えないと行けないけど、xacroを使うと記述量を減らすことができます。
便利ですね。
使い方はこちらのようになっています。
ROS講座73 xacroを使う1
ROS講座73 xacroを使う2

robot_state_publisherはURDFフォーマットの文字列をロードして、joint_statesトピックをサブスクライブすることでロボットの座標をtfの形式で出力するパッケージです。
ROS1ではこのやり方は確立されており、xacroパッケージを使ってrobot_descriptionというrosparamにURDFの文字列を設定、robot_state_publisherはそのrosparamを読んでtfを出力とうい流れになっていました。
ROS2でこれと同じことできるよねと思って色々試したらハマった上にrobot_state_publisherの中にあるlaunchを真似してもうまく行かなかったので色々試してみてこれならできたよって方法があったので備忘録を残したいと思います。

ロードが成功したlaunchはこちらになります。
どうやっているかというと以下のようになります。

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
import launch
import launch_ros.actions
import xacro
from launch_ros.substitutions import FindPackageShare

share_dir_path = os.path.join(get_package_share_directory('wamv_description'))
xacro_path = os.path.join(share_dir_path, 'urdf', 'wamv_base.urdf.xacro')
urdf_path = os.path.join(share_dir_path, 'urdf', 'wamv.urdf')

def generate_launch_description():
    # xacroをロード
    doc = xacro.process_file(xacro_path)
    # xacroを展開してURDFを生成
    robot_desc = doc.toprettyxml(indent='  ')
    # urdf_pathに対してurdfを書き出し
    f = open(urdf_path, 'w')
    f.write(robot_desc)
    f.close()
    rsp = launch_ros.actions.Node(package='robot_state_publisher',
                                  node_executable='robot_state_publisher',
                                  output='both',
                                  # argumentsでURDFを出力したパスを指定
                                  arguments=[urdf_path])

    return launch.LaunchDescription([rsp])

robot_state_publisherの中にあるlaunchから変更したのはxacroをparamterで引き渡すのではなくファイルに一度書き出してからロードしているところです。
ROS2のlaunchをPythonで記述した場合に実行バイナリ・スクリプトにargumentを引き渡したい場合はarguments=[]で、rosparamを引き渡したい場合はparameters=[]で渡すというような感じで切り替えます。
今回はargumentでurdfのパスを渡せばうまく行くことがros2 runコマンドでrobot_state_publisherを動かした際にわかっていたのでこの方法で記述を試みたところうまく行きました。

最後に注意なのですが、ROS2からrobot_descriptionはparamではなくtopicとなっています。
上記のlaunchを実行してrviz2を立ち上げ、DescriptionSourceをTopic,DescriptionTopicに/robot_descriptionを指定すると以下のような画面が出てきます。