<launch>
<node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" args="-d $(find rviz_imu)/rviz/imu_generator.rviz" />
<node name="imu_generator pkg="rviz_imu" type="imu_generator.py" />
</launch>
#!/usr/bin/env python
import rospy, math, os, rospkg
from sensor_msgs.msg import Imu
from tf.transformations import quaternion_from_euler # quaternion_from_euler 사용하기 위해 import
degree2rad = float(math.pi) / float(180.0)
rad2degrees = float(180.0) / float(math.pi) # 라디안 / 각도 변환 계산식
rospy.init_node("imu_generator") # 노드 이름 정하고 생성
pub = rospy.Publisher('imu', Imu, queue_size=1) # Imu 타입의 데이터를 담아 /imu 이름의 토픽으로 발행하겠다고 정의
data = []
path = rospkg.RosPack().get_path('rviz_imu')+"/src/imu_data.txt"
f = file(path, "r")
lines = f.readlines() # imu_data.txt 파일을 찾아서 한 줄씩 읽기
for line in lines:
temp = line.split(",")
extract = []
for i in temp:
extract.append(float(i.split(":")[1]))
data.append(extract) # Roll, Pitch, Yaw 숫자 값만 추출
imuMsg = Imu() # Imu 타입으로 imuMsg 만들기
imuMsg.header.frame_id = 'map' # imuMsg.header.frame_id 값에 'map' 넣기 (값 하나씩 채워넣기)
r = rospy.Rate(10) # 1초에 10번씩
seq = 0
for j in range(len(data)): # imu_data.txt 파일의 Line 수 만큼 루프 돌기
msg_data = quaternion_from_euler(data[j][0], data[j][1], data[j][2])
imuMsg.orientation.x = msg_data[0]
imuMsg.orientation.y = msg_data[1]
imuMsg.orientation.z = msg_data[2]
imuMsg.orientation.w = msg_data[3] # 오일러(롤,피치,요) 데이터를 쿼터니언(x,y,z,w) 으로 변환
imuMsg.header.stamp = rospy.Time.now() # stamp에 시간 정보 넣기
imuMsg.header.seq = seq # seq에 순차 넘버 넣기
seq += 1
pub.publish(imuMsg) # /imu 토픽 발행 (1초에 10번씩)
r.sleep()
