ROS講座35 車輪ロボットをシミュレーションする

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

| 項目 | 値 |
|:-:|:-:|
| 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でmodelを記述して車輪ロボットをgazebo上に出現させます。またgazeboプラグインを使って車輪ロボットを動かします。

# ソースコード

差動2輪ロボットのGazeboモデルを作成します。

```xml:gazebo1_lecture/models/wheel_robot_simple/model.config
<?xml version="1.0" ?>
<model>
    <name>wheel_robot_simple</name>
    <version>1.0</version>
    <sdf version="1.6">model.sdf</sdf>
    <author>
        <name></name>
        <email></email>
    </author>
    <description></description>
</model>
```

```xml:gazebo1_lecture/models/wheel_robot_simple/model.sdf
<?xml version='1.0'?>
<sdf version="1.4">
  <model name="wheel_robot_simple">
    <link name="body_link">
      <pose>-0.05 0 0.1 0 0 0</pose>
      <inertial>
        <mass>5.0</mass>
        <inertia>
          <ixx>0.042</ixx>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyy>0.042</iyy>
          <iyz>0.0</iyz>
          <izz>0.075</izz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>0.3 0.3 0.1</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>0.3 0.3 0.1</size>
          </box>
        </geometry>
      </visual>
    </link>

    <joint name='back_ball_joint' type='fixed'>
      <parent>body_link</parent>
      <child>back_ball_link</child>
      <pose>0 0 0 0 0 0</pose>
    </joint>
    <link name="back_ball_link">
      <pose>-0.15 0 0.05 0 0 0</pose>
      <inertial>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.000001</ixx>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyy>0.000001</iyy>
          <iyz>0.0</iyz>
          <izz>0.000001</izz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <sphere>
            <radius>0.05</radius>
          </sphere>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>0.01</mu>
              <mu2>0.01</mu2>
            </ode>
          </friction>
          <contact>
            <ode>
              <kp>100000</kp>
              <kd>1</kd>
            </ode>
          </contact>
        </surface>
      </collision>
      <visual name="visual">
        <geometry>
          <sphere>
            <radius>0.05</radius>
          </sphere>
        </geometry>
      </visual>
    </link>

    <joint name='left_wheel_joint' type='revolute'>
      <parent>body_link</parent>
      <child>left_wheel_link</child>
      <pose>0 0 0 0 0 0</pose>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>
    <link name="left_wheel_link">
      <pose>0 0.18 0.072 1.5707 0 0</pose>
      <inertial>
        <mass>0.2</mass>
        <inertia>
          <ixx>0.00026</ixx>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyy>0.00026</iyy>
          <iyz>0.0</iyz>
          <izz>0.00052</izz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <cylinder>
            <radius>0.072</radius>
            <length>0.04</length>
          </cylinder>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>0.5</mu>
              <mu2>0.5</mu2>
            </ode>
          </friction>
          <contact>
            <ode>
              <kp>100000</kp>
              <kd>1</kd>
            </ode>
          </contact>
        </surface>
      </collision>
      <visual name="visual">
        <geometry>
          <cylinder>
            <radius>0.072</radius>
            <length>0.04</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Red</name>
          </script>
        </material>
      </visual>
    </link>

    <joint name='right_wheel_joint' type='revolute'>
      <parent>body_link</parent>
      <child>right_wheel_link</child>
      <pose>0 0 0 0 0 0</pose>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>
    <link name="right_wheel_link">
      <pose>0 -0.18 0.072 1.5707 0 0</pose>
      <inertial>
        <mass>0.2</mass>
        <inertia>
          <ixx>0.00026</ixx>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyy>0.00026</iyy>
          <iyz>0.0</iyz>
          <izz>0.00052</izz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <cylinder>
            <radius>0.072</radius>
            <length>0.04</length>
          </cylinder>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>0.5</mu>
              <mu2>0.5</mu2>
            </ode>
          </friction>
          <contact>
            <ode>
              <kp>100000</kp>
              <kd>1</kd>
            </ode>
          </contact>
        </surface>
      </collision>
      <visual name="visual">
        <geometry>
          <cylinder>
            <radius>0.072</radius>
            <length>0.04</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Red</name>
          </script>
        </material>
      </visual>
    </link>

    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <commandTopic>/cmd_vel</commandTopic>
      <odometryTopic>/odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <robotBaseFrame>base_link</robotBaseFrame>
      <publishWheelTF>false</publishWheelTF>
      <publishOdomTF>true</publishOdomTF>
      <publishWheelJointState>false</publishWheelJointState>
      <wheelSeparation>0.36</wheelSeparation>
      <wheelDiameter>0.144</wheelDiameter>
      <wheelAcceleration>30</wheelAcceleration>
      <wheelTorque>5.0</wheelTorque>
      <updateRate>40</updateRate>
      <odometrySource>encoder</odometrySource>
      <leftJoint>left_wheel_joint</leftJoint>
      <rightJoint>right_wheel_joint</rightJoint>
      <publishTf>true</publishTf>
    </plugin>
  </model>
</sdf>
```

## link-joint構造
以下の記述でlinkとlinkを接続することが出来ます。この例ではbody_linkの子リンクとしてback_ball_jointを介してback_ball_linkを接続します。

```xml:モデルの一部
<link name="body_link">
</link>

<joint name='back_ball_joint' type='fixed'>
  <parent>body_link</parent>
  <child>back_ball_link</child>
  <pose>0 0 0 0 0 0</pose>
</joint>
<link name="back_ball_link">
</link>
```

今回のモデルは全体では以下のようなリンク構造です。

* body_link
    * (fixed)back_ball_joint--back_ball_link: 後輪のキャスターに相当
    * (revolute)left_wheel_joint--left_wheel_link: 左の動輪
    * (revolute)right_wheel_joint--right_wheel_link: 右の動輪

`fixed`は2つのlinkを固定で接続します。`revolute`は2つのlinkは軸を中心に回転します。

## 衝突演算パラメーター設定

```xml:collision要素の一部
<surface>
  <friction>
    <ode>
      <mu>0.5</mu>
      <mu2>0.5</mu2>
    </ode>
  </friction>
  <contact>
    <ode>
      <kp>100000</kp>
      <kd>1</kd>
    </ode>
  </contact>
</surface>
```

* `friction`は摩擦係数の設定です。基本的にmu=mu2の値にします。ボールキャスター等の滑るものは0.01~0.1程度、動輪のタイヤなどのグリップするものなら0.5程度にします。
* `contact`は表面の硬さに関するパラメーターです。
    * kpは表面の「めり込み」についての反力です。めり込み量[m]xkp[N/m]=反力[N] となります。小型ロボットではkp=1e5~7程度になります。kdは速度に関する反力です。影響はそこまで大きくないので1にしておきます。 
* このようなSDFの要素は[SDFフォーマットリファレンス](http://sdformat.org/spec?ver=1.9)に載っています。


## プラグインの設定
差動2輪のタイヤを回すプラグインであるdiff_driveプラグインを使用します。このプラグインはGazebo ModelプラグインなのでModelタグの直下に書きます。

```xml:modelの一部
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
  <commandTopic>/cmd_vel</commandTopic>
  <odometryTopic>/odom</odometryTopic>
  <odometryFrame>odom</odometryFrame>
  <robotBaseFrame>base_link</robotBaseFrame>
  <publishWheelTF>false</publishWheelTF>
  <publishOdomTF>true</publishOdomTF>
  <publishWheelJointState>false</publishWheelJointState>
  <wheelSeparation>0.36</wheelSeparation>
  <wheelDiameter>0.144</wheelDiameter>
  <wheelAcceleration>30</wheelAcceleration>
  <wheelTorque>5.0</wheelTorque>
  <updateRate>40</updateRate>
  <odometrySource>encoder</odometrySource>
  <leftJoint>left_wheel_joint</leftJoint>
  <rightJoint>right_wheel_joint</rightJoint>
  <publishTf>true</publishTf>
</plugin>
```

ROSトピックで受信したTwistを受けてタイヤを回す`libgazebo_ros_diff_drive.so`の設定をします。

* 駆動側
    * commandTopic: Twistのトピック名
    * leftJoint: 左タイヤのjoint名
    * wheelSeparation: 左右輪の間の距離
    * wheelDiameter: 車輪の直径
    * leftJoint: 左タイヤのjoint名
    * rightJoint: 右タイヤのjoint名
    * wheelAcceleration: タイヤの最大加速度
    * wheelTorque: タイヤの最大トルク
* odom出力側
    * odometryTopic: Odomトピック名
    * updateRate: 出力レート
    * odometryFrame: odomフレーム名
    * robotBaseFrame: baseフレーム名
    * odometrySource: 出力される位置のタイプ(encoder or world)
* joint_state出力側
    * publishWheelJointState: 出力enable
* tf出力側
    * publishOdomTF: jointのtfの出力enable
    * publishWheelTF: odom->baseのtfの出力enable
    * publishTf: tfの出力enable

# 実行

## モデルの確認
各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。

```
roslaunch gazebo_ros empty_world.launch world_name:=wheel_robot_simple.world 
```

SDFで定義したように差動2輪ロボットが出現します。

![gazebo1_wheel1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/e06b76fe-9252-6026-5e4f-651b9319740e.png)

ここで「view」->「Inertials」、「Contacts」をチェックします。そうするとイナーシャ相当の直方体や接地点が表示されます。ここで

* Inertials: ピンクの図形が、おおむね表示されている図形と一致しているか確認します。ずれが大きい場合は、<inertia>タグを修正します。
* Contacts: 接地点が安定しているか確認します。振動して接地点が現れたり消えたりする場合は<contact>タグの中身を修正してください。

![gazebo1_wheel2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/9bace5ed-bda8-d1cb-88c6-272ed52109c0.png)

## 駆動
rvizも起動します。のちのパートで作るものですが、twist panel(plugin編で作成)を起動します。これを使って/cmd_velにTwistをpublishします。

![gazebo1_wheel3.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/74dce25d-bdee-b59a-219c-7941fa5d3e00.gif)

以下のようなコマンドでもTwistをpublishできます。

```shell:cmd_velのpublish
rostopic pub -1 /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"
```

# 参考
[ROSwiki urdf/link](http://wiki.ros.org/urdf/XML/link)
[Gazebo + ROS で自分だけのロボットをつくる](https://qiita.com/RyodoTanaka/items/174e82f06b10f9885265)
[SDFフォーマットリファレンス](http://sdformat.org/spec?ver=1.9)

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