MPU6050を用いた姿勢推定＆Rvizで可視化

# はじめに
最近、研究室に転がっていたRaspberry Piを使って遊んでいます。  
今回は、ワンコインで買える6軸センサであるMPU6050を使って姿勢の推定を行い、その結果をRvizで可視化できるようにしました。

MPU6050
![71RF2BSPAGL._SL1500_.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/246881/ee4d244d-eae8-4d48-c411-713e4b8b5de0.jpeg)
# 実行環境
- [Raspberry Pi 3 Model B](https://www.amazon.co.jp/RASPBERRY-PI-TSI-PI018-Clear-Raspberry-Pi3-Model/dp/B01D1FR29M)
- [MPU6050](https://www.amazon.co.jp/gp/product/B008BOPN40/ref=ppx_yo_dt_b_asin_title_o00_s00?ie=UTF8&psc=1)
- ROS : Kinetic
- Python : 2.7
- Host PC : Ubuntu16.04


# Raspberry Pi側の設定
Raspberry PiにROSをインストールする方法は、[こちら](https://qiita.com/atotto/items/9cf58a847de062f9ab2a)の記事を参考にさせていただきました。

## mpu6050との接続
Raspberry Piの各ピンをMPU6050の以下の部分に繋げてください。  
- 1番ピン → VCC
- 3番ピン → SDA
- 5番ピン → SCL
- 6番ピン → GND  

きちんと接続できれば、LEDが点灯します。

## ROSのプログラム
MPU6050から得られたx, y, z方向の角速度の値から、オイラー角を計算し、  
その値をTFにブロードキャストします。

```mpu6050.py
#! /usr/bin/env python
# coding:utf-8
import smbus
import math
import time
import rospy, tf
from sensor_msgs.msg import *
from geometry_msgs.msg import *

#アドレスの設定
DEV_ADDR = 0x68
ACCEL_XOUT = 0x3b
ACCEL_YOUT = 0x3d
ACCEL_ZOUT = 0x3f
PWR_MGMT_1 = 0x6b
PWR_MGMT_2 = 0x6c

bus = smbus.SMBus(1)
bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0)

def read_word(adr):
	high = bus.read_byte_data(DEV_ADDR, adr)
	low = bus.read_byte_data(DEV_ADDR, adr+1)
	val = (high << 8) + low
	return val

def read_word_sensor(adr):
	val = read_word(adr)
	if (val >= 0x8000):#minus
		return -((65535 - val) + 1)
	else:#plus
		return val

def getAccel():
	x = read_word_sensor(ACCEL_XOUT)/16384.0
	y = read_word_sensor(ACCEL_YOUT)/16384.0
	z = read_word_sensor(ACCEL_ZOUT)/16384.0
    return [x, y, z]

def calcEuler(x,y,z):
	theta = math.atan( x / math.sqrt(y*y + z*z) )
	psi = math.atan( y / math.sqrt(x*x + z*z) )
	phi = math.atan( math.sqrt( x*x + y*y ) / z)
	return [theta, psi, phi]

if __name__ == "__main__":
	rospy.init_node("mpu6050")
	br = tf.TransformBroadcaster()

	while True:
		try:
			ax, ay, az = getAccel()

			roll, pitch, yaw = calcEuler(ax,ay,az)

			br.sendTransform((0,0,0), tf.transformations.quaternion_from_euler(roll,pitch,yaw), rospy.Time.now(), "mpu6050", "map")

			#print "Roll:[%s]"%str(roll)
			#print "Pitch:[%s]"%str(pitch)
			#print "Yaw:[%s]"%str(yaw)

		except KeyboardInterrupt:
			print "Ctrl + c"
			break

		except Exception as e:
			print str(e)
			#break
```
適当なパッケージを作って、上記のファイルをコピペするか、
[Github](https://github.com/KoutaOhishi/mpu6050)から直接パッケージをcloneしてください。


# ROSの分散処理の設定
Raspberry PiだとRvizで実行結果を見ることができません。  
そこで、別のPCをマスターとしてroscoreを実行し、RvizもそのPCで起動します。
ROSの分散処理については、[こちら](https://qiita.com/srs/items/7d4aeb5e44138f97c770)の記事が参考になりました。

## Raspberry Pi と Host PC 共通の設定
LANケーブルで接続し、`ping`や`ifconfig`コマンドを使って、接続の確認とIPアドレスの確認をしてください。

## Host PC側の設定
端末を立ち上げて、
  
```.terminal
export ROS_IP=XXX.YYY.ZZZ.WWW  #"XXX.YYY.ZZZ.WWW"はHost PCのIPアドレス
roscore
```
を実行してください。
端末を切ると、設定がリセットされるのでご注意を。  
毎回設定するのが面倒な場合は`.bashrc`に書いておいてください。

## Raspberry Pi側の設定
端末を立ち上げて、  

```.terminal
export ROS_MASTER_URI=http://XXX.YYY.ZZZ.WWW:11311  #"XXX.YYY.ZZZ.WWW"はHost PCのIPアドレス
export ROS_IP=aaa.bbb.ccc.ddd  #"aaa.bbb.ccc.ddd"はRaspberry PiのIPアドレス
```
を実行してください。
端末を切ると、設定がリセットされるのでご注意を。  
毎回設定するのが面倒な場合は`.bashrc`に書いておいてください。


# 実行方法
- **Host PC側**

  ```
  rviz
  ```

  別の端末で、

  ```
  rosrun tf static_transform_publisher 0 0 0 0 0 0 map frame 100
  ```

- **Raspberry Pi側** 
 
  ```
  rosrun {package_name} mpu6050.py
  ```

Rvizが初期状態だと、何も表示されないと思います。
`Add`を押して、`By display type`の中から`TF`を選んでください。

# 実行結果
[Twitter](https://twitter.com/Kouta_Ohishi/status/1118323503936098305)

うまく姿勢の推定ができてるっぽいですが、誤差や振動が大きいです。
カルマンフィルタ等を使って補正する必要があります。