자이카 - IMU센서

자이카와 IMU

• 자이카에서 IMU패키지는 ~/xycar_ws/src/xycar_device/xycar_imu에 위치
• /imu토픽을 이용
  • 헤더 - uint32 seq, time stamp, string frame_id
  • 쿼터니언 정보 - 쿼터니언, 그러니까 기울기 정보를 담은 x, y, z ,w 숫자

자이카 IMU센서 활용

Roll, Pitch, Yaw 값을 추출해 출력해 보자

• my_imu패키지 생성

catkin_create_pkg my_imu std_msgs rospy

• my_imu패키지 구성
  • 소스 파일 - roll_pitch_yaw.py
  • 런치 파일 - roll_pitch_yaw.launch

# roll_pitch_yaw.py
#!/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('Rool:%.4f, Pitch:%.4f, Yaw:%.4f' % (roll, pitch, yaw))

	time.sleep(1.0)

# roll_pitch_yaw.launch
<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>

# 패키지 실행
roslaunch my_imu roll_pitch_yaw.launch

• 자이카가 없어 결과를 확인할 수 없지만,
  • 주어진 예시를 보면 다음 결과를 예상할 수 있다.

IMU 센싱 데이터 시각화

IMU 데이터 시각화 플러그인 설치

• ~/xycar_ws/src 폴더에 압축 풀기
• cm명령어로 빌드

RVIZ에서 IMU 센서 데이터 시각화

rviz_imu 패키지 만들기

• 위치는 ~/xycar_ws/src

catkin_create_pkg rviz_imu rospy tf geometry_msgs urdf rviz xacro

• 패키지 구성
  • ~/xycar_ws/src/rviz_imu/rviz/imu_3d.rviz
  • ~/xycar_ws/src/rviz_imu/launch/imu_3d.launch

# imu_3d.launch
<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>

# 실행
roslaunch rviz_imu imu_3d.launch

• 아무것도 안 나올 때는
  • Add→Imu
  • Imu→Topic=/imu
  • Box properties→IMU데이터를 시각화 하기 위해 육면체를 출력함
  • Axes properties→IMU데이터를 시각화 하기 위해 축을 출력함

IMU 데이터를 받아 박스를 움직여 보자

• 실제 IMU데이터를 받은 imu_data.txt파일을 읽고 RVIZ에 표현해 보자.

• 패키지 구성

  • 기존 rviz_imu 패키지에
    - 런치 파일 - imu_generator.launch
    - 소스 파일 - imu_generator.py, imu_data.txt
    - Rviz파일 - imu_generator.rviz
    - 참고로, .rviz파일은 타이핑치는 것이 아님

    # imu_generator.launch
    <launch>
    	<!-- rviz display -->
    	<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>

    #imu_generator.py
    #!/usr/bin/env python
    
    import rospy, math, os, rospkg
    from sensor_msgs.msg import Imu
    
    from tf.transformations import quaternion_from_euler
    
    degrees2rad = float(math.pi)/float(180.0)
    rad2degrees = float(180.0)/float(math.pi)
    
    rospy.init_node("imu_generator")![](https://velog.velcdn.com/images/mixergim/post/f937d0fc-5ec7-4116-a310-af119bb8bc92/image.gif)
    
    
    pub = rospy.Publisher('imu', Imu, queue_size=1)
    
    data = []
    
    path = rospkg.RosPack().get_path('rviz_imu')+"/src/imu_data.txt"
    f = file(path,"r")
    lines = f.readlines()
    
    for line in lines:
    	tmp = line.split(",")
    	extract = []
    	for i in tmp:
    		extract.append(float(i.split(":")[1]))
    	data.append(extract)
    
    imuMsg = Imu()
    imuMsg.header.frame_id = 'map'
    
    r = rospy.Rate(10)
    seq = 0
    
    for j in range(len(data)):
    	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.x = msg_data[2]
    	imuMsg.orientation.w = msg_data[3]
    
    	imuMsg.header.stamp = rospy.Time.now()
    	imuMsg.header.seq = seq
    	seq = seq+1
    
    	pub.publish(imuMsg)
    	r.sleep()

    # 실행
    roslaunch rviz_imu imu_generator.launch
    
    # 토픽 확인
    rostopic echo /imu
    # Output
    ---
    header: 
      seq: 440
      stamp: 
        secs: 1696914327
        nsecs: 125488996
      frame_id: "map"
    orientation: 
      x: 0.127963726583
      y: 0.202621813128
      z: 0.0
      w: 0.100058729798
    orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    angular_velocity: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    linear_acceleration: 
      x: 0.0
      y: 0.0
      z: 0.0
    linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    ---

    

    잘 구른다.