ROS講座88 オムニホイールの簡易シミュレーション

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

| 項目 | 値 |
|:-:|:-:|
| 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)を参照してください。

# 概要
これまでにオムニホイールを厳密にシミュレーションする方法を説明してきましたが、今回はとにかく物理的にシミュレーションせずに、twist通りに動かすplanar_moveというプラグインの使い方を説明します。

# ソースコード

## urdf
簡易移動プラグインを使用したurdfです。

```xml:sim2_lecture/urdf/odm_simple.urdf
<?xml version="1.0"?>
<robot name="odm_robot" xmlns:xacro="http://ros.org/wiki/xacro">
  <material name="red">
    <color rgba="1.0 0 0 2.5"/>
  </material>

  <link name="base_link"/>

  <joint name="body_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="body_link"/>
    <origin xyz="0 0 0.05" rpy="0 0 0"/>
  </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>
       <cylinder radius="0.1" length="0.1"/>
     </geometry>
     <material name="red" />
   </visual>
   <collision>
     <origin xyz="0 0 0" rpy="0 0 0"/>
     <geometry>
       <cylinder radius="0.1" length="0.1"/>
     </geometry>
   </collision>
  </link>
  <gazebo reference="body_link">
    <material>Gazebo/Red</material>
    <mu1 value="0.1" />
    <mu2 value="0.1" />
  </gazebo>

  <joint name="head_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="head_link"/>
    <origin xyz="0.05 0 0.12" rpy="0 0 0"/>
  </joint>
  <link name="head_link">
   <visual>
     <origin xyz="0 0 0" rpy="0 0 0" />
     <geometry>
       <cylinder radius="0.03" length="0.04"/>
     </geometry>
     <material name="red" />
   </visual>
  </link>
  <gazebo reference="head_link">
    <material>Gazebo/Red</material>
  </gazebo>

  <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>
</robot>
```

以下がプラグインの本体です。`<commandTopic>`で受け取るtwistのrostopic名を、`<robotBaseFrame>`で移動対象のlink名を指定します。

```xml:移動プラグイン
  <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>
    </plugin>
  </gazebo>
```

## launch

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

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model odm_robot" />

</launch>
```

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

```shell:実行
roslaunch sim2_lecture odm_simple.launch 
```

```shell:twist送信
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:
  x: 0.5
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.5" 
```

![sim_odm_simple.gif](https://qiita-image-store.s3.amazonaws.com/0/254442/964e7dcb-b4f1-46ed-141c-359133d67825.gif)


#問題点
このプラグインを適用すると重力が働かなくなるという特徴があります。具体的には

* 段差から落ちそうになっても段差などなかったかのように宙を浮いて進みます。
* 重心が高かったり、床との摩擦が大きかったりすると、ロボットが傾くことがあります。その後傾いた状態で空間に固定されます。

解決策はあるようですが、ソースからビルドしなおさないといけなくなるので面倒です。

# 参考
[PlanarMovePlugin](http://gazebosim.org/tutorials?tut=ros_gzplugins#PlanarMovePlugin)
[PlanarMovePluginの問題](https://github.com/ros-simulation/gazebo_ros_pkgs/issues/121)
[PlanarMovePluginの問題の修正方法](https://github.com/130s/gazebo_ros_pkgs/commit/5af4a0a90e9f0a17a95323296790a7cb94d1c8b8)

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