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:53:11 +02:00
parent a675ecfc88
commit 35971fbcee
1 changed files with 16 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
drive_between_arucos.py
View File

@ -1,4 +1,5 @@
from time import sleep from time import sleep
from math import cos, sin
import cv2 import cv2
import numpy as np import numpy as np
@ -38,20 +39,29 @@ def find_aruco(image):
def find_arucos(arlo): def find_arucos(arlo):
aruco_dict = {} aruco_dict = {}
theta = np.deg2rad(30)
rot = [[[cos(theta), -sin(theta)], [sin(theta), cos(theta)]]]
while True: while True:
arucos = find_aruco(arlo.take_photo()) arucos = find_aruco(arlo.take_photo())
if arucos != []: if arucos != []:
for aruco, id_ in arucos: for aruco, id_ in arucos:
if id_ in [1,6]: if id_ in [1,6] and id_ not in aruco_dict:
print(f"found box {id_}") print(f"found box {id_}")
aruco_dict[id_] = aruco position = cv2.aruco.estimatePoseSingleMarkers(
np.array([aruco]), 14.5, CAMERA_MATRIX, DIST_COEF
)[1][0][0]
position = np.array([position[0], position[1]])
position[0] += 22.5
aruco_dict[id_] = position
if len(aruco_dict) >= 2: if len(aruco_dict) >= 2:
break break
arlo.go_diff(40, 40, 1, 0) arlo.go_diff(POWER, POWER, 1, 0)
sleep(0.3) sleep((20 * TURN_T)/1000)
arlo.stop() arlo.stop()
for key, value in aruco_dict.items():
aruco_dict[key] = np.dot(rot, value)
return list(aruco_dict.values())[:2] return list(aruco_dict.values())[:2]
@ -64,6 +74,8 @@ def main():
np.array([aruco]), 14.5, CAMERA_MATRIX, DIST_COEF np.array([aruco]), 14.5, CAMERA_MATRIX, DIST_COEF
)[1][0][0] for aruco in arucos]) )[1][0][0] for aruco in arucos])
print(aruco_positions)
position = [ position = [
np.average(aruco_positions[:,0]), np.average(aruco_positions[:,0]),
np.average(aruco_positions[:,2]) np.average(aruco_positions[:,2])