9軸IMUモジュールWT901CをROS Noeticで使用する


# はじめに

ROSで使いやすいIMUモジュールをさがしていたところ、Witmotion製のWT901CというモジュールがAmazonで入手できたので、ROS Noeticで動かしてみた。

# ROSで使いやすいIMUの条件

- ROSパッケージが公開されている
- 9軸である（地磁気が使える）
- 姿勢（orientation）を内部で計算して出力してくれる
- シリアル通信でPCに直接接続して使える（SPIなどのI/Oが不要）


姿勢に関しては加速度と角速度の生データだけでは、sensor_msgs/Imuのorientationの情報を`imu_filter_madgwick`などで自分で計算する必要があるので、モジュール内部でいい感じの値を推定して出力してくれた方が使いやすい。
また9軸に関しては平面を移動する自律移動ロボットにおいて最も重要なYaw角の姿勢情報がジャイロのみだと微妙なので、地磁気も搭載していた方がYaw角の推定精度が良い。

http://wiki.ros.org/imu_filter_madgwick

# 動作環境

OS : ubuntu20.04.3LTS
ROS : Noetic

動作確認用アプリ
Windows10

# 必要なもの

WT901C本体（RS232C版）
※同じ型番でRS485 Modbus版とTTL版があるので購入時に注意

https://www.amazon.co.jp/gp/product/B07SLJJJWJ/ref=ppx_yo_dt_b_asin_title_o01_s00?ie=UTF8&psc=1


USB - UART変換モジュール
（こちらは公式のものでなくても何でもよい）

https://www.amazon.co.jp/gp/product/B09CTJ86FR/ref=ppx_yo_dt_b_asin_title_o00_s00?ie=UTF8&psc=1

![DSC_0354.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/b2c2736f-1b50-c9b7-41db-c0a200992ccc.jpeg)


# WT901Cの動作確認
以下Windows10が必要

以下から公式のWindowsアプリケーションをダウンロード
`Standard Software for Windows PC.zip`をダウンロードして解凍

https://drive.google.com/drive/folders/1dq6J0l351LG8G4BD3A-M4JOgC9z3BX4H

`MiniIMU.exe`をクリックして起動

![fig.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/dadd9c9b-2765-a2f1-877b-4688be1ddc57.png)


`3D`をクリックするとUnityアプリが立ち上がってモジュールを回転させると画面上のオブジェクトも回転する。

![ezgif.com-gif-maker.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/11f251cb-1193-e5b3-dbfa-fb4e02e87cb4.gif)





https://github.com/yowlings/wit_node

# ROSで動かす準備

### ROSパッケージのインストール

以下Witmotion IMUのROSパッケージをインストール

https://github.com/yowlings/wit_node

ros-ecl関係をインストール

```bash
$ sudo apt install ros-noetic-ecl*
```

wit_nodeのインストールとBuild

```bash
$ cd ~/catkin_ws/src
$ git clone https://github.com/yowlings/wit_node.git
$ cd ~/catkin_ws
$ catkin build
```






### UARTの通信ボーレートの変更

先程のWindowsアプリから変更できる。
購入時のデフォルトのボーレート設定は9600bpsになっているが、ROSノード側では115200bpsになっている（include/parameter.hpp 8行目）。
そのため、WT901Cの設定を変更してボーレートを115200bpsに変更する。
(任意の通信速度で使用したい場合はこの部分を変更する)

1. 上のconfigをクリック
2. Communicationの欄からBaud RateとOutput Rateを選択して一番右のchangeをクリック

![fig2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/b8afb5f6-8254-50ce-be32-f3612cd8ed02.png)

```cpp:parameter.hpp
namespace wit {
class Parameter {
 public:
  Parameter() : port_("/dev/ttyUSB0"), baut_rate_(115200), ns("wit") {}
```







### Pose Topicを出力するノードの作成（必要なら）

rvizのIMUプラグインは回転が分かりづらいので、imuトピックの回転をPoseStamped型に変換してPulishするノードを作成する。


```python:imu_to_pose.py
#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import Imu

def imu_callback(imu_msg):
    ps_msg = PoseStamped()
    ps_msg.header=imu_msg.header
    ps_msg.pose.orientation=imu_msg.orientation
    posestamped_pub.publish(ps_msg)

if __name__ == "__main__":
    rospy.init_node("imu_to_pose")
    #Publisher
    posestamped_pub = rospy.Publisher("imu_pose", PoseStamped, queue_size=10)
    #Subscriber
    rospy.Subscriber("imu", Imu, imu_callback)
    rospy.spin()
```




# 実行

PCにWT901Cを接続
実行権限を付与してlaunchファイルを実行

```bash
$ sudo chmod 777 /dev/ttyUSB0
$ roslaunch wit_node wit.launch
```

まずモジュールを動かしてIMUトピックの中身が変化しているかを確認

```bash
$ rostopic echo /imu
```

### rvizで回転を見る

先ほど作成したPythonノードを実行

```bash
$ python3 imu_to_pose.py
```

rvizを起動して`ADD`からPoseを選択。
Topicで`/imu_pose`を選択

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">ROS Noeticで9軸IMU WT901C使えるようになった<a href="https://twitter.com/hashtag/ros?src=hash&amp;ref_src=twsrc%5Etfw">#ros</a> <a href="https://t.co/SrcMaBhaES">pic.twitter.com/SrcMaBhaES</a></p>&mdash; porizou (@porizou1) <a href="https://twitter.com/porizou1/status/1466754842035384322?ref_src=twsrc%5Etfw">December 3, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

# おわりに

ROSで使える9軸IMUとしてBNO055 などが有名であるが、近頃の半導体不足などの影響からか国内で入手しづらくなっている。
執筆時現在（2021/12/07）WitmotionのIMUモジュールはAmazonで簡単に入手できるようなのでROSで手軽に使えるIMUとしてWT901Cを検討してみてはどうでしょうか。




# 参考

https://kim-xps12.github.io/b-sky-lab/technology/2021/04/06/wt901c.html

https://github.com/yowlings/wit_node/issues/2