クワマイでもできる

クワマイでもわかる

Rvizで同一形状のrobot modelを複数台表示する

ROS

f:id:kuwamai:20210731215619p:plain

マニピュレータをマスタースレーブ制御する際、Rvizでマスターとスレーブの2つのrobot modelを重ね合わせて表示したい。同一モデルを使用すると名前空間が衝突してしまうため、tf_prefixを用いて両方表示する方法を紹介する。

使用した環境

  • Ubuntu 18.04
  • ROS Melodic Morenia
    • 現在(2021年7月31日)ROS Noetic Ninjemysではtf_prefixが廃止されていて使えない(後述)

動いている様子

youtu.be

半透明なモデルがマスターアーム、不透明なモデルがスレーブアームの姿勢を表示している。マスターアームにグリッパの情報がないためグリッパのモデルが表示されていないが、今回はグリッパ姿勢を表示する必要がないのでこのままにしとく。

Robot modelが表示されるまでの流れ

tf_prefixの使い方を話す前にrobot modelがRvizに表示されるまでの流れを説明する。Robot modelを表示する際はxacro、joint_state_publisher、robot_state_publisherを大体用いてる。例えば今回使用したマニピュレータ(Gen3 lite)の例↓

xacroのやること

xacroはURDFという形式でロボットの3Dモデルや関節配置、関節稼働範囲などロボットの情報が記述されたファイルを読み込むやつ。詳しくは↓

joint_state_publisherのやっていること

joint_state_publisherは関節位置(回転関節の場合関節角)を配信しているtopicの*/joint_statesをまとめてjoint_statesとして配信するやつ。*は大体ロボット名が含まれるので、joint_state_publisherのおかげでどんな名前のロボットでも気にせずjoint_statesを見ればロボットの関節位置がわかるようになる。

今回joint_state_publisherを使うとマスターもスレーブも関節の名前が同じなので関節位置の名前空間が衝突してしまう。これをなんとかしたい。

名前空間が衝突するイメージについては以前書いたこの記事が参考になるかも。

robot_state_publisherのやること

robot_state_publisherはrobot_descriptionとjoint_statesを受け取って各リンクの位置姿勢を計算してtfとして配信する。tfもjoint_statesと同様にマスターとスレーブでリンクの名前が被る。これもなんとかしたい。

tf_prefixの設定方法

書いたlaunchファイルがこんな感じ。

<launch>
    <!-- arguments -->
    <arg name="model" default="$(find kortex_description)/robots/gen3_lite_gen3_lite_2f.xacro"/>

    <!-- prameters -->
    <param name="robot_description" command="$(find xacro)/xacro $(arg model)"/>

    <group ns = "master_arm">
      <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
        <param name="tf_prefix" value="master" />
        <remap from="joint_states" to="/master_arm/joint_states" />
      </node>
    </group>

    <group ns = "slave_arm">
      <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
        <param name="tf_prefix" value="slave" />
        <remap from="joint_states" to="/my_gen3_lite/joint_states" />
      </node>
    </group>

    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find master_arm_description)/config/urdf.rviz" required="true" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="base_links_tf" args="0 0 0 0 0 0 master/base_link slave/base_link" />
</launch>

マスターの関節角は/master_arm/joint_states、スレーブは/my_gen3_lite/joint_statesというtopic名で配信されている。remapを使うことで一つ目のrobot_state_publisherには/master_arm/joint_statesがjoint_statesというtopic名に見えてるし、二つ目は/my_gen3_lite/joint_statesがjoint_statesというtopic名に見えている。これで2つのロボットのjoint_statesを混ぜずにrobot_state_publisherに届けられる。

tf_prefixは計算したリンク位置姿勢のtfの前にそれぞれmasterとslaveという名前が着くように設定した。これでマスターとスレーブで同じリンク名でも、マスターのtfは最初にmasterとつくので名前でマスターとスレーブの見分けがつくようになった。

ちなみに最後のstatic_transform_publisherはマスターとスレーブの土台のリンクであるbase_linkの位置関係を設定している。0 0 0 0 0 0と設定したので重なって表示されているし、1 0 0 0 0 0と設定すればx軸方向に1ずらしてRvizに表示することができる。

Rvizの設定

RobotModelを2つ追加し、Robot Descriptionは2つとも同じrobot_descriptionを設定。TF Prefixはそれぞれmasterとslaveを設定することでマスターとスレーブそれぞれのtfを読み込んで表示するようになる。

f:id:kuwamai:20210731215619p:plain

tf_prefixの現状(解決済み)

というわけで今回みたいに同一形状のrobot modelを表示する際tf_prefixは便利なのだけど、ROS Noetic Ninjemys版のrobot_state_publisherでは廃止されてしまった。

そして現在復活させようと絶賛議論中だったりする。

かれこれ1年以上続いてるのだけどどうなるかな。

ROS Noetic Ninjemysでも使えるようになった

2021/12/15追記
めでたくバージョン1.15.2から使用できるようになったそうです。Tiryoh (id:tiryoh) さん教えていただきありがとうございます。

参考