def main():
rospy.init_node("fase_4", anonymous=False)
pub = ReferencePublisher()
z = 2.2
bases = [(-0.25, 6, z), (-3.25, 0, z), (-6.25, 0, z), (-5.25, 1, z),
(-4.25, 2, z)]
imagem = BlueFoxImage()
for base in bases:
print("Objetivo: {}".format(base))
go_to(pub.pub_reference, base)
go_to(pub.pub_reference, (base[0], base[1], 1))
go_to(pub.pub_reference, (base[0], base[1], 0.4))
time.sleep(1)
existe = imagem.exist_panel_qr()
if existe:
# align(pub.pub_reference)
letter = imagem.get_panel_qr()
if letter != "Erro de leitura":
go_to(pub.pub_reference, (base[0], base[1], 0.3))
print("Linking to: " + letter)
link(letter)
go_to(pub.pub_reference, (base[0], base[1], 1.0))
#go_to(pub.pub_reference, (-3, 3, 2.2))
go_to(pub.pub_reference, (0, 0, 2.2))
link(letter, mode=1)
time.sleep(1)
else:
print("Failed to read QR Code")
else:
print("No QR code here")
def test():
rospy.init_node("map_scan", anonymous=False)
# publisher = ReferencePublisher()
# base_photo = taking_photo()
centroide = BlueFoxImage()
centro = centroide.angle_scan()
print("Centro: {}".format(centro[1][0]))
def photo_analyse():
imagem = BlueFoxImage()
if imagem.exist_panel():
print("\nPainel localizado")
numeros_encontrados = imagem.get_numbers()
painel = Painel(numeros_encontrados[0], numeros_encontrados[1])
print(painel)
else:
print("\nNao ha painel nesta base\n")
def align_x(pub):
img = BlueFoxImage()
centro = img.angle_scan(img.get_centroid())[1][0]
flag = img.angle_scan(img.get_centroid())[0]
# print("FLAG: {}, ANGULOS: {}, {}\n".format(flag, centro[0], centro[1]))
# time.sleep(5)
if abs(centro[1]) > 0.01:
if centro[1] > 0:
while centro[1] > 0:
move_positive_dx(pub)
centro = img.angle_scan(img.get_centroid())[1][0]
elif centro[1] < 0:
while centro[1] < 0:
move_negative_dx(pub)
centro = img.angle_scan(img.get_centroid())[1][0]
def search_red_in_photo(n_signals, all_signals):
image = BlueFoxImage()
found_red = image.get_distances_red_signals()
new_points = []
for red_signal in found_red:
if not red_already_exists(red_signal, all_signals):
n_signals += 1
new_points.append(red_signal)
print("\n\nSinal numero: {}\nCooordenadas: {:.2f}, {:.2f}".format(
n_signals, red_signal[0], red_signal[1]))
print("Executando indicacao para o sinal {}".format(n_signals))
indicacao()
time.sleep(3)
for red_signal in new_points:
all_signals.append(red_signal)
return n_signals, all_signals
def align(pub):
img = BlueFoxImage()
print("Alinhamento do drone sendo realizado")
ang_x = img.angle_scan(img.get_centroid())[1][0][0]
ang_y = img.angle_scan(img.get_centroid())[1][0][1]
ang_total = math.sqrt(ang_x**2 + ang_y**2)
primeira = 1
cnt = 0
while (ang_total > 0.08 or primeira) and cnt < 2:
# print("ANGULO TOTAL: {}".format(ang_total))
align_y(pub)
align_x(pub)
primeira = 0
ang_x = img.angle_scan(img.get_centroid())[1][0][0]
ang_y = img.angle_scan(img.get_centroid())[1][0][1]
ang_total = math.sqrt(ang_x**2 + ang_y**2)
cnt += 1
def taking_photo():
imagem = BlueFoxImage()
c = imagem.get_distances_platforms()
print(c)
return c