물건 가져와주는 드론 제작과정 (2019.12.23 ~ 2020.1.20)
군산대 X-optimus 프로그램에서 드론 소프트웨어 제작하는 수업을 받았습니다
수업과정은 4주였고, 첫 주는 드론운용 기초과정, 2주차에는 ROS(Robot Operating System)에 대해 수업을 받았고
3~4주차에는 배운 내용을 이용한 소규모 프로젝트를 진행했습니다.
같은 과 친구 2명과 함께 2주동안 제작하였습니다.
저희는 GPS와 AR마커를 활용하여 물건을 가져오는 드론을 만들기로 기획하였습니다
위는 시현동영상과 받은 상장입니다~
실행과정은 다음과 같습니다
1. 핸드폰 앱으로 버튼을 누르면 현재좌표와 목표지점좌표를 전송합니다.
2. 드론은 목표지점좌표에 가서 AR마커를 찾고 인식합니다.
3. 드론이 AR마커를 찾으면 정해진 알고리즘으로 물건을 집어옵니다.
4. 드론이 처음 앱에서 버튼을 눌렀던 "현재좌표"로 이동합니다.
Parrot 사의 Bebop2이라는 이름의 드론으로 프로젝트를 진행하는데, 이 드론을 코딩으로 제어하기 위해
ROS설치가 필요했습니다. 하지만 ROS는 mac과 linux만을 지원하기 때문에 기본 윈도우로 되어있는 학교 컴퓨터에
리눅스를 설치하여 멀티부팅이 가능하도록 세팅을 하였습니다.
ROS에서는 node, topic, subscriber, publisher, master 개념이 있는데
bobop드론의 각 센서에서 자신의 정보를 계속 발행하고 있고, 그 발행된 메시지를 topic이라 합니다.
저희는 이 topic메시지를 subscribe 하여서 각 센서들의 정보들을 받아왔고 이를 활용하여 코드를 작성했습니다.
그리고 이러한 값들을 조정하기 위해 새로운 topic을 발행하여 bobop에게 publish하였습니다.
일단 문제해결을 위해 우리가 해결해야 할 미션들을 나누어 코드를 작성하고 테스트를 하였습니다.
1. 어떠한 각도가 주어지면 그 만큼 회전하는 코드
2. 어떠한 길이가 주어지면 그 만큼 전진하는 코드
3. 어떠한 높이가 주어지면 그 만큼 상승하는 코드
4. AR마크를 인식하고 마크 가운데로 정렬하는 코드
5. GPS좌표로 이동하는 코드
1. 어떠한 각도가 주어지면 그 만큼 회전하는 코드
bebop2_rotate.py
|
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
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
|
#!/usr/bin/env python
import rospy
from math import radians
DEFAULT_ANG_SPEED = 0.25
TARGET_ANGLE = 0.0
pi = 3.1415926535
class Rotate:
def __init__(self):
rospy.init_node('move_anglar_z', anonymous=True)
self.tw = Twist()
self.Kp_ang = 1.0
def rotate(self, ang): # v = d / t, t = d / v
wise = 1.0
if( ang < 0 ):
wise = -1
else:
wise = 1
duration = abs(ang) / DEFAULT_ANG_SPEED
if __name__ == '__main__':
try:
target = float(input("input angle: "))
TARGET_ANGLE = radians(target) # deg to rad
r = Rotate()
r.rotate(TARGET_ANGLE)
except rospy.ROSInterruptException: pass
|
http://colorscripter.com/info# |
self.pub = rospy.Publisher('bebop/cmd_vel, Twist, queue_size = 1) 를 통해, 드론의 센서값을 바꾸었습니다.
bebop/cmd_vel은 cmd콘솔을 말하고, 2번째 인자인 Twist는 메시지의 타입을 의미합니다.
Twist는 드론의 x,y,z축의 각도나 선속도 정보를 담고있는 클래스입니다.
이 부분이 실제로 회전하는 부분입니다. 현재시간과 드론이 회전해야 할 시간을 비교하면서 계속 회전합니다.
self.pub.publish(self.tw)로 드론에게 tw이름의 객체정보를 전달합니다. (tw는 Twist클래스의 객체입니다)
|
1
2
3
4
|
duration = abs(ang) / DEFAULT_ANG_SPEED
|
http://colorscripter.com/info#e" |
이 부분이 실제로 계산한 부분인데, 라인4를 보면 tw객체의 z축 회전방향에 양수값을 대입하여
계속 publish 할 수 있도록 만들었습니다.
2. 어떠한 길이가 주어지면 그 만큼 전진하는 코드
bebop2_move_x.py
|
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
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
|
#!/usr/bin/env python
import rospy
DEFAULT_LIN_SPEED = 0.25
# 0.25 = default
TARGET_DIST = 0.0
class Move:
def __init__(self):
rospy.init_node('move_linear_x', anonymous=True)
self.tw = Twist()
def move_x(self, dist): # v = d / t, t = d / v
direction = 1.0
if( dist < 0 ):
direction = -1
else:
direction = 1
duration = abs(dist) / DEFAULT_LIN_SPEED
if __name__ == '__main__':
try:
TARGET_DIST = float(input("input distance to fly: "))
m = Move()
m.move_x(TARGET_DIST)
except rospy.ROSInterruptException: pass
|
http://colorscripter.com/info#e |
전진하는 코드도 회전할 때와 매우 유사합니다.
단지 self.tw.angular.z 가 아닌 self.tw.linear.y의 값을 바꾸어 publish하는 부분만 다릅니다.
3. 어떠한 높이가 주어지면 그 만큼 상승하는 코드
bebop2_ctrl_height.py
|
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
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
|
#!/usr/bin/env python
import rospy
pi = 3.1415926535
TARGET_ALTITUDE = 1.0
class ChangeAlt:
def __init__(self):
rospy.init_node('change_altitude', anonymous=True)
rospy.Subscriber("bebop/states/ardrone3/PilotingState/AltitudeChanged",
Ardrone3PilotingStateAltitudeChanged,
self.get_alt)
self.tw = Twist()
self.current_alt = 1.0
def get_alt(self,msg):
self.current_alt = msg.altitude
print("current height = %f" %(self.current_alt))
def change_alt(self, target):
if(target > self.current_alt):
while(target > self.current_alt):
elif(target < self.current_alt):
while(target < self.current_alt):
print "reached target height!"
if __name__ == '__main__':
try:
TARGET_ALTITUDE = float(input("input target height: "))
ca = ChangeAlt()
ca.change_alt(TARGET_ALTITUDE)
except rospy.ROSInterruptException: pass
|
http://colorscripter.com/info#e" target="_blank" style="text-decoration:none;color:white">cs |
rospy.Subscriber("bebop/states/ardrone3/PilotingState/AltitudeChanged", Ardrone3PilotingStateAltitudeChanged, self.get_alt) 이 부분은 드론이 가지고 있는 센서들의 정보들을 실시간으로 가져오기 위해 일종의 Event Listner를 부착하는 것이라고 이해했습니다. bebop/states/ardrone3/PilotingState/AltitudeChanged 라는 이름을 가진 토픽의 값을 계속확인하며 get_alt라는 함수를 호출합니다. (여기서 get_alt는 콜백함수로 호출됩니다)
|
1
2
3
|
def get_alt(self,msg):
self.current_alt = msg.altitude
print("current height = %f" %(self.current_alt))
|
http://colorscripter.com/info#e |
get_alt라는 이름을 가진 콜백함수를 보면, msg인자가 있는데, msg인자는 bebop/states/ardrone3/PilotingState/AltitudeChanged 토픽의 값을 클래스형태로 반환됩니다. 이 토픽에는 altitude라는 이름의 값이 있는데, 실시간으로 이 값을 class의 current_alt 멤버변수에 대입하여 실시간으로 current_alt 값을 업데이트 하였습니다.
|
1
2
3
4
5
6
7
8
9
10
11
|
if(target > self.current_alt):
while(target > self.current_alt):
elif(target < self.current_alt):
while(target < self.current_alt):
|
http://colorscripter.com/info#e |
이 부분이 실제로 드론이 상승하는 코드인데, self.tw.linear.z값을 0.5로 고정하고 현재높이가 목표높이보다 높은지 낮은지를 검사하여 음수, 양수를 결정하였고 마지막에 반복문으로 tw값을 publish 하였습니다.
4. 드론이 처음 앱에서 버튼을 눌렀던 "현재좌표"로 이동합니다.
GPS_turn.py
|
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
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
|
#! /usr/bin/env python
from py_lib.DistanceGPS import DistanceGPS
#from py_lib.GetGPSInfo import GetGPSInfo
import rospy
from math import radians, pi
DEFAULT_LIN_SPEED = 0.25
TARGET_LAT = 35.9440192
TARGET_LON = 126.6843312
class Goto:
def __init__(self):
rospy.init_node("get_gps", anonymous=10)
self.sub1 = rospy.Subscriber("bebop/states/ardrone3/GPSState/NumberOfSatelliteChanged", Ardrone3GPSStateNumberOfSatelliteChanged, self.cbSatelliteNumCheck, queue_size=1)
self.sub2 = rospy.Subscriber("bebop/states/ardrone3/PilotingState/PositionChanged", Ardrone3PilotingStatePositionChanged, self.cbFirstLocationSave, queue_size=1, buff_size=2**24)
self.sub3 = rospy.Subscriber("bebop/states/ardrone3/PilotingState/AttitudeChanged", Ardrone3PilotingStateAttitudeChanged, self.cbGetAzi, queue_size=1)
self.sub4 = rospy.Subscriber("bebop/states/ardrone3/PilotingState/AltitudeChanged", Ardrone3PilotingStateAltitudeChanged, self.cbGetAlt)
self.is_satellite_check = False
self.first_latitude = 0.0
self.first_longitude = 0.0
self.cur_latitude = 0.0
self.cur_longitude = 0.0
self.cur_azi = 0.0
self.tw = Twist()
self.distance = 0.0
self.bearing = 0.0
self.current_alt = 0
#def
def fnchange_yaw(self,target): # 목적지까지의 방위각을 계산하여 그만큼 회전하는 코드
state_round = 0
if( self.cur_azi >= 0 ):
if( (self.cur_azi > radians(target)) and (self.cur_azi - pi< radians(target)) ):
state_round = -1
else:
state_round = 1
else:
if( (self.cur_azi < radians(target)) and (self.cur_azi + pi> radians(target)) ):
state_round = 1
else:
state_round = -1
if( state_round == 1 ):
elif(state_round == -1):
while( not( ((self.cur_azi + 0.1 > radians(target)) and (self.cur_azi - 0.1 < radians(target))) )):
def fnCalAzi(self,lat_p1, lon_p1, lat_p2, lon_p2): # 목적지까지와의 거리와 방위각을 갱신
gps = DistanceGPS()
self.distance = gps.get_distance(lat_p1, lon_p1, lat_p2, lon_p2)
self.bearing = gps.get_bearing( lat_p1, lon_p1, lat_p2, lon_p2)
print("distance = %f, bearing = %f." %(self.distance, self.bearing))
self.fnchange_yaw(self.bearing)
def cbGetAlt(self, msg): #
self.current_alt = msg.altitude
def cbGetAzi(self,msg):
self.cur_azi = msg.yaw
def cbSatelliteNumCheck(self, msg): # 위성개수 저장하는 콜백함수
n = msg.numberOfSatellite
#print("number of satellite = " + str(n))
if n >= 10:
self.is_satellite_check = True
def cbFirstLocationSave(self, msg):
gps = DistanceGPS()
self.cur_latitude = msg.latitude
self.cur_longitude = msg.longitude
self.distance = gps.get_distance(msg.latitude, msg.longitude, TARGET_LAT, TARGET_LON)#35.944029, 126.684297)
print("latitude = %f, longitude = %f, distance = %f" % (self.cur_latitude, self.cur_longitude, self.distance))
def move_x(self, des_lat, des_lon): # 목적지로 전진하는 코드
gps = DistanceGPS()
count = 0
print('move !!!')
#print(self.distance , self.tw.linear.x)
while not rospy.is_shutdown(): # 움직이면서 계속 gps검사 & 보정
print("moving...")
count += 1
if(self.distance < 1.00003): # 남은 거리가 매우 짧을 때
break # 전진을 멈춤
elif(self.distance < 1.00007) # 남은 거리가 보통일 때
if(count >= 20000):
count = 0
else:
if(count >= 20000): # 앞으로 가는 도중 중간중간 검사
count = 0
target_yaw = gps.get_bearing(self.cur_latitude, self.cur_longitude,des_lat,des_lon)
self.fnchange_yaw(target_yaw)
self.distance=gps.get_distance(self.cur_latitude, self.cur_longitude, des_lat,des_lon) # 현재위치와 목적지까지의 거리를 갱신
print('end')
if __name__ == '__main__':
try:
goto = Goto()
while True:
if goto.is_satellite_check == True:
break
print('i found satelite!')
goto.fnCalAzi(goto.cur_latitude, goto.cur_longitude, TARGET_LAT, TARGET_LON)
print("First loaction save!")
goto.move_x(TARGET_LAT, TARGET_LON)
except KeyboardInterrupt:
pass
|
http://colorscripter.com/info#e" target="_blank" style="text-decoration:none;color:white">cs |
이 코드는 gps를 이용하여 출발지에서 목적지까지 이동하는 코드입니다.
요약하면 현재 방위각과 목표지점 방위각 차이만큼 회전하여 전진하는 코드입니다.
회전(fnchange_yaw)부분과 전진(move_x)부분은 위와 동일합니다.
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
|
def fnchange_yaw(self,target): # 목적지까지의 방위각을 계산하여 그만큼 회전하는 코드
state_round = 0
if( self.cur_azi >= 0 ):
if( (self.cur_azi > radians(target)) and (self.cur_azi - pi< radians(target)) ):
state_round = -1
else:
state_round = 1
else:
if( (self.cur_azi < radians(target)) and (self.cur_azi + pi> radians(target)) ):
state_round = 1
else:
state_round = -1
if( state_round == 1 ):
elif(state_round == -1):
while( not( ((self.cur_azi + 0.1 > radians(target)) and (self.cur_azi - 0.1 < radians(target))) )):
|
http://colorscripter.com/info#e |
이 부분이 방위각차이만큼 회전하는 코드입니다.
cur_azi는 드론의 현재방위각이 저장된 변수이고, target은 목표지점의 방위각입니다. 여기서는 각도계산이 라디안 단위로 하기 때문에 radian변환도 간간히 하였습니다.
state_round가 양수면 -0.2씩 회전(시계반대방향)하고, 음수면 시계방향으로 회전합니다.
// while문 구체적인 설명
다음 글은
이 코드들을 조합하여 AR마커 중앙으로 정렬하는 코드를 만들어 본 내용을 작성하겠습니다