ROS講座81 IMU(GY-955)を使う

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

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 16.04 |
| ROS | Kinetic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
IMUはロボットにおいて重要なセンサーです。IMUというセンサーがあるのではなく以下の主に3つのセンサーを組み合わせたセンサーです。

* 加速度センサー
* 角速度センサー
* 地磁気センサー

最終的にはこれらの3つを総合して姿勢（Orientation）を算出します。通常のIMUは上記の3つの値を出してPC側でカルマンフィルターなどの処理を用いて姿勢を出します。しかしGY-955には内部でこの処理を行うので、PC側での負荷が軽くなります。

# GY-955について
GY-955はモジュールの名前でIMUはBNO055を使っています。[BNO055をUSB変換と合わせたモジュール](https://www.digikey.jp/product-detail/ja/bosch-sensortec/BNO055-USB-STICK/828-1045-ND/6136288)が売っているのですが、今回は安さや実装性を求めてGY-955を使います。
GY-955はモジュールになっていて、さらにパッドがハーフカットになっているので簡単に基板上に付けれます。また10~15$程度と安価に購入できます。しかし、GY-955の回路図や素性は不明な点が多く、情報が少ないのが現状です。

## 外観
gy-955は以下のような基板の片面で実装されています。10ピンのパッドがあります。

![hard_gy955_outlook.png](https://qiita-image-store.s3.amazonaws.com/0/254442/c609291f-ad25-2454-9969-b79ff59b7afd.png)

## 接続
今回はUARTで接続します。必要なのはVCC、GND、TX、RXの4本だけです。ほかのピンは接続しません。電源は3.3Vと5Vの両対応です。
[秋月のUSBシリアル変換ケーブル](http://akizukidenshi.com/catalog/g/gM-05841/)のようなUSBシリアル変換をかませます。

| GY-955 | <-> | USBシリアル |
|:-:|:-:|:-:|
| VCC | <-> | VCC |
| TX  | <-> | RX  |
| RX  | <-> | TX  |
| GND | <-> | GND |
| SI  |     | NC |
| SR  |     | NC |
| REST|     | NC |
| AD0 |     | NC |
| SDA |     | NC |
| SCL |     | NC |

ちなみに僕はこんな感じで専用のUSB変換基板でつなげています。
![hard_gy955_connect.JPG](https://qiita-image-store.s3.amazonaws.com/0/254442/8297fe2d-d23f-d1bf-db90-0876cb3a8727.jpeg)


## 通信プロトコル
UARTで、初期設定では9600bpsになっています。

### 送信側
3バイトの固定です。

| byte | 0 | 1 | 2 |
|:-:|:-:|:-:|:-:|
| 説明 | 170固定 | 転送データ選択 | 10で固定? |

「転送データ選択」はGY-955からどのようなデータが欲しいかを選択します。

* bit0: 加速度計
* bit1: 地磁気計？
* bit2: 角速度計
* bit3: オイラー角
* bit4: クオータニオン

今回はROSの姿勢に使うのでbit4のクオータニオンのみ使います。よって送るデータは「170, 16,10」になります。
一度送ると1秒ほど遅れて指定してデータで情報が受信できます。
（ちなみに初期設定ではオイラー角とクオータニオンの両方が選ばれた24になっていました。）

### 受信側
特に設定しなくてもGY-955は順次データを送ってきます。「転送データ選択」で送った内容によって変わりますがここではクオータニオンのみ（16）を前提に書きます。
（電源ON時は24なのでもう少し余計なデータが乗ります。）

| byte | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 
|:-:|:-:|:-:|:-:|:-:|:-:|:-:|:-:|:-:|:-:|:-:|:-:|:-:|:-:|:-:|
| 説明 | 固定 | 固定 | 転送データ選択 | バイト数 | w上位 | w下位 | x上位 | x下位 | y上位 | y下位 | z上位 | z下位 | ステータス1 | ステータス2 |
| 値   | 90  | 90   | 16 | 9 | | | | | | | | | | |

例えばwの値だと

```math
w_{tmp}=((w上位) \times 256 + (w下位))\\
(wの値)= \left\{
\begin{array}{ll}
\frac{w_{tmp}}{10000} & (0<=w_{tmp}<32768) \\
\frac{(w_{tmp}-65536)}{10000} & (else)
\end{array}
\right.
```

のようになります。
ROSのクオータニオンの並び(x,y,z,w)ではない並び（w,x,y,z）になっていることに注意です。ちなみにx軸はだいたい東を向きます。

# ソースコード

## GY-955接続ノード
GY-955が/dev/ttyUSB0に接続していることを前提に書いています。

```c++:hard_lecture/src/hard_gy955.cpp
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"

#include <string>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>

int open_serial(const char *device_name){
	int fd1=open(device_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
	fcntl(fd1, F_SETFL,0);
	//load configuration
	struct termios conf_tio;
	tcgetattr(fd1,&conf_tio);
	//set baudrate
	speed_t BAUDRATE = B9600;
	cfsetispeed(&conf_tio, BAUDRATE);
	cfsetospeed(&conf_tio, BAUDRATE);
	//non canonical, non echo back
	conf_tio.c_lflag &= ~(ECHO | ICANON);
	//non blocking
	conf_tio.c_cc[VMIN]=0;
	conf_tio.c_cc[VTIME]=0;
	//store configuration
	tcsetattr(fd1,TCSANOW,&conf_tio);
	return fd1;
}

int fd1;
int main(int argc, char **argv)
{
	ros::init(argc, argv, "hard_gy955");
	ros::NodeHandle n;

	//Publisher
	ros::Publisher imu_pub  = n.advertise<sensor_msgs::Imu >("imu", 10);
	
	char device_name[]="/dev/ttyUSB0";
	fd1=open_serial(device_name);
	if(fd1<0){
		ROS_ERROR("Serial Fail: cound not open %s", device_name);
		printf("Serial Fail\n");
		ros::shutdown();
	}

	char send_data[3]={170,16,10};
	int rec=write(fd1,send_data,3);
	if(rec==0){
		ROS_ERROR_ONCE("Serial Fail: cound not write");
	}

	ros::Rate loop_rate(100); 
	static unsigned char buffer_data[256]={0};
	static int buffer_size=0;
	while (ros::ok()){
		unsigned char recv_data[256]={0};
		int recv_size=read(fd1, recv_data, sizeof(recv_data));
		if(recv_size>0){
			//add
			for(int i=0;i<recv_size;i++){
				buffer_data[buffer_size+i]=recv_data[i];
			}
			buffer_size+=recv_size;

			//dencode
			int state=0;//0:serch 1st 90, 1:serch 2nd 90, 2:check size, 3:end
			int next_index=0;
			for(int i=0;i<buffer_size;i++){
				if(state==0 || state==1){
					if(buffer_data[i]==90)state++;	
				}
				else if(state==2){
					if(i+11<buffer_size){
						int mode=buffer_data[i+0];
						int size=buffer_data[i+1];
						int16_t tmp_w=(buffer_data[i+2]*256+buffer_data[i+3]);
						float quat_w=tmp_w/10000.0;
						int16_t tmp_x=(buffer_data[i+4]*256+buffer_data[i+5]);
						float quat_x=tmp_x/10000.0;
						int16_t tmp_y=(buffer_data[i+6]*256+buffer_data[i+7]);
						float quat_y=tmp_y/10000.0;
						int16_t tmp_z=(buffer_data[i+8]*256+buffer_data[i+9]);
						float quat_z=tmp_z/10000.0;
						state++;
						next_index=i+12;
						float quat_size=quat_w * quat_w + quat_x * quat_x + quat_y * quat_y + quat_z * quat_z;
						if(0.9<quat_size && quat_size<1.1){
							//publish
							sensor_msgs::Imu imu_msg;
							imu_msg.header.frame_id="map";
							imu_msg.header.stamp=ros::Time::now();
							imu_msg.orientation.x=quat_x;
							imu_msg.orientation.y=quat_y;
							imu_msg.orientation.z=quat_z;
							imu_msg.orientation.w=quat_w;
							imu_pub.publish(imu_msg);
						}
						else{
							ROS_WARN("quat size error: %f",quat_size);
						}
					}
				}
			}
			//sub
			if(next_index>0){
				for(int i=0;i<buffer_size-next_index;i++){
					buffer_data[i]=buffer_data[i+next_index];
				}
				buffer_size-=next_index;
			}
		}
		ros::spinOnce();
		loop_rate.sleep();
	} 
 	return 0;
}
```

## imu->pose stamped変換ノード
imuのorientationを表示するために一旦pose stampedに変換します

```c++:hard_lecture/src/hard_imu_to_pose.cpp
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/PoseStamped.h"

ros::Publisher posestamped_pub;
void imu_callback(const sensor_msgs::Imu& imu_msg){
	geometry_msgs::PoseStamped ps_msg;
	ps_msg.header=imu_msg.header;
	ps_msg.pose.orientation=imu_msg.orientation;
	posestamped_pub.publish(ps_msg);	
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "hard_imu_to_pose");
	ros::NodeHandle n;
	//Publisher
	posestamped_pub = n.advertise<geometry_msgs::PoseStamped >("imu_pose", 10);
	//Subscriber
	ros::Subscriber serial_sub = n.subscribe("imu", 10, imu_callback); 
	ros::spin();
}
```

## launch

```xml:hard_lecture/launch/gy955.launch
<launch>
  <arg name="rvizconfig" default="$(find hard_lecture)/rviz/gy955.rviz" />
  <node name="hard_gy955" pkg="hard_lecture" type="hard_gy955" output="screen" />
  <node name="hard_imu_to_pose" pkg="hard_lecture" type="hard_imu_to_pose" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
```

# ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# 実行

```shell
roslaunch hard_lecture gy955.launch 
```

GY-955センサーを動かすとこのように表示のフレームが動きます。imuのデータは大体42Hzで出ています。
![hard_gy955.gif](https://qiita-image-store.s3.amazonaws.com/0/254442/8f280644-15a6-3274-e2d7-6eb65b59020b.gif)

# 参考
[Aliexpress GY-955](https://ja.aliexpress.com/item/WAVGAT-AHRS-GY-955-High-Stability-Calman-Filter-Sensor-Nine-Axis-Navigation-Module-BNO-055/32823250757.html)
[GY-955 モジュール使い方](https://www.elektroda.pl/rtvforum/topic3458413.html)
[GY-955 モジュール使い方](https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=6&cad=rja&uact=8&ved=2ahUKEwjw0-WS8M7eAhXOEXAKHTK9CAsQFjAFegQICRAC&url=https%3A%2F%2Fforum.arduino.cc%2Findex.php%3Faction%3Ddlattach%3Btopic%3D576726.0%3Battach%3D280049&usg=AOvVaw3Dk1IggJCiC4tV02RbJXGk)
[BNO055 datasheet](https://ae-bst.resource.bosch.com/media/_tech/media/datasheets/BST_BNO055_DS000_14.pdf)

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