ROS Subcriber와 Publish 실습: OpenCR IMU 테스트

2017년도 굿바이를 할 때인가 봅니다. 올해초 그렇게 ROS를 정말 열심히 해보겠다는 계획은 데이터 사이언스[바로가기] 관련된 글들에 집중하고, 또 데이터 사이언스 관련 책을 출판[바로가기]하는 일과 또 이제 막 시작하는 블럭코딩[바로가기] 관련일들로 또 약간 후순위로 변경되고 말았네요. 그래도 오픈로보틱스 세미나에서 엄청 부족하지만, 발표[바로가기]도 하는 경험을 가졌습니다. 그래서 올해가 가지전에 그래도 저처럼 ROS를 독학하시는 분들께 뭔가 하나는 도움을 드려보자는 생각에 지난번에 제가 Turtlebot3[바로가기]를 얻어서 신나게 테스트하는 것[바로가기]에서 살짝 더 발전시켜 보기로 했습니다.

당시 제가 테스트(라고 쓰고 따라하기)했던 것은, Turtlebot3에 탑재된 OpenCR보드에 ROS에서 사용할 수 있는 IMU코드를 넣고 ROS에서 이를 관찰하던 것[바로가기]인데요. 이 글은 ROS 로봇 프로그래밍이라는 책의 내용을 따라한 것이었죠.

• 오늘은 거기서 조금 더 나아가서 제가 직접 Subscriber를 만들어서 IMU 데이터를 직접 받아오는 것에서
• 쿼터니언 표현으로 받은 데이터를 오일러각으로 변환하는 것과
• 오일러각으로 변환된 값을 다시 Publish해서 그래프로 관찰해보는 것

을 수행해 보려고 합니다. 약간 생소할 수 있는 ROS의 방식을 따라가면 진도는 잘 나오는데 혼자 뭘 할려고 하면 살짝꿍 막히기도 하죠. 오늘은 그걸 슬쩍 한 번 극복해보는 의도도 가지고 있습니다.^^. 일단 오늘은 OpenCR보드에 IMU 소스를 올리고 ROS에서 실행해보던 실습[바로가기]의 글 내용은 모두 실습되었다고 보고 이어서 시작되는 글입니다. 

실습을 위해 준비하기....

일단 시작하기 전에 패키지 하나를 만들어 둡니다. 먼저

cd ~/catkin_ws/src

catkin의 src폴더에서

catkin_create_pkg simple_subscriber_imu rospy

simple_subscriber_imu라는 이름의 패키지를 만들고 rospy를 의존성으로 지정해 줍니다.

cd ~/catkin_ws/src/simple_subscriber_imu

그리고 방금 만든 패키지 폴더로 이동해서 

mkdir scripts && cd scripts

Python 코드가 위치할 scripts라는 폴더를 만들고 해당 폴더로 이동합니다. 해당 폴더에서 저는 소개한 적이 있는 sublime text를 사용하기 때문에.. 파일을 만들때는 

subl <filename>.py

라는 명령으로 파일을 만들 것입니다. 그렇게 만들어진 파일은

chmod u+x <filename>.py

처럼 chmod로 바로 실행될 수 있는 권한을 주어야 합니다. 실행권한이 부여된 python 파일은 터미널에서 ls로 목록을 보면 녹색으로 나타납니다.

오늘 만들 python 파일은 세 개입니다. 미리 빈 파일을 만들어 두고, 권한(chmod)도 다 두면 좋습니다.^^. 아~ 혹시 탭이나 뭐 이런게 잘 안먹으면 rospack profile명령을 쓸쩍 입력해 주면 됩니다. 이제...

일단, roscore를 실행해 주시구요~~

위 그림처럼

rosrun rosserial_python serial_node.py __name:=opencr _port:=/dev/ttyACM0 _baud:=115200

를 실행해 줍니다.

rostopic list

그리고, 실행된 topic 리스트를 확인해보면,

위 그림처럼 imu 토픽이 있는 걸 확인할 수 있습니다.

첫 도전!!! 실행되는 토픽에서 메세지를 받아보자 Subcriber 작성

이제 첫 번째 도전은 방금 위에서 본 imu라는 토픽의 메세지를 받아오겠다는 것입니다. [바로가기]에서도 이야기했지만, 그냥 rospic echo 명령으로도 읽어볼 수 있습니다만, 오늘은 subscriber를 연습하는 거니까요~~ 먼저 코드를 작성하죠~~ 엄청 쉽습니다.^^

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Imu

def callback(msg):
	print "Quaternoion ================"
	print "x : ", msg.orientation.x
	print "y : ", msg.orientation.y
	print "z : ", msg.orientation.z
	print "w : ", msg.orientation.w

rospy.init_node('get_imu_data')

sub = rospy.Subscriber('imu', Imu, callback)

rospy.spin()

짧죠???^^ Python 코드를 사용할때는 첫 줄에 #!로 시작하는 위의 코드가 한 줄 들어가야 합니다. 그리고, Imu 메세지를 import하고, Imu 메세지에서 orientation만 읽어서 프린트하는 것입니다. 장난 아닌 깔끔한 코드입니다.^^ (당연하죠 짧은데^^)

그리고, 위 그림처럼 rosrun 명령으로 실행해 주면 됩디다.

그리고 로봇을 제가 손으로 막 흔드는 거죠!!! 

그러면 위 동영상처럼 각도 정보가 나타나는 것을 알 수 있습니다. 그러나 자세히 보면 출력되는 값은 Yaw, Pitch, Roll이 아니라 값이 네 개입니다. 네 개?? 넵 Qauternion 값입니다. 물체의 회전을 해석할때 익숙한 Euler 표현보다 더 유용하다고 합니다. 그런데, 이 아이~~ 우리가 눈으로 볼때는 불편하네요.. 한 단계 더 나가보죠...

두번째 도전!!! 쿼터니언 표현을 오일러로 바꿔서 출력해보기

넵~ 쿼터니언 표현을 오일러 각으로 바꿔서 출력하려고 합니다. 이론적 배견은 너무 힘드니까.. 위키백과의 관련항목[바로가기]을 가면 변환 식이 나타나 있고, 이를 코드로도 예제를 보여줍니다. 언젠가 이야기한 적이 있는데, 전 꼭 돈 많이 벌면 위키백과이 기부해야겠다고 다짐하면서 지금은 그냥 가져다 씁니다ㅠㅠ.

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Imu
import math

def quaternion_to_euler_angle(msg):
	x = msg.x
	y = msg.y
	z = msg.z
	w = msg.w

	ysqr = y * y
	
	t0 = +2.0 * (w * x + y * z)
	t1 = +1.0 - 2.0 * (x * x + ysqr)
	X = math.degrees(math.atan2(t0, t1))
	
	t2 = +2.0 * (w * y - z * x)
	t2 = +1.0 if t2 > +1.0 else t2
	t2 = -1.0 if t2 < -1.0 else t2
	Y = math.degrees(math.asin(t2))
	
	t3 = +2.0 * (w * z + x * y)
	t4 = +1.0 - 2.0 * (ysqr + z * z)
	Z = math.degrees(math.atan2(t3, t4))
	
	return X, Y, Z

def callback(msg):
	X, Y, Z = quaternion_to_euler_angle(msg.orientation)
	print "Quaternoion ================"
	print "x : ", msg.orientation.x
	print "y : ", msg.orientation.y
	print "z : ", msg.orientation.z
	print "w : ", msg.orientation.w
	print "Euler -----------------------"
	print "X : ", X
	print "Y : ", Y
	print "Z : ", Z

rospy.init_node('get_imu_data')

sub = rospy.Subscriber('imu', Imu, callback)

rospy.spin()

복잡하진 않죠? 그저 위키백과에서 얻은 변환하는 식만 더 추가했습니다. 아 결과를 degree로 표현하는군요~~ 역시 실행결과를 보면

넵~~~ 이렇습니다.^^ 그런데.. 각도의 표현이 그냥 텍스트로 나오니 좀 슬프네요~~~ 그래프로 표현해보고 싶네요~~~ 다음 단계로~~~~

마지막 단계!!! Publish해서 그래프(rqt_plot)로 확인해보자

그래프로 topic을 확인하는 가장 쉬운 도구는 rqt_plot입니다. 그걸 사용도 할 겸, subscriber 코드와 publish하는 것도 같이 학습할 겸, 학습을 좀 더 진행해 보겠습니다.

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25

#!/usr/bin/env python import rospy from sensor_msgs.msg import Imu from std_msgs.msg import Float32 from tf.transformations import euler_from_quaternion def callback(incomming_msg): list_orientation = [incomming_msg.orientation.x, incomming_msg.orientation.y, incomming_msg.orientation.z, incomming_msg.orientation.w] roll, pitch, yaw = euler_from_quaternion(list_orientation) pub_imu_roll.publish(roll) pub_imu_pitch.publish(pitch) pub_imu_yaw.publish(yaw) rospy.init_node('imu_sub_and_pub_test') pub_imu_roll = rospy.Publisher('IMU_Roll', Float32, queue_size=10) pub_imu_pitch = rospy.Publisher('IMU_Pitch', Float32, queue_size=10) pub_imu_yaw = rospy.Publisher('IMU_Yaw', Float32, queue_size=10) sub = rospy.Subscriber('imu', Imu, callback) rospy.spin()

어라.. 두번째 단계에서 위키가 준 함수 따라한 변환 코드가 사라졌네요??? 이유는 사실.. ROS도 당연히 변환하는 기능이 있기 때문입니다.^^. 6번줄에서 import한 tf.transformations의 euler_from_quaternion함수가 그 일을 해줍니다. 그래서 11번 줄처럼 그저 사용한거죠. 그리고, 하나의 array로 만들어도 되는게 그냥 roll, pitch, yaw를 따로 19,20,21번줄처럼 Publisher로 지정했습니다. 그리고, Subcriber callback함수 안에서 오일러각으로 변환하고 publish 시킨거죠^^

그리고, rqt_plot을 실행해 줍니다.~~ 그러면~~

이렇게 내가 Turtlebot3를 쥐고 회전시킬때의 결과가 나타나는거죠^^ 아... 두번째 단계는 degree였는데, 지금은 제가 따로 뭘 안해서 그냥 radian값입니다.

오늘은 Turtlebot3에 있는 OpenCR보드의 IMU를 가지고, 슬쩍 ROS 학습삼아 이것 저것 손을 대 보았습니다. Subscriber도 직접 만들어보고, 또 Quaternion을 Euler각으로 변환도 해보고, 다시 Publish도 시켜보는 과정이었죠~~~ 아.. 즐거운 연말 연시 되세요^^