Skip to content

Commit 5d979a6

Browse files
AngeloEspinozaAngeloEspinoza
committed
Camera changed to external and functions updated to NumPy docstring
1 parent 20c1c51 commit 5d979a6

10 files changed

+208
-122
lines changed

‎GUI.py

Lines changed: 146 additions & 70 deletions
Original file line numberDiff line numberDiff line change
@@ -3,10 +3,16 @@
33
import matplotlib.pyplot as plt
44

55
def display_background(img):
6-
""" Outputs the background lines for the GUI
6+
"""Outputs the background lines for the GUI.
77
8-
Parameters:
9-
img (ndarray): Image to be displayed
8+
Parameters
9+
----------
10+
img : array-like
11+
Image to be displayed.
12+
13+
Returns
14+
-------
15+
None
1016
"""
1117
# Flip by default shape to discharge in the correct variables
1218
x, y = img.shape[:2][::-1]
@@ -17,12 +23,20 @@ def display_background(img):
1723
cv2.line(img, ((x//3)*2, 0), ((x//3)*2, y//2+20), (255, 255, 255), 3)
1824

1925
def display_translation_info(img, tvec, tvec_d):
20-
""" Outputs the translation info of the ArUco marker
21-
22-
Parameters:
23-
img (ndarray): Image to be written on the information
24-
tvec (ndarray): Array with the x, y, and z positions
25-
tvec_d (ndarray): Array with the desired x, y, and z positions
26+
"""Outputs the translation info of the ArUco marker.
27+
28+
Parameters
29+
----------
30+
img : array-like
31+
Image to be written on the information.
32+
tvec : array-like
33+
Array with the x, y, and z positions.
34+
tvec_d : array-like
35+
Array with the desired x, y, and z positions.
36+
37+
Returns
38+
-------
39+
None
2640
"""
2741
x = tvec[0]
2842
y = tvec[1]
@@ -61,12 +75,20 @@ def display_translation_info(img, tvec, tvec_d):
6175
cv2.putText(img, error_z_str, (490, 120), cv2.FONT_HERSHEY_PLAIN, 2, (255, 100, 0), 1, cv2.LINE_AA)
6276

6377
def display_rotation_info(img, euler, euler_d):
64-
""" Outputs the translation info of the ArUco marker
65-
66-
Parameters:
67-
img (ndarray): Image to be written on the information
68-
euler (ndarray): Array with the roll, pitch, and yaw orientations
69-
euler_d (ndarray): Array with the desired roll, pitch, and yaw orientations
78+
"""Outputs the translation info of the ArUco marker.
79+
80+
Parameters
81+
----------
82+
img : array-like
83+
Image to be written on the information.
84+
euler : array-like:
85+
Array with the roll, pitch, and yaw orientations.
86+
euler_d : array-like
87+
Array with the desired roll, pitch, and yaw orientations.
88+
89+
Returns
90+
-------
91+
None
7092
"""
7193
roll = euler[0]
7294
pitch = euler[1]
@@ -105,17 +127,28 @@ def display_rotation_info(img, euler, euler_d):
105127
cv2.putText(img, error_yaw_str, (490, 300), cv2.FONT_HERSHEY_PLAIN, 2, (255, 100, 0), 1, cv2.LINE_AA)
106128

107129
def display_interpretation(img, tvec, euler, tvec_d, euler_d):
108-
""" Outputs a message to the user/robot stating how to move
109-
110-
Parameters:
111-
img (ndarray): Image to be written on the information
112-
tvec (ndarray): Array with the x, y, and z positions
113-
euler (ndarray): Array with the roll, pitch, and yaw orientations
114-
tvec_d (ndarray): Array with the desired x, y, and z positions
115-
euler_d (ndarray): Array with the desired roll, pitch, and yaw orientations
130+
"""Outputs a message to the user/robot stating how to move.
131+
132+
Parameters
133+
----------
134+
img : array-like
135+
Image to be written on the information.
136+
tvec : array-like
137+
Array with the x, y, and z positions.
138+
euler : array-like
139+
Array with the roll, pitch, and yaw orientations.
140+
tvec_d : array-like
141+
Array with the desired x, y, and z positions.
142+
euler_d : array-like
143+
Array with the desired roll, pitch, and yaw orientations.
144+
145+
Returns
146+
-------
147+
None
116148
"""
117-
global TOLERANCE
118-
TOLERANCE = 10 # milimeters
149+
global MILIMETERS_TOLERANCE, DEGREES_TOLERANCE
150+
MILIMETERS_TOLERANCE = 10 # milimeters
151+
DEGREES_TOLERANCE = 10 # degrees
119152

120153
x = tvec[0]
121154
y = tvec[1]
@@ -149,82 +182,99 @@ def display_interpretation(img, tvec, euler, tvec_d, euler_d):
149182
is_success_z(img, error_z)
150183

151184
def is_success_x(img, error):
185+
"""Displays the success message for the x-axis."""
152186
message_right = 'Translate{0:4.0f}mm to the right'.format(abs(error))
153187
message_left = 'Translate{0:4.0f}mm to the left'.format(abs(error))
154188
message_x_success = 'You\'ve reached the desired position in x!'
155189

156-
if error >= -TOLERANCE and error <= TOLERANCE:
190+
if error >= -MILIMETERS_TOLERANCE and error <= MILIMETERS_TOLERANCE:
157191
cv2.putText(img, message_x_success, (10, 430), cv2.FONT_HERSHEY_PLAIN, 1, (100, 255, 0), 1, cv2.LINE_AA)
158192
return True
159-
elif error > TOLERANCE:
193+
elif error > MILIMETERS_TOLERANCE:
160194
cv2.putText(img, message_left, (10, 430), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, cv2.LINE_AA)
161-
elif error < TOLERANCE:
195+
elif error < MILIMETERS_TOLERANCE:
162196
cv2.putText(img, message_right, (10, 430), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, cv2.LINE_AA)
163197

164198
def is_success_y(img, error):
199+
"""Displays the success message for the y-axis."""
165200
message_up = 'Translate{0:4.0f}mm up'.format(abs(error))
166201
message_down = 'Translate{0:4.0f}mm down'.format(abs(error))
167202
message_y_success = 'You\'ve reached the desired position in y!'
168203

169-
if error >= -TOLERANCE and error <= TOLERANCE:
204+
if error >= -MILIMETERS_TOLERANCE and error <= MILIMETERS_TOLERANCE:
170205
cv2.putText(img, message_y_success, (10, 450), cv2.FONT_HERSHEY_PLAIN, 1, (100, 255, 0), 1, cv2.LINE_AA)
171206
return True
172-
elif error > TOLERANCE:
173-
cv2.putText(img, message_up, (10, 450), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, cv2.LINE_AA)
174-
elif error < TOLERANCE:
207+
elif error > MILIMETERS_TOLERANCE:
175208
cv2.putText(img, message_down, (10, 450), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, cv2.LINE_AA)
209+
elif error < MILIMETERS_TOLERANCE:
210+
cv2.putText(img, message_up, (10, 450), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, cv2.LINE_AA)
176211

177212
def is_success_z(img, error):
213+
"""Displays the success message for the z-axis."""
178214
message_frontwards = 'Translate{0:4.0f}mm frontwards'.format(abs(error))
179215
message_backwards = 'Translate{0:4.0f}mm backwards'.format(abs(error))
180216
message_z_success = 'You\'ve reached the desired position in z!'
181217

182-
if error >= -TOLERANCE and error <= TOLERANCE:
218+
if error >= -MILIMETERS_TOLERANCE and error <= MILIMETERS_TOLERANCE:
183219
cv2.putText(img, message_z_success, (10, 470), cv2.FONT_HERSHEY_PLAIN, 1, (100, 255, 0), 1, cv2.LINE_AA)
184220
return True
185-
elif error > TOLERANCE:
221+
elif error > MILIMETERS_TOLERANCE:
186222
cv2.putText(img, message_frontwards, (10, 470), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, cv2.LINE_AA)
187-
elif error < TOLERANCE:
223+
elif error < MILIMETERS_TOLERANCE:
188224
cv2.putText(img, message_backwards, (10, 470), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, cv2.LINE_AA)
189225

190226
def is_success_roll(img, error):
227+
"""Displays the success message for the roll angle."""
191228
message_roll = 'Rotate{0:4.0f} deg around y'.format(error)
192229
message_roll_success = 'You\'ve reached the desired position in roll!'
193230

194-
if error >= -TOLERANCE and error <= TOLERANCE:
231+
if error >= -DEGREES_TOLERANCE and error <= DEGREES_TOLERANCE:
195232
cv2.putText(img, message_roll_success, (10, 350), cv2.FONT_HERSHEY_PLAIN, 1, (100, 255, 0), 1, cv2.LINE_AA)
196233
return True
197-
elif error > TOLERANCE or error < TOLERANCE:
234+
elif error > DEGREES_TOLERANCE or error < DEGREES_TOLERANCE:
198235
cv2.putText(img, message_roll, (10, 350), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, cv2.LINE_AA)
199236

200237
def is_success_pitch(img, error):
238+
"""Displays the success message for the pitch angle."""
201239
message_pitch = 'Rotate{0:4.0f} deg around x'.format(error)
202240
message_pitch_success = 'You\'ve reached the desired position in pitch!'
203241

204-
if error >= -TOLERANCE and error <= TOLERANCE:
242+
if error >= -DEGREES_TOLERANCE and error <= DEGREES_TOLERANCE:
205243
cv2.putText(img, message_pitch_success, (10, 370), cv2.FONT_HERSHEY_PLAIN, 1, (100, 255, 0), 1, cv2.LINE_AA)
206244
return True
207-
elif error > TOLERANCE or error < TOLERANCE:
245+
elif error > DEGREES_TOLERANCE or error < DEGREES_TOLERANCE:
208246
cv2.putText(img, message_pitch, (10, 370), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, cv2.LINE_AA)
209247

210248
def is_success_yaw(img, error):
249+
"""Displays the success message for the yaw angle."""
211250
message_yaw = 'Rotate{0:4.0f} deg around z'.format(error)
212251
message_yaw_success = 'You\'ve reached the desired position in yaw!'
213252

214-
if error >= -TOLERANCE and error <= TOLERANCE:
253+
if error >= -DEGREES_TOLERANCE and error <= DEGREES_TOLERANCE:
215254
cv2.putText(img, message_yaw_success, (10, 390), cv2.FONT_HERSHEY_PLAIN, 1, (100, 255, 0), 1, cv2.LINE_AA)
216255
return True
217-
elif error > TOLERANCE or error < TOLERANCE:
256+
elif error > DEGREES_TOLERANCE or error < DEGREES_TOLERANCE:
218257
cv2.putText(img, message_yaw, (10, 390), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, cv2.LINE_AA)
219258

220259
def display_info_on_screen(img, tvec, euler, tvec_d, euler_d):
221-
""" Outputs the pose and desired pose of the ArUco marker on screen
222-
Parameters:
223-
img (ndarray): Image to be written on the information
224-
tvec (ndarray): Array with the x, y, and z positions
225-
euler (ndarray): Array with the roll, pitch, and yaw orientations
226-
tvec_d (ndarray): Array with the desired x, y, and z positions
227-
euler_d (ndarray): Array with the desired roll, pitch, and yaw orientations
260+
"""Outputs the pose and desired pose of the ArUco marker on screen.
261+
262+
Parameters
263+
----------
264+
img : array-like
265+
Image to be written on the information.
266+
tvec : array-like
267+
Array with the x, y, and z positions.
268+
euler : array-like
269+
Array with the roll, pitch, and yaw orientations.
270+
tvec_d : array-like
271+
Array with the desired x, y, and z positions.
272+
euler_d : array-like
273+
Array with the desired roll, pitch, and yaw orientations.
274+
275+
Returns
276+
-------
277+
None
228278
"""
229279
x = tvec[0]
230280
y = tvec[1]
@@ -291,17 +341,30 @@ def display_info_on_screen(img, tvec, euler, tvec_d, euler_d):
291341
cv2.putText(img, current_yaw_str, (5, 70), cv2.FONT_HERSHEY_PLAIN, 0.8, (0, 0, 255), 1, cv2.LINE_AA)
292342

293343
def display_pose_graphs(time, current_time, x, y, z, R, P, Y, axis):
294-
""" Draws the pose graphs of the ArUco marker
295-
296-
Parameters:
297-
time (list): List containing the time elapsed
298-
x (list): List containing the x positions stored of the ArUco marker
299-
y (list): List containing the y positions stored of the ArUco marker
300-
z (list): List containing the z positions stored of the ArUco marker
301-
R (list): List containing the roll orientation stored of the ArUco marker
302-
P (list): List containing the pitch orientation stored of the ArUco marker
303-
Y (list): List containing the yaw orientation stored of the ArUco marker
304-
axis (ndarray): Axis to be displayed on
344+
"""Draws the pose graphs of the ArUco marker.
345+
346+
Parameters
347+
----------
348+
time : list
349+
List containing the time elapsed.
350+
x : list
351+
List containing the x positions stored of the ArUco marker.
352+
y : list
353+
List containing the y positions stored of the ArUco marker.
354+
z : list
355+
List containing the z positions stored of the ArUco marker.
356+
R : list
357+
List containing the roll orientation stored of the ArUco marker.
358+
P : list
359+
List containing the pitch orientation stored of the ArUco marker.
360+
Y : list
361+
List containing the yaw orientation stored of the ArUco marker.
362+
axis : array-like
363+
Axis to be displayed on.
364+
365+
Returns
366+
-------
367+
None
305368
"""
306369
axis[0].plot(time, x, color='b', label='x')
307370
axis[0].plot(time, y, color='g', label='y')
@@ -323,17 +386,30 @@ def display_pose_graphs(time, current_time, x, y, z, R, P, Y, axis):
323386
axis[1].set_ylabel('Camera \norientation (deg)')
324387

325388
def display_error_graphs(time, current_time, x_e, y_e, z_e, R_e, P_e, Y_e, axis):
326-
""" Draws the pose graphs of the ArUco marker
327-
328-
Parameters:
329-
time (list): List containing the time elapsed
330-
x_e (list): List containing the x position error recorded of the ArUco marker
331-
y_e (list): List containing the y position error recorded of the ArUco marker
332-
z_e (list): List containing the z position error recorded of the ArUco marker
333-
R_e (list): List containing the roll angle recorded of the ArUco marker
334-
P_e (list): List containing the pitch angle recorded of the ArUco marker
335-
Y_e (list): List containing the yaw angle recorded of the ArUco marker
336-
axis (ndarray): Axis to be displayed on
389+
"""Draws the pose graphs of the ArUco marker
390+
391+
Parameters
392+
----------
393+
time : list
394+
List containing the time elapsed.
395+
x_e : list
396+
List containing the x position error recorded of the ArUco marker.
397+
y_e : list
398+
List containing the y position error recorded of the ArUco marker.
399+
z_e : list
400+
List containing the z position error recorded of the ArUco marker.
401+
R_e : list
402+
List containing the roll angle recorded of the ArUco marker.
403+
P_e : list
404+
List containing the pitch angle recorded of the ArUco marker.
405+
Y_e : list
406+
List containing the yaw angle recorded of the ArUco marker.
407+
axis : array-like
408+
Axis to be displayed on.
409+
410+
Returns
411+
-------
412+
None
337413
"""
338414
axis[0].plot(time, x_e, color='g', label='Error x')
339415
axis[0].plot(time, y_e, color='r', label='Error y')

0 commit comments

Comments
 (0)