ROS講座26 urdfモデルをGazeboに出現させる

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
| Gazebo | 11.9.0 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。


# 概要
ここまでSDFファイルでモデルを記述してそれをgazeboで使っていましたが、Rvizの表示のために別途urdfを記述する必要があります。
ファイルを1つで済ますためにurdfにもシミュレーションに必要な情報を足して、urdfモデルからシミュレーションを起動する方法があります。

## コメント
この方法でシミュレーションを起動することのデメリットとしてシミュレーション起動までのファイルの変換が多く、エラーが起きた時にどこで起きかが分かりにくいです。

* xacto->urdf: xacro展開
* urdf->sdf: spawn_modelで変換
* sdf: Gazebo起動

# シンプルな箱モデルの出現

## ソースコード

いつものurdfにシミュレーション用のタグを追加します。
```xml:gazebo_urdf_lecture/urdf/simple_body.urdf
<?xml version="1.0"?>
<robot name="my_robo" xmlns:xacro="http://ros.org/wiki/xacro">
  <link name="base_link"/>

  <joint name="body_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="body_link"/>
  </joint>
  <link name="body_link">
   <inertial>
     <origin xyz="0 0 0.5" rpy="0 0 0"/>
     <mass value="1"/>
     <inertia ixx="0.1"  ixy="0"  ixz="0" iyy="0.1" iyz="0" izz="0.1" />
   </inertial>
   <visual>
     <origin xyz="0 0 0" rpy="0 0 0" />
     <geometry>
       <box size="1 1 1" />
     </geometry>
     <material name="Cyan">
       <color rgba="0 1.0 1.0 1.0"/>
     </material>
   </visual>
   <collision>
     <origin xyz="0 0 0" rpy="0 0 0"/>
     <geometry>
       <box size="1 1 1" />
     </geometry>
   </collision>
  </link>
  <gazebo reference="body_link">
    <material>Gazebo/Red</material>
  </gazebo>
</robot>
```

* `<visual>`タグはGUIでの表示に使われる値です。Rvizで表示するのと同様です。
* `<collision>`タグは衝突検出に使われる値です。例えば地面の上に箱があるのは地面と箱の表面で衝突があるからです。これがないと無限に下に落ちてしまいます。
* `<inertial>`タグを追加します。
    * `<origin xyz="0 0 0.5" rpy="0 0 0"/>`は重心の位置です。
    * `<mass value="1"/>`は重量です。
    * `<inertia ixx="0.1"  ixy="0"  ixz="0" iyy="0.1" iyz="0" izz="0.1" />`は慣性モーメントです。
    * 一番根元のリンクはこの設定をしない方が良いです（urdfの仕様？）

urdfモデルを出現させるlaunchファイルを記述します。

```xml:gazebo_urdf_lecture/launch/simple_body.launch
<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find gazebo_urdf_lecture)/urdf/simple_body.urdf"/>   
  <param name="robot_description" command="$(find xacro)/xacro $(arg model)"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch" />
     
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -model my_robo -param robot_description"/>
</launch>
```

* `<param name="robot_description" command="$(find xacro)/xacro $(arg model)"/>`でxacroファイルからurdfファイルに展開します。
* `<include file="$(find gazebo_ros)/launch/empty_world.launch" />`でまず何もない空のworldでGazeboを起動します。
* 今回の注目は`urdf_spawner`です。これでurdfモデルからgazeboにモデルを出現させます。

## 実行

```shell
roslaunch gazebo_urdf_lecture simple_body.launch 
```

実行すると以下のような画面が現れます。
画像で赤く示してるように矢印ボタンを押すと座標軸が出現します。これをマウスでドラッグすると図形を動かすことができます。物理シミュレーターで重力が効いているので上にあげるて離すと落ちます。
また下側の「Real Time Factor」はシミュレーションの時間が実時間の何倍の速度で動いているかということを示しています。1.0なら等速であり、それ以下ならシミュレーションが処理落ちしていることを示します。

![sim1.png](https://qiita-image-store.s3.amazonaws.com/0/254442/5b530f28-e10e-1d1f-ff7c-ed66b2c2f27d.png)

![sim_gazebo1.gif](https://qiita-image-store.s3.amazonaws.com/0/254442/07bf3ccf-eda4-84d7-7022-c7e4b6f6a17d.gif)

# プラグインの使い方
planarプラグインで動く全方向移動台車型のロボットを出現させます。

## ソースコード
```xml:gazebo_urdf_lecture/urdf/simple_body_move.urdfの一部
<gazebo>
  <plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
    <commandTopic>cmd_vel</commandTopic>
    <odometryTopic>odom</odometryTopic>
    <odometryFrame>odom</odometryFrame>
    <odometryRate>20.0</odometryRate>
    <robotBaseFrame>base_link</robotBaseFrame>
    <cmdTimeout>2.0</cmdTimeout>
  </plugin>
</gazebo>
```

`<gazebo>`タグの内部ではシミュレーションに必要な記述が出来ます。sdfファイルで書くのと同様の記述を書きます。

## 実行
```shell
roscd gazebo_urdf_lecture/urdf/
roslaunch gazebo_urdf_lecture simple_body.launch model:=simple_body_move.urdf
```

![gazebo1_urdf_move.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/51a6a388-13aa-538c-3cba-44eb42c4f013.gif)

```shell: /cmd_velのpub
rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.5" 
```

## コメント
`libgazebo_ros_planar_move`は物理演算を無視して強制的に物体を指定の速度で動かす作用があります。
摩擦などの設定によっては平行方向が思った通りに動かなかったり、重力が無視されるといったデメリットもあります。

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)