9軸IMUモジュールHWT901B-TTLをROS1で使用するメモ

# はじめに

ロボットを自律走行させるため、前回の記事の続きです。
IMU（9軸IMUモジュールHWT901B-TTL）を拾ったので、ROS1で使ってみました。

https://qiita.com/kiwsdiv/items/5699f294f33663ade2ae

# 参考

porizou1さんの記事を参考にさせていただきました。ありがとうございました。
この記事はWT901のUSB版でしたが、シリアル接続後はこの記事を参考にしました。

https://qiita.com/porizou1/items/0e9c956c89c584f6c072

# 環境

- RasberryPi 4B(Ubuntu 20.04.4 LTS)

| 項目 | バージョン |
|-- | -- | 
| Ubuntu|20.04|
|ROS|Noetic|

- IMU： HWT901B-TTL

https://www.wit-motion.com/10-axis/witmotion-hwt901b-ttl-10.html

公式ドキュメント：[Witmotion WT901 Datasheet.pdf (elettra.eu)](https://ipfs.elettra.eu/ipfs/QmQZpNbeAdFcg3kDyjyeXBRpPW3McUyCzeR2fbV3Uqyo9Z/Witmotion%20WT901%20Datasheet.pdf)

amazonで1万円以下で購入できるみたいですね。

https://www.amazon.co.jp/%E3%80%90%E3%83%9F%E3%83%AA%E3%82%BF%E3%83%AA%E3%83%BC%E3%82%B0%E3%83%AC%E3%83%BC%E3%83%89%E5%8A%A0%E9%80%9F%E5%BA%A6%E8%A8%88-%E5%82%BE%E6%96%9C%E8%A8%88%E3%80%91HWT901B-TTL-%E8%A7%92%E5%BA%A6%EF%BC%88XY0-05%C2%B0%E7%B2%BE%E5%BA%A6%EF%BC%89-%E9%AB%98%E5%BA%A6%E3%80%90RM3100%E7%A3%81%E5%8A%9B%E8%A8%88%E8%A3%9C%E6%AD%A3%E3%81%8A%E3%82%88%E3%81%B3%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%83%BC%E3%80%91%EF%BC%88UART%E3%82%B7%E3%83%AA%E3%82%A2%E3%83%ABAHRS-IMU%E3%80%81200Hz%E3%83%87%E3%82%B8%E3%82%BF%E3%83%AB%E5%87%BA%E5%8A%9B%E3%80%81Z%E8%A7%92%E5%BA%A6%E3%81%AE%E4%BD%8E%E3%83%89%E3%83%AA%E3%83%95%E3%83%88%EF%BC%89/dp/B07S3K365T


# やったこと
## シリアル接続

手元にあるWT901はTTL版のため、シリアル接続させます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/2b57c483-bd6a-8f66-8214-6680e0ab2fa0.png)

本来はUSB変換が同梱されているみたいですが、無かったので、ラズパイのGPIOポートを使って接続します。

ラズパイのGPIOでシリアル通信するための設定は、以下の記事に別で記載しました。

https://qiita.com/kiwsdiv/items/4d4cb3ec007944104992

結線図を見ていきます。4本線が出ており、図のようにVCC、Rx, Tx, GNDとなっているようです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/0fcc21af-8131-8dba-36dc-557c59e66e2a.png)

HWT901B-TTL本体の裏にも記載されてました。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/6375daec-bd5c-1f6b-c845-9b37bcae0e82.png)

ラズパイ側のGPIO14(TXD)とGPIO15(RXD)と接続していきます。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/de20a0b4-54de-ac55-faa2-0e8ed6b166e6.png)


| ラズパイ | | 
|-- |-- | 
| GPIO14(TXD)| Green(RX)| 
| GPIO15(RXD)| Yellow(TX)|
| GND| Black(RX)|

5Vはラズパイから供給できない(動いたけど不安定だった)ので、別の基板(OpenCR)から拝借しました。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/d9e9509b-bded-343c-bb65-a82592a70d9b.png)

## 受信信号の確認

受信してみます。何かしら受信してるようですが、ボーレートが合ってないのか、文字化けしています。

```bash
$ sudo cat /dev/ttyAMA0
z pb     s ޚ*b )(  
*j )( ߆ ;8   T b  ߆ ;8븒 b )( ߊu _    C b  ߊu _    C b  
 ";8 äz b  
 ";8  æz b  
a  ;8     C`b  
a  ;8 b: b  
 E`   Z bR)( 
tݬ b (8 ߊ" ( ݬ bv(8 ߊ" ( ݬ b (8     ݬ b8 j        Ub8 j   
     Ub8 j   
      b9(8 Њ       b[(8 Њ       b8 j   
     b+(8 
     j[(8        j[          b8 j     j    b8 j     j    j8 j    "j    j8 j    "j    j8 j         j8 j         j    u      #   u     #   
   U  #   
    U   #   
 " (    j   
```

ラズパイ側のボーレートの設定は38400となっていました。別の速度に設定していたはずですが、接続するたびに自動で変更されるのでしょうか。よくわからないので後で調べます。

```bash
$ sudo stty -F /dev/ttyAMA0
speed 38400 baud; line = 0;
-brkint ixoff iutf8
echoprt
```
## ROS動作の準備
ここから、参考記事[9軸IMUモジュールWT901CをROS Noeticで使用する](https://qiita.com/porizou1/items/0e9c956c89c584f6c072)のように進めていきます。

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

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

### ボーレート、ポート設定の変更

ROSノード側の設定は `wit_node/include/wit_node/parameter.hpp`を変更します。
私の環境では、接続先のシリアルポートは"/dev/ttyAMA0"でした。

```cpp:parameter.hpp
#include <string>
#ifndef PARAMETER_HPP
#define PARAMETER_HPP
using namespace std;
namespace wit {
class Parameter {
 public:
  Parameter() : port_("/dev/ttyAMA0"), baut_rate_(38400), ns("wit") {}
  string port_;
  int baut_rate_;
  string ns;
};
}
#endif  // PARAMETER_HPP
```

また、launchファイル`wit_node/launch/wit.launch`を変更します。

```launch:wit.launch
<launch>
  <arg name="port"                  default = "/dev/ttyAMA0" />
  <node pkg="nodelet" type="nodelet" output="screen" name="wit_nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" output="screen" name="wit" args="load wit_node/WitNodelet wit_nodelet_manager" >
    <param name="port" value="$(arg port)" />
  </node>
</launch>
```

```bash
$ cd ~/catkin_ws
$ catkin build
```

### Pose Topicを出力するノードの作成（必要なら）
porizou1さんの記事をまるまる参考に、/imu_poseトピックを出力するノードを作成しました。

> 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()
```

## 実行
権限を付与しておきます。
```bash
$ sudo chmod 777 /dev/ttyAMA0
```

実行します。
```bash
$ roslaunch wit_node wit.launch
~~~
[ INFO] [1703815988.845498160]: Wit : initialised.
```

Wit : initialised.まで表示されたら、無事実行できているようです。
別のターミナルトピックを確認します。
```
$ rostopic list
/gps
/imu
/rosout
/rosout_agg
/wit/raw_data
/wit/related_yaw
/wit/reset_offset
/wit_nodelet_manager/bond
```

/imu トピックが流れていることも確認できます。
```bash
$ rostopic echo /imu
header:
  seq: 167
  stamp:
    secs: 1703816001
    nsecs: 645507122
  frame_id: "imu_link"
orientation:
  x: -0.0024576369886855193
  y: -0.03853854587173703
  z: 0.033045750629375904
  w: 0.998707524187069
orientation_covariance: [0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001]
angular_velocity:
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance: [1e-05, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 1e-05]
linear_acceleration:
  x: 0.6986328125000001
  y: -0.052636718750000006
  z: 9.766503906250001
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]

```

## rvizで回転を見る

先ほど作成したPythonノードを実行します。
```bash
$ python3 imu_to_pose.py
```
rvizを起動してADDからPoseを選択、PoseのTopicを/imu_poseに変更、Global OptionsのFixed Rrameをimu_linkに変更することで、pose表示されました。ここではShapeをAxesに変更しています。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/34b31a57-d997-18e3-c554-e28f06f8f88e.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/51e0daa4-eb68-d24e-fdf3-fb9ee3cba602.png)

https://x.com/kixxchllng/status/1740684470054002998?s=20

# 失敗例
## ラズパイの5V Powerに接続
これでも動いたけど、不安定。途中でラズパイがシャットダウンしてしまいました。ラズパイから直接5Vを取ってくるのは諦めました。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/d5e7c910-87f1-6638-e38e-8775ba158d4a.png)

## 失敗 Witmotion_ros 公式ページの手順
[witmotion_ros - ROS Wiki](https://wiki.ros.org/witmotion_ros)

インストールまではできましたが、message-enumeratorが存在しないとのこと。/opt/ros/noetic/share/witmotion_rosを見ても空でした。よくわからないので放置しましたが、別で
message-enumeratorを準備する手順があるのかもしれません。

$ sudo apt install ros-${ROS_DISTRO}-witmotion-ros
$ rosrun witmotion_ros message-enumerator -d ttyAMA0 -b 38400 -p 10
[rosrun] Couldn't find executable named message-enumerator below /opt/ros/noetic/share/witmotion_ros

## 権限付与できてなかった

権限付与したつもりでwit.launch を実行したら、Initializeのエラーが発生。権限付与できてなかったみたいです。

```bash
$ roslaunch wit_node wit.launch
~~~
[ERROR] [1703815890.517596984]: Wit : initialisation failed [/wit].
[ERROR] [1703815890.517767212]: Wit : could not initialise! Please restart.
```
同様のエラーを踏んでいる方のIssueがありました。

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

# まとめ

本記事ではROS1(Noetic)で9軸IMUモジュール HWT901B-TTLを使用したときのメモを記載WIしました。いつもそうですが、先人の記事に感謝です。