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 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 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