sdfファイルをrviz上のRobotDescriptionで表示する（前半）

実行環境

Ubuntu14.04
Gazebo 2.2.3
sdf 1.4
ROS indigo

背景

sdfを使ってモデルを作ったのにrvizのRobotDescriptionがurdfしか対応してないと知ってショックを受けた。
調べてみると、pysdfというROSのパッケージを使ってsdf to urdfの変換ができるようだ。
rvizでモデルを表示する練習（とblog更新）を兼ねてここに記録を残す。

手順

• 準備 pysdfのインストール
• sdfファイルの作成
• pysdfでsdfをurdfに変換
• launchファイルの作成
• 実行

準備

pysdfをインストールする。

ここhttp://wiki.ros.org/pysdf からpysdfをダウンロードしてインストールする。
適当なcatkinのワークスペース下で以下(ダウンロードして移動してcatkin_make)を実行する。

$ git clone https://github.com/andreasBihlmaier/pysdf.git
$ cd ~/catkin_ws
$ catkin_make

もしcatkin_ws以外の名前をつけている場合は、pysdf/src/pysdf/parser.pyにハードコードされているcatkin_wsを
自分のワークスペースの名前に変更する必要があるので注意が必要。

sdfファイルの作成

基本的なことはGazeboのチュートリアルやsdfのドキュメントを見ればいい。
以下のようなmodel.sdfを作成した。リンク2つがジョイントでつながっているだけ。

<?xml version='1.0'?>
<sdf version="1.4">

  <model name="my_model">
    <pose>0 0 0.5 0 0 0</pose>
    <link name="base_link">
      <inertial>
        <mass>1.0</mass>
        <inertia> <!-- interias are tricky to compute -->
          <ixx>0.083</ixx>       <!-- for a box: ixx = 0.083 * mass * (y*y + z*z) -->
          <ixy>0.0</ixy>         <!-- for a box: ixy = 0 -->
          <ixz>0.0</ixz>         <!-- for a box: ixz = 0 -->
          <iyy>0.083</iyy>       <!-- for a box: iyy = 0.083 * mass * (x*x + z*z) -->
          <iyz>0.0</iyz>         <!-- for a box: iyz = 0 -->
          <izz>0.083</izz>       <!-- for a box: izz = 0.083 * mass * (x*x + y*y) -->
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
      </visual>
    </link>

    <link name="child_link">
      <pose>0 0 2.5 0 0 0</pose>
      <inertial>
        <mass>1.0</mass>
        <inertia> <!-- interias are tricky to compute -->
          <ixx>0.083</ixx>       <!-- for a box: ixx = 0.083 * mass * (y*y + z*z) -->
          <ixy>0.0</ixy>         <!-- for a box: ixy = 0 -->
          <ixz>0.0</ixz>         <!-- for a box: ixz = 0 -->
          <iyy>0.083</iyy>       <!-- for a box: iyy = 0.083 * mass * (x*x + z*z) -->
          <iyz>0.0</iyz>         <!-- for a box: iyz = 0 -->
          <izz>0.083</izz>       <!-- for a box: izz = 0.083 * mass * (x*x + y*y) -->
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
      </visual>
    </link>
    
    <joint name="joint" type="revolute">
      <parent>base_link</parent>
      <child>child_link</child>
      <axis>
        <xyz>0 0 0</xyz>
        <limit>
          <upper>0</upper>
          <lower>0</lower>
        </limit>
      </axis>
    </joint>

  </model>
</sdf>

pysdfでsdfをurdfに変換

先ほどインストールしたpysdfを使って、sdfファイルをurdfファイルに変換する。
pysdf/script/pysdf.pyを実行して、入力と出力を指定するだけでいい。

$ python pysdf.py model.sdf model.urdf

うまく変換できているかはcheck_urdfで確認できる。

$ check_urdf model.urdf

launchファイルの作成

作ったurdfファイルをgazeboの世界にspawnするためのlaunchファイルを作成する。
この実験のためにgazebo_urdf_to_sdfというパッケージを作った。

robot_descriptionという変数に、textfileでmodel.urdfを指定している。
xacroを使ってモデルを記述した場合は、xacro.pyでurdfに変換するようだ。

joint_state_publisherやrobot_sate_publisherでjointのlinkのtfをpublishしている。

<launch>
  <include file="$(find gazebo_ros)/launch/empty_world.launch"></include>

  <node pkg="gazebo_ros" 
	type="spawn_model" 
	name="spawn_model" 
	respawn="false" 
	args="-file $(find gazebo_sdf_to_urdf)/urdf/model.urdf -urdf -model object1 -z 1" />

  <param name="robot_description" 
	 textfile="$(find gazebo_sdf_to_urdf)/urdf/model.urdf" />

  <param name="use_gui" 
	 value="true"/>
  
  <node name="joint_state_publisher"
	pkg="joint_state_publisher" 
	type="joint_state_publisher" />

  <node name="robot_state_publisher" 
	pkg="robot_state_publisher" 
	type="state_publisher" />  

  <node name="rviz" 
	pkg="rviz" 
	type="rviz"/>  

</launch>

実行

後は実行して、rvizで結果を確認するだけ。

$ roslaunch gazebo_sdf_to_urdf spawn_urdf.launch

gazebo上の箱2つが、rvizで表示されていることを確認。

次回

• colladaファイル(daeファイル)のインポートがうまく行くのか試す
• sdfで変数を使えるような仕組み（変数をパースするスクリプトとか）を考える