環境
この記事は以下の環境で動いています。
| 項目 | 値 |
|---|---|
| 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に接続していることを前提に書いています。
#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に変換します
#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
<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