IMU

Ⅰ. 9축 IMU 센서

0. 중력 가속도 감지
1. 직선 방향 가속도 감지
2. 회전 속도 감지
3. 동서남북 방위각 감지

Ⅱ. RPY와 Querternion

물체의 orientation을 표기하는 방법

1. roll, pitch, yaw

• 오일러 표기법, 인간이 인지하기 쉬운 표기법이나 계산비용이 크다

2. querternion

• 인간이 인지하기는 어려우나 컴퓨터가 계산하기 편한 표기법

3. 변환

#!/usr/bin/env python

from tf.transformations import euler_from_quaternion, quaternion_from_euler

q = quaternion_from_euler(1, 1, 1)
print(q)
e = euler_from_quaternion(q)
print(e)

jsg@jsg-ubuntu:~/xycar_ws/src/my_imu/src$ python transform.py 
[ 0.16751879  0.57094147  0.16751879  0.78606663]
(1.0000000000000004, 1.0, 1.0000000000000007)

Ⅲ. IMU 센서가 발행하는 msg

• sensor_msgs/Imu

jsg@jsg-ubuntu:~$ rosmsg show sensor_msgs/Imu
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Quaternion orientation
  float64 x
  float64 y
  float64 z
  float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
  float64 x
  float64 y
  float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
  float64 x
  float64 y
  float64 z
float64[9] linear_acceleration_covariance

Ⅳ. IMU 센서 rpy로 출력

#!/usr/bin/env python
import rospy
import time

from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion

Imu_msg = None

def imu_callback(data):
    global Imu_msg
    Imu_msg = [data.orientation.x, data.orientation.y,
    	       data.orientation.z, data.orientation.w] 

rospy.init_node("Imu_Print")
rospy.Subscriber("imu", Imu, imu_callback)

while not rospy.is_shutdown():
    if Imu_msg == None:
        continue

    (roll, pitch, yaw) = euler_from_quaternion(Imu_msg)
   
    print('Roll:%.4f, Pitch:%.4f, Yaw:%.4f' % (roll, pitch, yaw))
   
    time.sleep(0.5)

<launch>
  <node pkg="xycar_imu" type="9dof_imu_node.py" name="xycar_imu" output="screen">
    <param name="rviz_mode" type="string" value="false" />
  </node>
  <node pkg="my_imu" type="roll_pitch_yaw.py" name="Imu_Print" output="screen" />
</launch>

Ⅴ. IMU 데이터 시각화

• rviz_imu_plugin 설치 : http://wiki.ros.org/rviz_imu_plugin

• launch파일에 rviz 추가

<launch>
  <!-- rviz display -->
  <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" 
        args="-d $(find rviz_imu)/rviz/imu_3d.rviz"/>
  <node pkg="xycar_imu" type="9dof_imu_node.py" name="xycar_imu" output="screen">
    <param name="rviz_mode" type="string" value="false" />
  </node>
</launch>

• Rviz 에서 add 누른 후 imu 추가

• 옵션 설정

• 결과