ROS講座50 オムニホイールのシミュレーション2（全方向台車のモデリング）

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

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

# 概要
前回でオムニホイールとモーターのセットを作ることができました。今回はこれを3つ使って全方向移動台車を製作します。またros_controlをつかって軸を回してみます。

# ソースコード
前回でオムニホイールとモーターのxacroを作ったのでそれを利用します。

## urdf
モーター+オムニホイールのセットを3セット使って全方向移動台車を作ります。またボディーとして直方体の物体を入れます。

```xml:sim2_lecture/xacro/odm_try_wheel1.xacro
<?xml version="1.0"?>
<robot name="odm_robot" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:include filename="$(find sim2_lecture)/xacro/odm_common.xacro"/>
  <xacro:include filename="$(find sim2_lecture)/xacro/odm_omni_wheel_set1.xacro"/>

  <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">
    <visual>
      <geometry>
        <box size="0.14 0.08 0.02" />
      </geometry>
      <material name="gray"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.14 0.08 0.02" />
      </geometry>
    </collision>
    <inertial>
      <mass value="2.0"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>  
  </link>

  <joint name="wheel0_attach_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="wheel0_attach_link"/>
    <origin xyz="${0.07*cos(radians(60))} ${0.07*sin(radians(60))} 0.019" rpy="0 0 ${radians(60)}"/>
  </joint>
  <link name="wheel0_attach_link" />
  <xacro:omni_wheel_set prefix="wheel0" parent="wheel0_attach_link" />

  <joint name="wheel1_attach_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="wheel1_attach_link"/>
    <origin xyz="${0.07*cos(radians(180))} ${0.07*sin(radians(180))} 0.019" rpy="0 0 ${radians(180)}"/>
  </joint>
  <link name="wheel1_attach_link" />
  <xacro:omni_wheel_set prefix="wheel1" parent="wheel1_attach_link" />

  <joint name="wheel2_attach_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="wheel2_attach_link"/>
    <origin xyz="${0.07*cos(radians(300))} ${0.07*sin(radians(300))} 0.019" rpy="0 0 ${radians(300)}"/>
  </joint>
  <link name="wheel2_attach_link" />
  <xacro:omni_wheel_set prefix="wheel2" parent="wheel2_attach_link" />

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/odm_robot</robotNamespace>
    </plugin>
  </gazebo>
</robot>
```

## launch

ロボットのgazeboへの出現とros_controlを起動します。

```xml:sim2_lecture/launch/odm_gazebo2.launch
<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find sim2_lecture)/xacro/odm_try_wheel1.xacro" />
  <arg name="rvizconfig" default="$(find sim2_lecture)/rviz/odm_display.rviz" />  
  <param name="robot_description" command="$(find xacro)/xacro $(arg model) --inorder"/>

  <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" />

  <rosparam command="load" file="$(find sim2_lecture)/config/odm_move_controller.yaml" ns="odm_robot"/>  
  <rosparam command="load" file="$(find sim2_lecture)/config/odm_joint_state_controller.yaml" ns="odm_robot"/>  
  <node name="controller_spawner" pkg="controller_manager"
        type="spawner" respawn="false"
        output="screen" ns="/odm_robot"
        args="joint_controller0 joint_controller1 joint_controller2 joint_state_controller"/>
</launch>
```

ros_control用のパラメーターです。

```yaml:sim2_lecture/config/odm_move_controller.yaml
joint_controller0:
  type: effort_controllers/JointVelocityController
  joint: wheel0_motor_shaft_joint
  pid: {p: 0.05, i: 0.0, d: 0.0001, i_clamp: 1.0, antiwindup: true}

joint_controller1:
  type: effort_controllers/JointVelocityController
  joint: wheel1_motor_shaft_joint
  pid: {p: 0.05, i: 0.0, d: 0.0001, i_clamp: 1.0, antiwindup: true}

joint_controller2:
  type: effort_controllers/JointVelocityController
  joint: wheel2_motor_shaft_joint
  pid: {p: 0.05, i: 0.0, d: 0.0001, i_clamp: 1.0, antiwindup: true}
```

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

```shell
roslaunch sim2_lecture odm_gazebo2.launch 
```

![sim_odm_sim1.png](https://qiita-image-store.s3.amazonaws.com/0/254442/87b73f4e-0f92-2e86-52ef-16eded53454e.png)

軸を回してみます。

```shell
rostopic pub -r 5 /odm_robot/joint_controller0/command std_msgs/Float64 "data: 6.0"
```

![sim2_odm2.gif](https://qiita-image-store.s3.amazonaws.com/0/254442/7980d29b-8e36-056c-b2c9-2ad94c0b52c0.gif)

上記の画像のようにオムニホイールの中のバレルが回って、実際のオムニホイールと同様に動いているのが分かります。

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