SWD Starter Kitを使ってみた

# SWD Starter Kitを使ってみた

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/35ce5ceb-5981-f966-e06b-1cc7be610a60.png" width="50%"/>

SWD Starter KitはezWheel社が開発しているAGV/AMR向けのギヤモーター、モータードライバーが一体化されている製品です。ROS用のドライバソフトも提供されています。
国内ではIDEC株式会社が販売しています。

https://jp.idec.com/solutions/safety-wheel-drive

「安全自律走行ホイール」という事で、セーフティレーザスキャナとセーフティドライブをセットにすることで、安全規格(ISO3691-4)に適合するAMR/AGVの開発をサポートします。

今回、お勉強用にSWD Starter Kitを購入し自律移動ができるように設定しました。幾つかトラブルポイントも有りましたので本投稿にまとめました。

# 配線の接続

今回購入したのは自律移動コントローラが付属していないモデル(EZSK-CORE.14/WOID)です。簡単なSLAMができる産業用PCがついたモデルもありますが、色々と試す目的で購入していますのでコントローラは別途選定しました。

産業用PC無しモデルの場合、セーフティレーザスキャナ(SE2L)と接続するためのEthernetケーブル、モータードライバと接続するためのCANケーブルが付属していないため下記のものを手配しました。

コントローラ用PCにはUSB-CAN変換アダプタを使用しました。

|用途|品番|URL|
|---|---|---|
|SE2L用イーサネットケーブル|SE9ZHS2XCD13|https://www.monotaro.com/p/0951/4077/|
|SE2L設定用 microUSB ケーブル| --- | --- |
|CANケーブル|航空コネクタ M12 5ピン|https://www.amazon.co.jp/dp/B07JZLHMMJ|
|D-SUB9ピン ハウジング|---|https://www.amazon.co.jp/dp/B07G4PK97N|
|CAN-USBインタフェース|PCAN-USB IPEH-002021|https://www.gailogic.co.jp/ae/can_pcif/pcan_usb|

CAN-USBインタフェースは[innomakerのUSB2CAN-C](https://www.amazon.co.jp/dp/B09K3LL93Q)でも動作しましたが、コントローラPCを再起動したとき再接続できない事がたまにあるみたいです。業務などで使用する場合はPCAN-USB等のちゃんとした物を使用したほうが良さそうです。

## CANケーブルの加工

購入した航空コネクタはM12コネクタの反対側はバラ線になっているのでD-SUB9ピンのコネクタを取り付けます。
M12コネクタとPCAN-USBのD-SUB9のピン配置は下記の様になっています。(USB2CAN-Cのピン配置も同じでした。)

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/ff8aacd0-2829-8329-ed21-d6966e125598.png" width="45%"/> <img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/6859d212-f25e-d6b9-ef3d-dcdf625ffe24.png" width="50%"/>

D-SUB9コネクタハウジング内で、CAN_HとCAN_Lの間に終端抵抗120Ωを取り付けています。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/e163b4ce-4e06-9e7a-988a-79107bad4f33.jpeg" width="50%"/>

# コントローラPCの設定

ez-WheelをROSから制御するためのドライバはgithubで公開されています。ROS 2(Foxy or Galactic)/ROS(noetic)に対応していますが、今回はnoeticで動かしました。

https://github.com/ezWheelSAS/

## ROS noeticのインストール

コントローラ用PCにubuntu 20.04をインストールした後、こちらを参考にnoeticのインストールします。

http://wiki.ros.org/noetic/Installation/Ubuntu/

**ユーザ名は swd_sk とします**。ユーザ名はSWDドライバサービスを起動する時に指定されるため、これ以外のユーザ名で起動すると上手く動作しません。ユーザ名をswd_sk以外に設定した場合、systemdの設定ファイル（/etc/systemd/system/ezw-*）に記載されている実行ユーザ名を変更します。

## CAN-IFの設定

USBポートにCAN-IFを接続した状態で行わないとエラーになります。

### ドライバをインストール。
```shell
$ sudo -E apt update
$ sudo -E apt-get install can-utils
$ sudo modprobe can
$ sudo modprobe can_raw
```

### IFの設定
```shell
$ sudo ip link set can0 type can bitrate 1000000
$ sudo ip link set up can0
```

### 疎通確認

SWD Starter Kitと接続し

```shell
$ candump can0 | grep 7F
```
と打って、データが流れてきたらOKです。

## セーフティレーザスキャナ(SE2L)の設定

設定方法は[こちらのドキュメント](https://github.com/ezWheelSAS/swd-starter-kit-config/blob/main/docs/Commissioning.md)に記載されています。

https://github.com/ezWheelSAS/swd-starter-kit-config

SE2LのIPアドレス設定、保護エリア設定は[SLS Project Designer](https://us.idec.com/idec-us/en/USD/Software-SLS-Project-Designer)を起動し、取得した設定ファイル [StarterKit_IDEC.hucx](https://github.com/ezWheelSAS/swd_starter_kit_scripts/blob/main/commissioning/StarterKit_IDEC.hucx) を読み込んでSE2Lに書き込みます。

SE2LとSLS Project Designerを使うWindowsPCは、SE2Lのフロントカーバを外したところにあるmicroUSBコネクタを使って接続します。

**書き込みステップの１つ前の確認画面にて、複数のタブの内容を全て表示して確認しないと、書き込みボタンがEnableになりません。**

## SWDドライバのインストール

githubの[README](https://github.com/ezWheelSAS/swd_ros_controllers/blob/main/README.md)記載に従ってドライバのインストールを行います。

### リポジトリ設定とパッケージのインストール

```shell
$ echo "deb http://packages.ez-wheel.com:8081/apt-repo focal main" | sudo tee /etc/apt/sources.list.d/ez-wheel.list
$ wget -qO - http://packages.ez-wheel.com:8081/archive.key | sudo apt-key add –
$ sudo -E apt update && sudo -E apt install swd-services ros-noetic-swd-ros-controllers swd-system-config-2wheels
```

### 環境変数の設定

ライブラリパスの設定
```shell
$ echo 'export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/opt/ezw/usr/lib' >> ~/.bashrc
```

DBUSの設定
```shell
$ echo 'export $(cat /tmp/SYSTEMCTL_dbus.id)' >> ~/.bashrc
```

### 動作の確認

再起動後、swd_diff_drive_controllerノードを起動してみます。

```shell
$ roslaunch swd_ros_controllers swd_diff_drive_controller.launch
```

```/swd_diff_drive_controller/cmd_vel```にTwistコマンドを送信し、モーターが回転することを確認します。もし回転しない場合、```dbus-launch```コマンド実行結果と、環境変数に設定されている値が同じか確認してください。


# SLAMをしてみる

swd_starter_kit_bringupリポジトリに、hector-slamで環境地図を作成するためのlaunchファイルが登録されています。

https://github.com/ezWheelSAS/swd_starter_kit_bringup

## パッケージの取得とビルド

```shell
$ sudo -E apt install git ros-noetic-urg-node ros-noetic-teleop-twist-joy \
  ros-noetic-hector-mapping ros-noetic-rosbridge-server ros-noetic-tf2-web-republisher \
  --no-install-recommends
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ git clone https://github.com/ezWheelSAS/swd_starter_kit_bringup.git
$ git clone https://github.com/ezWheelSAS/swd_robot_manager.git
$ cd ~/catkin_ws
$ catkin build
```

## ゲームパッドのパラメータ変更

今回は台車をリモートから操作するためのエレコムのゲームパッドを使用しました。

https://www.elecom.co.jp/products/JC-U4113SBK.html

swd_starter_kit_bringupに含まれる設定では xbox のコントローラを使うように設定されているので次の様の変更します。

```xml:swd_starter_kit_bringup/launch/starter_kit.launch
  <node pkg="teleop_twist_joy" name="teleop_twist_joy"
  type="teleop_node">
    <param name="joy_config" value="$(arg joy_config)" />
※  <param name="enable_button" value="4" />         
※  <param name="enable_turbo_button" value="5" />
    <param name="axis_linear" value="1" />
※  <param name="axis_angular" value="0" />
※  <param name="scale_linear" value="0.4" />
※  <param name="scale_angular" value="0.4" />
    <param name="scale_linear_turbo" value="1.0" />
    <param name="scale_angular_turbo" value="0.8" />
    <remap from="/cmd_vel" to="/swd_diff_drive_controller/cmd_vel" />
  </node>
```
**scale_linearとscale_angularはデフォルト設定だとSWDの速度制限に引っかかるので遅めに変更しました。**
制限値はswd_diff_drive_controllersのlaunchファイルの設定で変更できます。

メーカーの[資料](https://ez-wheel.com/storage/upload/pdf/swd-core-v10x-19042022-instruction-manual-en-0.pdf)によると、ギヤの減速比が14:1で、モーターの回転リミットは1800rpm、減速後のホイール回転で130rpmです。starter kitのデフォルト設定はホイール回転制限75rpm、ホイールは直径125mmなので速度は約0.49m/secとなります。

## SLAM実行

```shell
$ cd ~/catkin_ws
$ source devel/setup.bash
$ roslaunch swd_starter_kit_bringup starter_kit.launch
```

ゲームコントローラで台車を移動させると地図が作成できます。会社のオフィス内を一周回った時の地図をrvizで表示すると下記のような地図が確認できました。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3112366/a579dc25-6ac9-5340-4cac-93015686abf6.png" width="50%"/> 

# おわりに

SWD Starter Kitを立ち上げるために必要なケーブルと、コントローラPC設定方法についてまとめました。作業時間１日ぐらいで、SLAMで地図を作成するところまでできました。

今回は地図を作成するところまでですが、次回はナビゲーション用ソフトを載せて自律移動させてみます。