gazeboを使いこなす！

gazebo上で自作のロボットモデルを走行させる．

* [gazeboとは](#gazeboとは)
* [gazeboに表示するモデルの作成](#gazeboに表示するモデルの作成)
* [gazebo上で駆動制御を行う](#gazebo上で駆動制御を行う)
* [状態をrvizで表示する](#状態をrvizで表示する)

# gazeboとは
Gazeboでは，主にSDF（Simulation Description Format）というフォーマットを採用しており，それを用いてでモデルやシミュレーション環境を定義している．これは，URDFの「閉リンク機構に対応していない」という問題を解決するためだが，SDFもSDFで課題がある．

特に，
	• URDFでも，<gazebo>タグを使えばURDFと同じ内容（必要な内容）を記述できること
	• URDFのほうが，SDFより書きやすく，xacroという使いやすいツールもあること
という理由から，URDFで記述する人が多いらしい．

 そこで，今回はxacroファイルを用いたURDFによって記述していく．（複雑なURDFを書くときにおすすめツール：[VScode拡張機能](https://kuwamai.hatenablog.com/entry/2019/10/06/193715)）


# gazeboに表示するモデルの作成
gazeboモデルとrvizモデルの最も大きな違いは，物理情報の有無である．
rvizでは見た目の形と色のみ設定していたが，物理シミュレーションを行うにあたって以下の3つの情報が追加で必要になる．
* collision：衝突判定に用いる物体の形状を設定
* inertial：衝突時の状態計算に用いる慣性モーメントを設定
* gazebo reference：（☝２つとは異なり）gazebo内で表示するモデルの色を設定．シミュレーション環境の重力設定も行える．
<table>
    <thead>
        <tr>
            <th> </th>
            <th>rviz</th>
            <th>gazebo</th>
        </tr>
    </thead>
    <tr>
        <td>joint</td>
        <td>origin，parent_link，child_link</td>
        <td>origin，parent_link，child_link</td>
    </tr>
    <tr>
        <td>link</td>
        <td>visual</td>
        <td>visual + <b>collision，inertial</b></td>
    </tr>
    <tr>
        <td>その他</td>
        <td> - </td>
        <td><b>gazebo reference</b></td>
    </tr>
</table>

以下，手順
[1-1. パッケージの新規作成](#1-1-パッケージの新規作成)
[1-2. 各部品のxacroファイルを入れるフォルダを作成](#1-2-各部品のxacroファイルを入れるフォルダを作成)
[1-3. 作成したurdfをrvizで確認](#1-3-作成したurdfをrvizで確認)
[1-4. gazeboで表示する](#1-4-gazeboで表示する)

## 1. パッケージの新規作成
xacroでロボットを定義するパッケージを作る．慣習的に「*_description」という名前にする．
```
$ cd ~/practice_ws/src/
$ mkdir robot_lecture
$ catkin_create_pkg robot_description std_msgs rospy roscpp tf sensor_msgs urdf xacro joint_state_publisher robot_state_publisher
```

## 2. 各部品のxacroファイルを入れるフォルダを作成
今回作るロボットは，以下3部品+α で構成される対向2輪ロボットとする．
* [chassis](#chassis)：ロボットの車体．直方体の部品からなる．
* [wheel](#wheel)：車輪．薄い円柱部品からなる．これを駆動させることで移動できる．
* [caster](#caster)：非駆動輪．小さな球体部品からなり，駆動輪の動きに応じて自然に動く．
* [common](#common)：色情報や形に応じた慣性モーメントの計算式などのすべての部品で使う共通コードを記載．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/96c6b118-bcfd-feab-f1fa-901039e58b6e.png)

一つのurdfによって長々と書くより，xacroファイルを用いて，ロボットの部品ごとに分割して記述すると分かりやすい．（xacroについては[こちら](https://qiita.com/srs/items/43528d00ee789171367f)）

```
$ cd robot_description
$ mkdir -p urdf/chassis
$ cd urdf
$ mkdir wheel caster robot common
```

### common
<details><summary>形状ごとの慣性モーメントの計算式や色の定義など，すべての部品で共通の情報についてはここにまとめてある．</summary><div>

    ```xml:common/common.urdf.xacro
    <?xml version="1.0"?>
    <robot xmlns:xacro="http://<ros.org/wiki/xacro">
    	<!-- PROPERTY LIST -->
    	  <!-- Inertial. Units are kg*m^2 -->
    	  <xacro:macro name="box_inertia" params="m x y z">
    	    <inertia ixx="${m*(y*y+z*z)/12}" ixy = "0" ixz = "0"
    	             iyy="${m*(z*z+x*x)/12}" iyz = "0"
    	             izz="${m*(x*x+y*y)/12}" /> 
    	  </xacro:macro>
    	  <xacro:macro name="cylinder_inertia" params="m r h">
    	    <inertia ixx="${m*(3*r*r+h*h)/12}" ixy="0" ixz="0"
    	             iyy="${m*(3*r*r+h*h)/12}" iyz="0"
    	             izz="${m*r*r/2}" />
    	  </xacro:macro>
    	  <xacro:macro name="sphere_inertia" params="m r">
    	    <inertia ixx="${m*r*r/5}" ixy="0" ixz="0"
    	             iyy="${m*r*r/5}" iyz="0"
    	             izz="${m*r*r/5}" /> 
    	  </xacro:macro>
    	
    	  <!-- Color -->
    	  <material name="blue">
    	    <color rgba="0.0 0.0 0.8 1.0"/>
    	  </material>
    	  <material name="green">
    	    <color rgba="0.0 0.8 0.0 1.0"/>
    	  </material>
    	  <material name="red">
    	    <color rgba="0.8 0.0 0.0 1.0"/>
    	  </material>
    	  <material name="white">
    	    <color rgba="1.0 1.0 1.0 1.0"/>
    	  </material>
    	  <material name="yellow">
    	    <color rgba="0.8 0.8 0.0 1.0"/>
    	  </material>
    	
    </robot>
    ```

</div></details>

### chassis
<details><summary>1.0x0.5x0.2 mの黄色い直方体を表示する．</summary><div>

```xml:chassis/chassis.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
//直方体の大きさを事前設定
   <xacro:property name="size" value="1.0 0.5 0.2" /> 
  	
//"chassis"という名前のマクロを作成する．
//ただし，parent linkとjoint位置はマクロ使用時に代入できるようにする
   <xacro:macro name="chassis" params="parent *joint_origin">
	<joint name="chassis_joint" type="fixed">
		<xacro:insert_block name="joint_origin"/>
		<parent link="${parent}"/>
		<child link="chassis_link"/>
	</joint>

	<link name="chassis_link">
		<visual>
			<geometry>
				<box size="${size}"/>
			</geometry>
			<material name="yellow"/>
		</visual>

        //以下，gazeboで使うために追加で必要な情報（collision，inertial，gazebo reference）
		<collision>
			<geometry>
				<box size="${size}"/>
			</geometry>
		</collision>

		<inertial>
			<origin xyz="0 0 0"/>
			<mass value="0.6"/>
			<xacro:box_inertia m="0.6" x="1.0" y="0.5" z="0.2"/>
		</inertial>
	</link>
    //以下のコードがないと，gazeboで色を表示できない
	<gazebo reference="chassis_link">
			<material>Gazebo/Yellow</material>
			<turnGravityOff>false</turnGravityOff>
	</gazebo>
  </xacro:macro>
</robot>
```
</div></details>


### wheel
<details><summary>直径0.1m，長さ0.05mの赤い円柱を表示する．</summary><div>

```xml:wheel/wheel.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">

	  <xacro:macro name="wheel" params="prefix parent *joint_origin *joint_axis">
	    <joint name="${prefix}_wheel_joint" type="continuous">
	      <xacro:insert_block name="joint_origin" />
	      <xacro:insert_block name="joint_axis" />
	      <parent link="${parent}" />
	      <child link="${prefix}_wheel_link" />
	    </joint>
	    
	    <link name="${prefix}_wheel_link">
	      <visual>
	        <geometry>
	          <cylinder radius="0.1" length="0.05"/>
	        </geometry>
	      </visual>
	      
	      <collision>
	        <geometry>
	          <cylinder radius="0.1" length="0.05"/>
	        </geometry>
	      </collision>
	      
	      <inertial>
	        <origin xyz="0.0 0.0 0.0" />
	        <mass value="0.500"/>
	        <xacro:cylinder_inertia m="0.5" r="0.1" h="0.05"/>
	      </inertial>
	    </link>
		<gazebo reference="${prefix}_wheel_link">
			<material>Gazebo/Red</material>
			<turnGravityOff>false</turnGravityOff>
		</gazebo>
	  </xacro:macro>
</robot>
```
</div></details>

    
### caster
<details><summary>直径0.05mの赤い球体を表示する．</summary><div>

```xml:caster/caster.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
	  <xacro:macro name="caster" params="parent *joint_origin">
	    <joint name="$caster_joint" type="fixed">
	      <xacro:insert_block name="joint_origin" />
	      <parent link="${parent}" />
	      <child link="caster_link" />
	    </joint>
	
	    <link name="caster_link">
	      <visual>
	        <geometry>
	          <sphere radius="0.05"/>
	        </geometry>
	      </visual>
	
	      <collision>
	        <geometry>
	          <sphere radius="0.05"/>
	        </geometry>
	      </collision>
	
	      <inertial>
	        <origin xyz="0.0 0 0.0" />
	        <mass value="0.50" />
	        <xacro:sphere_inertia m="0.5" r="0.05"/>
	      </inertial>
	    </link>
		<gazebo reference="caster_link">
			<material>Gazebo/Red</material>
			<turnGravityOff>false</turnGravityOff>
		</gazebo>
	  </xacro:macro>
</robot>
```
</div></details>

### robot
<details><summary>各部品についてのxacroファイルを読み込み，それらを組み立てる．</summary><div>

```xml:robot/robot.urdf.xacro
<?xml version="1.0"?>
	<robot name="myrobot" xmlns:xacro="http://ros.org/wiki/xacro">
	  <xacro:include filename="$(find robot_description)/urdf/robot/common.urdf.xacro"/>
	   <xacro:include filename="$(find robot_description)/urdf/chassis/chassis.urdf.xacro"/>
	  <xacro:include filename="$(find robot_description)/urdf/wheel/wheel.urdf.xacro"/>
	  <xacro:include filename="$(find robot_description)/urdf/caster/caster.urdf.xacro"/>

	  <link name="base_footprint"/>
	
	  <xacro:chassis parent="base_footprint">
	    <origin xyz="0 0 0"/>
	  </xacro:chassis>
	
	  <xacro:wheel prefix="right" parent="chassis_link">
	    <origin xyz="0.25 -0.3 0.0" rpy="1.57 0 0" />
	    <axis xyz="0 0 -1" />
	  </xacro:wheel>
	  
	  <xacro:wheel prefix="left" parent="chassis_link">
	    <origin xyz="0.25 0.3 0.0" rpy="-1.57 0 0" />
	    <axis xyz="0 0 1" />
	  </xacro:wheel>
	
	  <xacro:caster parent="base_link">
	    <origin xyz="-0.15 0 -0.1"/>
	  </xacro:caster>
</robot>
```
</div></details>

## 3. 作成したurdfをrvizで確認
作成したurdfのあるディレクトリに移動してから，以下のコマンドを入力．
```	
$ roslaunch urdf_tutorial display.launch model:=robot.urdf.xacro
```

:::note info
エラー表示が出ても，
Global Options>「Fixed frame」をbase_linkからbase_footprintに変更すればOK（下矢印を開くと，選べるはず）
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/34b9fb2d-7cb4-e070-9556-0d4232514017.png)

:::


☟のように表示されればOK．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/73033c3d-6d64-76a7-c0fc-76cfe918e42e.png)

※launchファイルからちゃんと書きたい場合は，[rvizを使いこなす！](https://qiita.com/Karin-Sugi/items/5bfbaf7cca164e04773e#3-launch%E3%83%95%E3%82%A1%E3%82%A4%E3%83%AB%E3%82%92%E8%A8%98%E8%BF%B0%E3%81%97%E3%81%A6rviz%E3%81%A8%E4%B8%80%E7%B7%92%E3%81%AB%E3%81%BB%E3%81%8B%E3%81%AE%E3%83%8E%E3%83%BC%E3%83%89%E3%82%92%E8%B5%B7%E5%8B%95%E3%81%99%E3%82%8B)を参照．

## 4. gazeboで表示する
gazeboを作成したモデルで起動するlaunchファイルを記述する．

```
$ mkdir launch
$ cd launch
$ vim gazebo_display.launch
```

```xml:gazebo_display
<launch>
	<!-- Gazeboを起動する -->
	  <include file="$(find gazebo_ros)/launch/empty_world.launch"/>

	<!-- xacroファイルをurdfに展開して，パラメータに格納 -->
	<param name="robot_description" command=$"(find xacro)/xacro $(find robot_description)/urdf/robot/robot.urdf.xacro"/>

	<!-- "robot_description"パラメータに格納されたurdfをもとに，gazebo上にモデルを作成するプログラムを起動 -->
	<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
	    args="-param robot_description -urdf -model myrobot"/>
</launch>
```

```
$ roslaunch robot_description rviz_display.launch
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/3942e4c6-a755-3e69-b7c7-ac4fae244281.png)
☝このように表示されればOK！

# gazebo上で駆動制御を行う

https://qiita.com/Karin-Sugi/items/a65ec59bbe9d655b6e7c

# 状態をrvizで表示する
1. [rviz上の車輪を回転させる](rviz上の車輪を回転させる)
2. [rviz上の車両を移動させる](rviz上の車両を移動させる)

## rviz上の車輪を回転させる
joint位置をgazeboから出力させるために，以下のコードをlaunchファイルに追記する．
```xml:launch/gazebo_display.launch
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
```

gazeboを起動したうえで，もう一度rvizを表示してみる
```
$ roslaunch robot_description gazebo_display.launch
$ rviz
```
	
さらに移動させてみる
```
$ roslaunch robot_description gazebo_display.launch
$ rviz
$ rostopic pub -l /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0}, angular: {z: 2.0}}'
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/a4c095b3-0e2d-84db-936c-269d8f6858a7.png)
☝rviz側の車輪が動けばOK！

:::note info
rviz上で車輪の動きを確認するとき，
"Axes"や"TF"を追加して車輪の軸を表示させると分かりやすい．
:::



## rviz上の車両を移動させる
車輪の回転に用いている*differential_drive_controller*に車両の位置情報をpublishさせればよい．
```xml:robot.urdf.xacro
<gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
        <!-- for running robot in gazebo -->
        <leftJoint>left_wheel_joint</leftJoint>
        <rightJoint>right_wheel_joint</rightJoint>
        <robotBaseFrame>base_footprint</robotBaseFrame>
        <wheelSeparation>0.5</wheelSeparation>
        <wheelDiameter>0.15</wheelDiameter>
        <publishWheelJointState>true</publishWheelJointState>
    
        <!-- for running robot in rviz -->
        <odometryFrame>world</odometryFrame>
        <publishWheelTF>true</publishWheelTF>
        <publishOdomTF>true</publishOdomTF>
        <odometrySource>world</odometrySource>
        <publishTf>true</publishTf>
    </plugin>
</gazebo>
```

gazeboを起動したうえで，もう一度rvizを表示してみる
```
$ roslaunch robot_description gazebo_display.launch
$ rviz
```
	
さらに移動させてみる
```
$ roslaunch robot_description gazebo_display.launch
$ rviz
$ rostopic pub -l /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0}, angular: {z: 2.0}}'
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/9d20808f-a074-04ce-62fe-99e687bd777b.png)
☝のように，rvizの原点を表す*world frame*から車両が離れていけばOK!

# トラブルシューティング
### Error [Err] [REST.cc:205] Error in REST request

https://github.com/ros-industrial/universal_robot/issues/412

### [gazebo-2] process has died [pid 8983, exit code 255
結構，何度も直面するエラー

https://answers.gazebosim.org/question/4153/gazebo-crashes-immediately-using-roslaunch-after-installing-gazebo-ros-packages/

```
$ killall gzserver
$ killall gzclient
```