NikolajDanger/Roboteksperimentarium
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-09-28 13:39:25 +02:00
parent 76f7e922cc
commit a90ef9d8f9
1 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
drive_between_arucos.py
View File

@ -37,19 +37,21 @@ def find_aruco(image):
return [(box[0], ids[i]) for i, box in enumerate(corners)] return [(box[0], ids[i]) for i, box in enumerate(corners)]
def find_arucos(arlo): def find_arucos(arlo):
aruco_list = [] aruco_dict = {}
while True: while True:
arucos = find_aruco(arlo.take_photo()) arucos = find_aruco(arlo.take_photo())
if arucos != []: if arucos != []:
aruco_list += arucos for aruco, id_ in arucos:
if len(aruco_list) >= 2: aruco_dict[id_] = aruco
if len(aruco_dict) >= 2:
break break
arlo.go_diff(40, 40, 1, 0) arlo.go_diff(40, 40, 1, 0)
sleep(0.3) sleep(0.3)
arlo.stop() arlo.stop()
return arucos[:2] return list(arucos.items())[:2]
def main(): def main():
arlo = Robot() arlo = Robot()
@ -57,7 +59,7 @@ def main():
aruco_positions = np.array([cv2.aruco.estimatePoseSingleMarkers( aruco_positions = np.array([cv2.aruco.estimatePoseSingleMarkers(
np.array([aruco[0]]), 14.5, CAMERA_MATRIX, DIST_COEF np.array([aruco[0]]), 14.5, CAMERA_MATRIX, DIST_COEF
)[1][0][0] for aruco in arucos]) )[0][0] for aruco in arucos])
position = [ position = [
np.average(aruco_positions[:,0]), np.average(aruco_positions[:,0]),