ROS2でURDFを記述してRviz2でロボットを表示してみた

　これまで私はROSをロボットあまり関係ない分野で使ってきましたが、やはりROSユーザを名乗るからにはロボットらしいプログラム(?)をしたいところです。そこで、Rvizで表示したりtfを扱うためにロボットをURDFで記述しました。ROS1で図形をURDFで記述したことはありましたが、ロボットの部品位置を確認しながらURDFを作成するのは初めてだったので結構苦戦しました。また、現時点でのURDFはシミュレーション用には作っていないので、タイヤの軸などは作っていません。

　urdfやlaunchはGitHubにアップしています。このリポジトリは継続的にアップデートしていく予定です。

github.com

メカナムロボットにおけるURDF記述

　URDFは、ロボットの位置関係を定義するファイルです。URDFは複雑になりがちなロボットの関節などの記述を簡潔にすることができ、tf2と連携することで座標変換の手間を省くことができます。現在のロボットはRealsense T265を使用してオドメトリを取得しています。しかし、T265がロボットの中心かと言われるとそうではありません。当然、ロボットの中心は4つのメカナムホイールの中心です。今のところはそれほど複雑なロボットではないので直接座標を計算するプログラムを書いてもいいかもしれません。しかし、ほかのロボットにT265を取り付ける場合や他の可動関節を取り付ける場合などのアップグレードがある場合はいちいちメンテナンスするのが面倒になります。

　今回は、ROS2でのURDF記述や今後のtf2練習のためにURDFを記述してRviz2に表示したいと思います。

ロボットのリンク関係

　ロボットにリンクを示すマーカーをつけて分かりやすくしてみました。tfはループが発生してはいけない（=親子孫の関係）ので、Rvizフレームがない場合はT265が基準となります。そして、メカナムロボットの中心となるフレーム（base_link）はT265(odom)の子となります。そして、base_linkから各ホイールとLRFにつなぎます。

f:id:Ray_ar:20210328110826p:plain

　これをURDFとして記述します。次に、GitHubから抜粋したものを示します。なお、衝突判定などの記述は省略します。

リンク作成

1．odom(=T265)の記述

　T265の模型を作成します。長方形を作成しているだけです。

<link name="odom">
    <inertial>
        <mass value="1"/>
        <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
    </inertial>
    <visual>
        <origin xyz="0 0 0" />
        <geometry>
            <box size=".015 .105 .02" />
        </geometry>
        <material name="black">
            <color rgba="0 0 0 1"/>
        </material>
    </visual>
</link>

2．base_link(メカナムロボットの本体)の記述

　メカナムロボットの中心とそのビジュアルを作成します。

<link name="base_link">
    <inertial>
        <mass value="1"/>
        <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
    </inertial>

    <visual>
        <origin xyz="0 0 .0575" />
        <geometry>
            <box size=".21 .15 .12" />
        </geometry>
        <material name="white">
            <color rgba="1 1 1 1"/>
        </material>
    </visual>
</link>

3．wheel(メカナムホイール)とlidar(LRF)の記述

　メカナムホイールとLRFの記述をします。ここではメカナムホイールの1輪のみ示します。メカナムホイールは円柱を使用して表現しました。円柱は高さと半径を指定します。

<link name="wheel0">
    <visual>
        <geometry>
            <cylinder length="0.03" radius="0.04"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.57 0 0"/>
        <material name="blue">
            <color rgba="0 0 1 1"/>
        </material>
    </visual>
</link>

ジョイント作成

1．odom→base_linkの作成

　odom→base_link間はx軸方向に-12cm、z軸方向に-5.5cmずれていました。parentはodom、childはbase_linkです。

<joint name="base_linkconnect" type="fixed">
    <origin xyz="-.12 0 -.055" />
    <parent link="odom"/>
    <child link="base_link"/>
</joint>

2．base_link→wheel・lidarの作成

　parentがbase_link、childが各リンクの関係になるように記述します。

<joint name="wheel0_connect" type="fixed">
    <origin xyz=".05 .105 .04" />
    <parent link="base_link"/>
    <child link="wheel0"/>
</joint>

URDF確認

　robot_state_publisherの引数にURDFを指定することで反映することができます。launch.pyでは次のように設定します。このプログラムから抜粋しています。

#抜粋
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
urdf_file_name = 'ar_ray_robot.urdf.xml'

robot_state_pub = Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=[urdf]
        )

　また、Realsense T265の設定は次のようにします。

#抜粋
t265_base_frame_id = LaunchConfiguration('base_frame_id', default='odom')
t265_serial_no = LaunchConfiguration('serial_no', default='925122110087')
    
t265_node = Node(
    package='realsense_node',
    node_executable='realsense_node',
    node_namespace="/t265",
    output='screen',
    remappings=[('/t265/camera/odom/sample','/odom')],
    parameters=[{'serial_no':t265_serial_no ,'base_frame_id': t265_base_frame_id}]
    )

　Rvizも起動します。

#抜粋
config_file_name = 'ar_ray_robot.rviz'

rviz_config = os.path.join(
    get_package_share_directory('ar-ray-robot'),
    config_file_name)
#    
# ・・・
# returnの中
Node(
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    output='screen',
    arguments=['-d', rviz_config]),

　実行結果です。

f:id:Ray_ar:20210326132140p:plain — 軸を消したあと

　リンクの関係は発行されたtf_treeで確認できます。画像は、出力されたtf_treeに先ほどのリンクマーカーを当てはめた結果です。ちゃんと親子関係が守られています。ロボットを動かして移動していることを確認する場合は、Rviz2のfixed_frameをodom_frameに変更してください。

f:id:Ray_ar:20210326132449p:plain