ROSの勉強　第30弾：移動ロボットの作成（3）

# #プログラミング ROS< 移動ロボットの作成（3） >

##はじめに
1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第30弾として，「移動ロボットの作成（3）」を扱う．

##環境
#####仮想環境
|||
|:-:|:--|
|ソフト|VMware Workstation 15|
|実装RAM|2 GB|
|OS|Ubuntu 64 ビット|
|isoファイル|ubuntu-mate-20.04.1-desktop-amd64.iso|

#####コンピュータ
|||
|:-:|:--|
|デバイス|MSI|
|プロセッサ|Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz|
|実装RAM|8.00 GB (7.89 GB 使用可能)|
|OS|Windows (Windows 10 Home, バージョン：20H2)|

#####ROS
|||
|:-:|:--|
|Distribution|noetic|
|シミュレーション|gazebo|

##移動ロボットの作成
ROSを使って，ほとんどの新しいロボットを制御する手順は次のようである．

**1. ROSのメッセージインタフェースを決める．
2. ロボットのモータ用ドライバを書く．
3. ロボットの物理構造を書く．
4. Gazeboのシミュレーションで使用できるようにモデルに物理的特性を追加する．
5. tfを介して座標変換データを配信し，rvizでそれを可視化する．
6. センサを追加する．ドライバとシミュレーションのサポートも必要．
7. ナビゲーション等の標準的なアルゴリズムを適用する．**

移動ロボットを例にその流れを確認していく．
今回は，4についてまとめて移動ロボットの作成（3）として扱うこととする．

##手順4:Gazeboでのシミュレーション
例：TortoiseBot

URDFモデルは，ロボットの運動学上の構造と外観を記述するが，
モデルをシミュレーションするのに必要な物理的特性について何も教えてくれない．

GazeboでTortoiseBotをシミュレートするために，モデル内のすべてのリンクに新しいタグを2つ追加する必要がある．

* ＜collision＞ (干渉)　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　
　visualと同じように，このタグはロボットの身体の大きさと形を定義する．このタグで定義された情報は他のオブジェクトとどのように作用するかを計算するために使われる．collisionのgeometry（幾何学情報）タグはvisualのgeometryタグと同じになるはずだが，多くの場合異なる．
  * なぜ？　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　
　　visualだと見た目重視にメッシュを使って複雑な形を用いる場合もあるが，collisionの場合，干渉検出のための処理の効率化のために単純な形状（箱や円柱など）を使用することが多いため．

※collisionタグにはmaterialタグは必要でない

* ＜inertial＞（慣性）　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　
　このタグはリンクの質量と慣性モーメントを定義する．これらは万有引力の法則に従ってリンクを動かすのに必要．

これら2つのタグを<visual/><gometry/>と同じ階層（linkタグの中）に書く．

##Gazeboに読み込む
必要な操作をlaunchファイルにまとめる．ここで最低限必要となるのは次の操作である．

* ロボットのURDFモデルをパラメータサーバにロードする
* （空のワールドで）Gazeboを起動する
* ROSのサービスコールを使用して，Gazeboの中にそのロボットのインスタンスを生成する．
　この際のURDFデータはパラメータサーバから読み込む

#####launchファイル
```xml:tortoisebot.launch
<launch>
	<!--TortoiseBotのURDFモデルをパラメータサーバにロードする-->
	<param name = "robot_description" textfile = "$(find make_robot1)/urdf/tortoisebot.urdf"/>

	<!--空のワールドでGazeboを開始する-->
	<include file = "$(find gazebo_ros)/launch/empty_world.launch"/>

	<!--GazeboでTortoiseBotを生成し，パラメータサーバからのその記述を受ける-->
	<node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model tortoisebot"/>
</launch>
```

**パラメータサーバは慣例として``/robot_description``という名前で保存する．（ほかの名前を使うこともできるが，結果として多くのツールのデフォルトの設定を変更しなければならなくなる）**

※以下では，これを用いて実装するが，urdfファイルの部分は実行したいものに合わせて変更している．

##実装
以下では，構築したURDFファイルを示す．
#####ソースコード
```xml:tortoisebot1.urdf
<?xml version="1.0"?>
<robot name="tortoisebot">

	<!--台座のモデル-->
	<link name="base_link">
        <!--外観-->
		<visual>
			<geometry>
				<box size="0.6 0.3 0.3"/>
			</geometry>
			<material name="silver">
				<color rgba="0.75 0.75 0.75 1"/>
			</material>
		</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <box size="0.6 0.3 0.3"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="1.0"/>
          <inertia ixx="0.015" iyy="0.0375" izz="0.0375" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	
	<!--キャスターのモデル-->
	<link name="front_caster">
        <!--外観-->
		<visual>
			<geometry>
				<box size="0.1 0.1 0.3"/>
			</geometry>
			<material name="silver"/>
		</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <box size="0.1 0.1 0.3"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="0.00083" iyy="0.00083" izz="0.000167" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	<!--台座とキャスターをつなぐ関節-->
	<joint name="front_caster_joint" type="continuous">
		<axis xyz="0 0 1"/>
		<parent link="base_link"/>
		<child link="front_caster"/>
		<origin rpy="0 0 0" xyz="0.3 0 0"/>
	</joint>

	<!--前輪のモデル-->
	<link name="front_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
      		</geometry>
      		<material name="black"/>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	<!--前輪とキャスターをつなぐ関節-->
	<joint name="front_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="front_caster"/>
    	<child link="front_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="0.05 0 -.15"/>
    </joint>

    <!--右後方車輪のモデル-->
    <link name="right_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
    		</geometry>
    		<material name="black">
    			<color rgba="0 0 0 1"/>
    		</material>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
    </link>
    <!--右後方車輪と台車をつなぐ関節-->
    <joint name="right_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="base_link"/>
    	<child link="right_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="-0.2825 -0.125 -.15"/>
    </joint>

    <!--左後方車輪のモデル-->
    <link name="left_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
    		</geometry>
    		<material name="black"/>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
    </link>
    <!--左後方車輪と台車をつなぐ関節-->
    <joint name="left_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="base_link"/>
    	<child link="left_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="-0.2825 0.125 -.15"/>
    </joint>
</robot>
```
次に，先ほどのlaunchファイルを実行した様子を示す．
#####実行の様子
![tortoisebot1_1](https://user-images.githubusercontent.com/61382602/112591464-bd055e80-8e47-11eb-8b12-04a850a1acaa.PNG)
![tortoisebot1_2](https://user-images.githubusercontent.com/61382602/112591465-bd9df500-8e47-11eb-980f-860a7b2f780b.PNG)
**ワイヤーフレームを指定すると，以下のように構造を確認することができる**
![tortoisebot1_3](https://user-images.githubusercontent.com/61382602/112591458-bbd43180-8e47-11eb-90d4-f854b35e509e.PNG)
**ジョイントを指定すると，以下のように関節の向きを確認することができる**
![tortoisebot1_4](https://user-images.githubusercontent.com/61382602/112591461-bd055e80-8e47-11eb-9f66-96c087f6b05c.PNG)

##差動駆動プラグイン
* 実際のロボットでは，ハードウェアドライバがcmd_vel/odomインタフェースを
　実装することになるが，シミュレーションでも同じようなことをする必要がある．
　これには，Gazeboのプラグインをロードするだけという単純作業で済む．
　特に差動駆動プラグインをロードすれば，cmd_velメッセージを介してTortoiseBotを
　制御することができる．（本来のTortoiseBotとは少し異なる移動手段だが代用）

###実装
まず，プラグインを追加したURDFファイルを示す．
#####ソースコード
```xml:tortoisebot.urdf
<?xml version="1.0"?>
<robot name="tortoisebot">

	<!--台座のモデル-->
	<link name="base_link">
        <!--外観-->
		<visual>
			<geometry>
				<box size="0.6 0.3 0.3"/>
			</geometry>
			<material name="silver">
				<color rgba="0.75 0.75 0.75 1"/>
			</material>
		</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <box size="0.6 0.3 0.3"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="1.0"/>
          <inertia ixx="0.015" iyy="0.0375" izz="0.0375" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	
	<!--キャスターのモデル-->
	<link name="front_caster">
        <!--外観-->
		<visual>
			<geometry>
				<box size="0.1 0.1 0.3"/>
			</geometry>
			<material name="silver"/>
		</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <box size="0.1 0.1 0.3"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="0.00083" iyy="0.00083" izz="0.000167" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	<!--台座とキャスターをつなぐ関節-->
	<joint name="front_caster_joint" type="continuous">
		<axis xyz="0 0 1"/>
		<parent link="base_link"/>
		<child link="front_caster"/>
		<origin rpy="0 0 0" xyz="0.3 0 0"/>
	</joint>

	<!--前輪のモデル-->
	<link name="front_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
      		</geometry>
      		<material name="black"/>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	<!--前輪とキャスターをつなぐ関節-->
	<joint name="front_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="front_caster"/>
    	<child link="front_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="0.05 0 -.15"/>
    </joint>

    <!--右後方車輪のモデル-->
    <link name="right_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
    		</geometry>
    		<material name="black">
    			<color rgba="0 0 0 1"/>
    		</material>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
    </link>
    <!--右後方車輪と台車をつなぐ関節-->
    <joint name="right_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="base_link"/>
    	<child link="right_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="-0.2825 -0.125 -.15"/>
    </joint>

    <!--左後方車輪のモデル-->
    <link name="left_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
    		</geometry>
    		<material name="black"/>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
    </link>
    <!--左後方車輪と台車をつなぐ関節-->
    <joint name="left_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="base_link"/>
    	<child link="left_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="-0.2825 0.125 -.15"/>
    </joint>

<!--差動駆動プラグインをロードするための追加要素-->
  <gazebo>
    <!--差動駆動プラグインを指定-->
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <!--left_wheel_jointとright_wheel_jointをそれぞれ左関節右関節に指定-->
      <leftJoint>left_wheel_joint</leftJoint>
      <rightJoint>right_wheel_joint</rightJoint>
      <!--台座であるbase_linkをrobotBaseFrameに指定-->
      <robotBaseFrame>base_link</robotBaseFrame>
      <!--トレッドを0.25m，車輪直径0.07mとする-->
      <wheelSeparation>0.25</wheelSeparation>
      <wheelDiameter>0.07</wheelDiameter>
      <!--車輪の状態を配信する設定にする-->
      <publishWheelJointState>true</publishWheelJointState>
    </plugin>
  </gazebo>
</robot>
```
これにより，
![cmd_vel](https://user-images.githubusercontent.com/61382602/112594513-6fd7bb80-8e4c-11eb-9617-16bac5efb565.PNG)
``cmd_vel``のメッセージを購読しており，速度指定してあげることで動かせそうだとわかる．``teleop``を用いて実際に動かしたのが次に示すものである．

#####実行の様子
<iframe width="560" height="315" src="https://www.youtube.com/embed/N4mdS0C7EQ0" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

#####位置情報の確認
[![asciicast](https://asciinema.org/a/4BQdHeKA13XMNDcRnO46vf5kZ.svg)](https://asciinema.org/a/4BQdHeKA13XMNDcRnO46vf5kZ)
odomインタフェースがロボットから位置データを提供している．時間の経過に従ってロボットが移動，これによって変化する位置と向きの値が表示される．これらのメッセージは差動駆動のコントローラによって配信されている．このコントローラは，ロボットの車輪1つ1つで観測された動きを，ロボット本体の座標系に変換し，このメッセージを送っている．

##感想
ロボットの外観を前回に作成し，今回はようやくシミュレーションに必要になってくる物理的特性について実装を通して学ぶことができた．URDFは情報が多い分，文字がたくさん並ぶが今のところ同じようなものが複数ある状態で，1つ1つを理解して見ると，そこまで煩雑には見えない．最も，上で示したようにコメントを記載しておくと見やすくデバッグもしやすいと感じた．また，今回では差動駆動プラグインを入れることで，実際に移動ロボットを動かすこともできた．次回も楽しみである．


##参考文献
プログラミングROS Pythonによるロボットアプリケーション開発
　　　　　　　　Morgan Quigley, Brian Gerkey, William D.Smart 著
　　　　　　　　　　　　　　　　　　　　　　　河田　卓志　監訳
　　　　　　　　　　  　松田　晃一，福地　正樹，由谷　哲夫　訳
　　　　　　　　　　　　　　　　　　オイラリー・ジャパン　発行