def get_latest_image(self):
print("Called get latest image.")
# Note: it is necessary to create new objects in this method instead of creating instance variables
# in the constructor because of weird video threading issues.
mambo_vision = DroneVision(self.mambo, is_bebop=False, buffer_size=30)
user_vision = UserVision(mambo_vision)
success = mambo_vision.open_video() # Open the video stream using ffmpeg for capturing and processing
print("Success in opening vision is %s" % success)
print("Vision successfully started!")
# FIXME: investigate if these sleeps are needed.
self.mambo.smart_sleep(5)
frame = mambo_vision.get_latest_valid_picture()
print("in save pictures on image %d " % self.index)
if frame is not None:
filename = "test_image_%06d.png" % self.index
cv2.imwrite('./images/' + filename, frame)
self.index += 1
# Save another copy of the same image to a hardcoded filename, which
# we'll use as the latest image
magic_filename = "latest_image.png"
cv2.imwrite('./images/' + magic_filename, frame)
#print(self.index)
self.mambo.smart_sleep(5)
print("Ending the sleep and vision")
mambo_vision.close_video()
return frame
if __name__ == "__main__":
anafi = Anafi()
if anafi.connect(num_retries=3):
print("Anafi connected")
# State information
print("Updating state information")
anafi.smart_sleep(1)
anafi.ask_for_state_update()
anafi.smart_sleep(1)
# Vision
print("Starting vision")
anafi_vision = DroneVision(anafi, Model.ANAFI)
user_vision = UserVision(anafi_vision)
anafi_vision.set_user_callback_function(user_vision.save_pictures,
user_callback_args=None)
# Video feed
if anafi_vision.open_video():
print("Opened video feed")
print("Sleeping for 15 seconds - move Anafi around to test feed")
anafi.smart_sleep(15)
print("Closing video feed")
anafi_vision.close_video()
anafi.smart_sleep(5)
else:
print("Could not open video feed")
filename = "test_image_%06d.png" % self.index
cv2.imwrite(filename, img)
self.index +=1
# make my bebop object
bebop = Bebop()
# connect to the bebop
success = bebop.connect(5)
if (success):
# start up the video
bebop.set_video_framerate("24_FPS")
bebop.set_video_resolutions("rec720_stream720")
bebopVision = DroneVision(bebop, is_bebop=True)
bebopVision.cleanup_old_images = True
userVision = UserVision(bebopVision)
bebopVision.set_user_callback_function(userVision.save_pictures, user_callback_args=None)
success = bebopVision.open_video()
# if (success):
# print("Vision successfully started!")
# print("Fly me around by hand!")
# bebop.smart_sleep(1)
# print("Moving the camera using velocity")
# bebop.pan_tilt_camera_velocity(pan_velocity=0, tilt_velocity=-2, duration=4)
# bebop.smart_sleep(1)
# print("Finishing demo and stopping vision")
# bebopVision.close_video()
# make my mambo object
# remember to set True/False for the wifi depending on if you are using the wifi or the BLE to connect
mambo = Mambo(mamboAddr, use_wifi=True)
print("trying to connect to mambo now")
success = mambo.connect(num_retries=3)
print("connected: %s" % success)
if success:
# get the state information
print("sleeping")
mambo.smart_sleep(1)
mambo.ask_for_state_update()
mambo.smart_sleep(1)
print("Preparing to open vision")
mamboVision = DroneVision(mambo, is_bebop=False, buffer_size=30)
userVision = UserVision(mamboVision)
# mamboVision.set_user_callback_function(userVision.follow_aruco, user_callback_args=None)
success = mamboVision.open_video()
print("Success in opening vision is %s" % success)
if success:
print("Vision successfully started!")
# removed the user call to this function (it now happens in open_video())
# mamboVision.start_video_buffering()
userVision.main()
print("out of main")
# make my mambo object
# remember to set True/False for the wifi depending on if you are using the wifi or the BLE to connect
mambo = Mambo(mamboAddr, use_wifi=True)
print("trying to connect to mambo now")
success = mambo.connect(num_retries=3)
print("connected: %s" % success)
if (success):
# get the state information
print("sleeping")
mambo.smart_sleep(1)
mambo.ask_for_state_update()
mambo.smart_sleep(1)
print("Preparing to open vision")
mamboVision = DroneVision(mambo, is_bebop=False, buffer_size=30)
userVision = UserVision(mamboVision)
mamboVision.set_user_callback_function(userVision.save_pictures,
user_callback_args=None)
success = mamboVision.open_video()
print("Success in opening vision is %s" % success)
if (success):
print("Vision successfully started!")
#removed the user call to this function (it now happens in open_video())
#mamboVision.start_video_buffering()
if (testFlying):
print("taking off!")
mambo.safe_takeoff(5)
mamboAddr = "e0:14:d0:63:3d:d0"
mambo = Mambo(mamboAddr, use_wifi=True)
print("trying to connect to mambo now")
success = mambo.connect(num_retries=3)
print("connected: %s" % success)
while (success):
# get the state information
print("sleeping")
mambo.smart_sleep(1)
mambo.ask_for_state_update()
mambo.smart_sleep(1)
print("Preparing to open vision")
mamboVision = DroneVision(mambo, is_bebop=False, buffer_size=30)
userVision = UserVision(mamboVision)
mamboVision.set_user_callback_function(userVision.save_pictures, user_callback_args=None)
success = mamboVision.open_video() #Open the video stream using ffmpeg for capturing and processing
print("Success in opening vision is %s" % success)
print("Vision successfully started!")
frame = mamboVision.get_latest_valid_picture()
#here should be input to detect.py
# run inference
res = run_inference_for_single_image(frame, sess, tensor_dict, image_tensor)
print("frame")
# view the bounding boxes:
bebopVision.close_video()
found = True
###------------------###
#Start main:
# make my bebop object
bebop = Bebop()
# connect to the bebop
success = bebop.connect(5)
if (success):
# start up the video
bebopVision = DroneVision(bebop, is_bebop=True)
userVision = UserVision(bebopVision)
bebopVision.set_user_callback_function(userVision.save_pictures,
user_callback_args=None)
success = bebopVision.open_video()
if (success):
print("Vision successfully started!")
print("sleeping")
bebop.smart_sleep(2)
bebop.ask_for_state_update()
bebop.safe_takeoff(10)
bebop.smart_sleep(2)
org = (50, 50)
cv2.putText(img, 'Angle is: {}'.format(velocity), org, self.font, self.fontScale, self.color, self.thickness, cv2.LINE_AA)
org = (50, 150)
cv2.putText(img, 'Probability is: {}%'.format(prob), org, self.font, self.fontScale, self.color, self.thickness, cv2.LINE_AA)
for box in v_boxes:
cv2.rectangle(img, (box.xmin, box.ymin), (box.xmax, box.ymax), (0,0,0), 2)
# make my bebop object
bebop = Bebop()
# connect to the bebop
success = bebop.connect(5)
if (success):
bebopVision = DroneVision(bebop, Model.BEBOP, buffer_size = 1)
userVision = UserVision(bebopVision)
bebopVision.set_user_callback_function(userVision.streamVideo, user_callback_args=None)
success = bebopVision.open_video()
if (success):
print("Vision successfully started!")
#removed the user call to this function (it now happens in open_video())
#bebopVision.start_video_buffering()
# skipping actually flying for safety purposes indoors - if you want
# different pictures, move the bebop around by hand
print("Fly me around by hand!")
bebop.smart_sleep(5)
bebop.smart_sleep(120)
print("Finishing demo and stopping vision")
def perform_landing(self):
self.drone.safe_land(5)
self.vision.close_video()
self.drone.disconnect()
if __name__ == '__main__':
INSTRUCTIONS = file_manager.get_navigation_instructions_from_json()
print("[INSTRUCTION] Dictionary created")
MAMBO_ADDRESS = "d0:3a:4d:78:e6:36"
mambo = Mambo(MAMBO_ADDRESS, use_wifi=True)
mambo_connected = mambo.connect(num_retries=20)
print("[MAMBO]connected: %s" % mambo_connected)
mambo.smart_sleep(1)
if mambo_connected:
vision = DroneVision(mambo, is_bebop=False, buffer_size=30)
userVision = UserVision(vision, INSTRUCTIONS, mambo)
vision.set_user_callback_function(
userVision.user_code_after_vision_opened, user_callback_args=None)
success = vision.open_video()
print("[VISION] Success in opening vision is %s" % success)
mambo.smart_sleep(3)
else:
print("[VISION]Error connecting to bebop. Retry")
# make my mambo object
# remember to set True/False for the wifi depending on if you are using the wifi or the BLE to connect
mambo = Mambo(mamboAddr, use_wifi=True)
print("trying to connect to mambo now")
success = mambo.connect(num_retries=3)
print("connected: %s" % success)
if (success):
# get the state information
print("sleeping")
mambo.smart_sleep(1)
mambo.ask_for_state_update()
mambo.smart_sleep(1)
print("Preparing to open vision")
mamboVision = DroneVision(mambo, is_bebop=False, buffer_size=5)
userVision = UserVision(mamboVision)
mamboVision.set_user_callback_function(userVision.store_frame,
user_callback_args=None)
success = mamboVision.open_video()
print("Success in opening vision is %s" % success)
if (success):
while True:
frame = userVision.latest_frame
if frame is not None:
# Our operations on the frame come here
painted_frame = SymbolTracker.track_and_paint_helipad(frame)
# Display the resulting frame
cv2.imshow('painted_frame', painted_frame)
def main():
tc = tracker.createTrackerByName("KCF")
# 일반모드에서 블루투스를 사용하여 드론을 찾는다, 드론의 연결 주소와 이름이 반환된다
# addr,name = findMinidrone.getMamboAddr()
addr = None
# 드론 객체 생성 FPV의 경우는 WIFI를 사용하며, use_wifi = True가 된다
mambo = Mambo(addr, use_wifi=True)
# 드론을 연결한다
success = mambo.connect(3)
print("Connect: %s" % success)
mambo.smart_sleep(0.01)
# 드론의 상태를 업데이트 한다
mambo.ask_for_state_update()
# 드론의 움직임 기울기를 1로 설정한다
mambo.set_max_tilt(1)
# 드론의 배터리를 확인 할 수 있다
battery = mambo.sensors.battery
# FPV 연결
mamboVision = DroneVision(mambo, is_bebop=False, buffer_size=30)
userVision = UserVision(mamboVision)
mamboVision.set_user_callback_function(userVision.get_image,
user_callback_args=None)
cv2.namedWindow('Vision')
cv2.setMouseCallback('Vision', draw_bbox)
success = mamboVision.open_video()
img = cv2.imread(mamboVision.filename, cv2.IMREAD_COLOR)
mask = np.ones_like(img, dtype=np.uint8)
tc = tracker.createTrackerByName("KCF")
# Tracking Mode
mode = False
bbox = []
angleStack, yawTime = 0, 0
mov = drawMov()
mov.mambo = mambo
while (True):
# OpenCV를 바탕으로 드론을 제어 하기위해 마스킹이미지를 생성한다
# mask = np.ones((480,600,3),dtype=np.uint8)
img = cv2.imread(mamboVision.filename, cv2.IMREAD_COLOR)
print(img.shape)
# 드론과 연결이 끊기지 않기 위해 매 프레임마다 상태 확인 신호를 보낸다
mambo.smart_sleep(0.01)
# 드론의 배터리를 확인 할 수 있다
battery = mambo.sensors.battery
print("Battery: %s" % battery)
#img=cv2.add(img,mask)
#if mode==True:
# bbox=tracker.updateTracker(img,tc)
# angleStack,yawTime=mov.adjPos(mask,bbox,angleStack,yawTime)
cv2.imshow("Vision", mask)
key = cv2.waitKey(10)
# 'q' 키를 누르면 드론을 착륙하고 프로세스를 종료한다
if ord('q') == key:
mambo.safe_land(5)
mamboVision.close_video()
exit(0)
# 'p' 키를 누르면 드론이 이륙한다
elif ord('p') == key:
mambo.safe_takeoff(5)
# 'w' 키를 누르면 드론에 앞으로 100 만큼 움직이라는 신호를 0.1초간 보낸다
elif ord('w') == key:
mambo.fly_direct(roll=0,
pitch=100,
yaw=0,
vertical_movement=0,
duration=0.1)
# 's' 키를 누르면 드론에 뒤로 100 만큼 움직이라는 신호를 0.1초간 보낸다
elif ord('s') == key:
mambo.fly_direct(roll=0,
pitch=-100,
yaw=0,
vertical_movement=0,
duration=0.1)
# 'a' 키를 누르면 드론에 왼쪽으로 50 만큼 움직이라는 신호를 0.1초간 보낸다
elif ord('a') == key:
mambo.fly_direct(roll=-50,
pitch=0,
yaw=0,
vertical_movement=0,
duration=0.1)
# 'd' 키를 누르면 드론에 오른쪽으로 50 만큼 움직이라는 신호를 0.1초간 보낸다
elif ord('d') == key:
mambo.fly_direct(roll=50,
pitch=0,
yaw=0,
vertical_movement=0,
duration=0.1)
elif ord('i') == key:
mambo.fly_direct(roll=0,
pitch=0,
yaw=0,
vertical_movement=50,
duration=0.1)
elif ord('k') == key:
mambo.fly_direct(roll=0,
pitch=0,
yaw=0,
vertical_movement=-50,
duration=0.1)
elif ord('j') == key:
mambo.fly_direct(roll=0,
pitch=0,
yaw=-50,
vertical_movement=0,
duration=0.1)
elif ord('l') == key:
mambo.fly_direct(roll=0,
pitch=0,
yaw=50,
vertical_movement=0,
duration=0.1)
elif ord('c') == key:
mask = np.ones((480, 600, 3), dtype=np.uint8)
elif ord('v') == key:
print("tracker start")
bbox = [tx, ty, bx - tx, by - ty]
mode = True
ok = tc.init(img, bbox)
mov.setBox([tx, ty, bx, by])