環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

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

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

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

GY-955について

GY-955はモジュールの名前でIMUはBNO055を使っています。BNO055をUSB変換と合わせたモジュールが売っているのですが、今回は安さや実装性を求めてGY-955を使います。
GY-955はモジュールになっていて、さらにパッドがハーフカットになっているので簡単に基板上に付けれます。また10~15$程度と安価に購入できます。しかし、GY-955の回路図や素性は不明な点が多く、情報が少ないのが現状です。

外観

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

接続

今回はUARTで接続します。必要なのはVCC、GND、TX、RXの4本だけです。ほかのピンは接続しません。電源は3.3Vと5Vの両対応です。
秋月のUSBシリアル変換ケーブルのような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変換基板でつなげています。

通信プロトコル

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の値だと

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に接続していることを前提に書いています。

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に変換します

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

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>

ビルド

cd ~/catkin_ws
catkin_make

実行

roslaunch hard_lecture gy955.launch

GY-955センサーを動かすとこのように表示のフレームが動きます。imuのデータは大体42Hzで出ています。

参考

Aliexpress GY-955
GY-955 モジュール使い方
 GY-955 モジュール使い方
 BNO055 datasheet

目次ページへのリンク

ROS講座の目次へのリンク