Gazebo/MoveIt のための 3D モデリング（１３）MoveIt の静モデルの作成
本シリーズ前回の記事 Gazebo/MoveIt のための 3D モデリング（１２）Gazebo の静モデルの作成 ではエクスポートしたメッシュデータファイルを Gazebo の静モデルとしてモデルファイルに組み込んで表示する方法を紹介しました．
今回はエクスポートしたメッシュデータファイルを MoveIt の静モデルとしてモデルファイルに組み込んで表示する方法を紹介します．
今回紹介するのは次の2通りの方法です．メッシュを MoveIt GUI で読み込んで障害物とする方法とロボットモデル作成につながる方法の URDF モデルの作成とそれを確認表示する方法です．
- MoveIt 空間内の動作計画に対する障害物として STL ファイルまたは DAE ファイルを読み込んで設置
- URDF モデルの作成（→ ロボットリンクモデル作成へつながる）
MoveIt 内の障害物モデルとしてのメッシュ読込み
MoveIt には動作計画における障害物として STL ファイルや DAE ファイルをそのまま読み込んで MoveIt の動作計画空間内に配置する機能があります．
MoveIt の Motion Planning パネル内の Scene Objects タブを開いて， “Mesh from file” を選択します．
“Mesh from file” セレクタの右隣にあるプラスボタン [ + ] を押します．
（左図拡大は画像をクリック）
保存してある STL もしくは DAE ファイルを選択します．
ファイルを読み込むときに MoveIt の GUI インタフェースである RViz のメッセージウィンドウが開いて，ミリメートル単位で記述されているモデルをメートル単位に変換する旨の問いがなされるので [ Yes ] をクリックします．
読み込んだモデルをインタラクティブマーカや座標などを指定して意図した位置に設置し，左のチェックボックスをクリックすると設置リンクの選択を促されますので適宜選択して [ OK ] ボタンを押します．
次に [ Publish ] ボタンを押すと読み込んで設置したモデルが MoveIt 空間内で障害物として認識されます．
あとは [ Plan ] や [ Plan & Execute ] などで動作計画を実行するとその経路上に障害物があるとそれを避けたマニピュレーションの軌道が生成されます．
MoveIt モデル URDF ファイルの作成
MoveIt モデルの URDF ファイルの作成は下記リンク先の ROS Wiki に書かれています．
本記事ではそれらから MoveIt 静モデル作成に絞って説明します．
MoveIt モデルの作成にあたっては ROS パッケージを作成してその中にモデルの URDF ファイルを置くのが本記事の内容に続く応用も含めると一番簡便なのではないかと思います．
今回 ROS パッケージをつくるのが面倒なようでしたら下記リンク先リポジトリをクローンして利用してください．
3dmodeling-examples/
├── CMakeLists.txt
├── images
│ ├── front_view_win.png
│ ├── left_view_win.png
│ ├── top_view_win.png
│ ├── washing-machine_catalogue.pdf
│ └── washing-machine_catalogue.png
├── launch
│ ├── spawn-washingmachine.launch
│ └── world-washingmachine.launch
├── LICENSE
├── models
│ ├── gazebo_models
│ │ ├── washing-machine
│ │ │ ├── meshes
│ │ │ │ ├── base_link_blue-gray.stl
│ │ │ │ ├── base_link_dark-gray.stl
│ │ │ │ ├── base_link_gray-white.stl
│ │ │ │ ├── base_link_light-gray.stl
│ │ │ │ └── base_link.stl
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ └── washing-machine-dae
│ │ ├── meshes
│ │ │ ├── base_link.dae
│ │ │ └── base_link.stl
│ │ ├── model.config
│ │ └── model.sdf
│ ├── urdf
│ │ ├── meshes
│ │ │ └── washing-machine
│ │ │ ├── base_link.dae
│ │ │ └── base_link.stl
│ │ └── washing-machine.urdf
│ └── washing-machine.3dm
├── package.xml
├── README.md
└── worlds
└── washing-machine.world
本記事執筆時のサンプルモデルパッケージは右に示すような構成になっていますので参考にしてみてください．
この中の MoveIt URDF モデルに関連するフォルダ・ファイルがハイライトされた部分です．
meshes フォルダに base_link.dae と base_link.stl の２つのファイルが含まれていますが，これはサンプルのためですのでどちらか１つのファイルだけでも URDF モデルは作成できます．
今回の MoveIt 静モデルのサンプル URDF ファイル washing-machine.urdf の中身は次のようになっています．
<?xml version="1.0" ?>
<robot name="washing-machine">
<link name="base_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://3dmodeling-examples/models/urdf/meshes/washing-machine/base_link.stl" scale="0.001 0.001 0.001" />
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://3dmodeling-examples/models/urdf/meshes/washing-machine/base_link.dae" />
</geometry>
</visual>
</link>
</robot>
URDF のデータ形式は XML で，モデル構成要素の link や collision， geometry， mesh が記述されています．今回は静モデルですので link 要素は１つですがロボットのように関節が多いとリンク数にともなって link 要素およびそこに含まれる子要素も増えます．
ROS パッケージ化していることで ROS のファイルシステムが名前空間から package://3dmodeling-examples/... によって 3dmodeling-examples パッケージのディレクトリを自動解決できるようになっています．
collision メッシュの STL データは単位情報を持っていないので scale="0.001 0.001 0.001" で 0.001倍（=1/1000） 変換をして [mm] のデータを [m] に換算しています．
- 注） 表示上コードの右の方に隠れていると思うので横スクロールで確認してください．
visual のメッシュは DAE ファイルを利用していて，DAE モデルは単位情報を持っていて読み込む側でスケール判断をするので scale は必要ありません．
URDF モデルファイルの RViz 表示
URDF モデルの確認には urdf_tutorial パッケージの display.launch を利用します．
urdf_tutorial は joint-state-publisher-gui パッケージをインストールすることで利用できるようになります．下記は ROS Melodic の場合のインストールコマンドですので他の ROS バージョンの場合は melodic の部分を noetic などに書き換えて実行してください．
$ sudo apt update $ sudo apt install ros-melodic-joint-state-publisher-gui
ターミナルで ROS 環境の設定と urdf_tutorial の display.launch の起動を行います．
$ source ~/$PathToYourWorkspace/devel/setup.bash $ roslaunch urdf_tutorial display.launch model:='$(find 3dmodeling-examples)/models/urdf/washing-machine.urdf'
- 注）
$PathToYourWorkspaceは各自の ROS ワークスペースへのパスを記述- Launch オプションの
model:=には URDF ファイルパスを指定 - 上記は 3dmodeling-examples パッケージ内のフォルダに washing-machine.urdf がある場合の例
正常に実行できると次の図のように洗濯機モデルが RViz 空間上に表示されます．
問題はありませんが気持ちの収まりが悪いようでしたらターミナルを追加で立ち上げて下記コマンドで例えば /world フレームから洗濯機モデルの /base_link フレームへの tf をパブリッシュすると警告が消えます．
$ rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 /world /base_link
今回の記事はここまでです．
本シリーズ次回の記事は洗濯機の URDF モデルにドアのヒンジなどの動く箇所を設定してより機械らしい（ロボットに近い）モデルにする様子を紹介する予定です．