def test_2_fly_straight_turn_fly_back():
drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
drone.start_video_streaming()
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
drone(
moveBy(dX=10, dY=0, dZ=0, dPsi=0) >> FlyingStateChanged(
state="hovering", _timeout=20)).wait()
drone(
moveBy(dX=0, dY=0, dZ=0, dPsi=math.pi) >> FlyingStateChanged(
state="hovering", _timeout=20)).wait()
drone(moveBy(dX=10, dY=0, dZ=0, dPsi=0))
# >> FlyingStateChanged(state="hovering", _timeout=20)
# ).wait()
drone(CancelMoveBy(_timeout=10))
drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
drone.stop_video_streaming()
drone.disconnection()
def goToHome():
print("Going to HOME")
drone(
moveBy(0, 0, -5, 0) >> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
drone(
moveTo(latitude=homeLatitude,
longitude=homeLongitude,
altitude=homeAltitude,
orientation_mode=0,
heading=0) >> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
return
def start(self):
# Connect the the drone
self.drone.connection()
print("Takeoff if necessary...")
self.drone(
FlyingStateChanged(state="hovering", _policy="check")
| FlyingStateChanged(state="flying", _policy="check")
| (
GPSFixStateChanged(fixed=1, _timeout=10, _policy="check_wait")
>> (
TakeOff(_no_expect=True)
& FlyingStateChanged(
state="hovering", _timeout=10, _policy="check_wait")
)
)
).wait()
self.drone(moveBy(0,-1.15,-0.5,-3.142)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
self.drone(moveBy(2.0,0, 0,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
# You can record the video stream from the drone if you plan to do some
# post processing.
self.drone.set_streaming_output_files(
h264_data_file=os.path.join(self.tempd, 'h264_data.264'),
h264_meta_file=os.path.join(self.tempd, 'h264_metadata.json'),
# Here, we don't record the (huge) raw YUV video stream
# raw_data_file=os.path.join(self.tempd,'raw_data.bin'),
# raw_meta_file=os.path.join(self.tempd,'raw_metadata.json'),
)
# Setup your callback functions to do some live video processing
self.drone.set_streaming_callbacks(
raw_cb=self.yuv_frame_cb,
h264_cb=self.h264_frame_cb
)
# Start video streaming
self.drone.start_video_streaming()
def function_8():
drone(
moveBy(0, 0, -1, 0)
>> FlyingStateChanged(state='hovering', _timeout=5)
>> f.write("drone(")
>> f.write("\n")
>> f.write(" moveBy(0, 0, -1, 0)")
>> f.write("\n")
>> f.write(" >> FlyingStateChanged(state='hovering', _timeout=5)")
>> f.write("\n")
>> f.write(").wait()")
>> f.write("\n")
)
def checkWP(self):
#This is a 1D exaple for checking if the drone has made it to a current waypoint
while True:
# Drone Pose
x_dr = self.curPose.x
y_dr = self.curPose.y
z_dr = self.curPose.z
yaw_dr = self.curPose.yaw
# Reference
x_R = self.waypoints[self.curWPindex].x
y_R = self.waypoints[self.curWPindex].y
z_R = self.waypoints[self.curWPindex].z
Thr_vec = np.array([0.1, 0.1, 0.05,
1 * pi / 180]) # Threshold Vector
er_mag = math.pow((x_dr - x_R), 2) + math.pow(
(y_dr - y_R), 2) + math.pow((z_dr - z_R, 2))
if er_mag <= math.pow(self.r, 2):
print(colored("MADE IT TO WP!", "green"))
#send new drone control command
self.curWPindex += 1
else:
gn_mat = [0.2, 0.2, 0.1, 0.1]
xEr = x_R - x_dr
yEr = y_R - y_dr
zEr = z_R - z_dr
th_req = New_Sign(yEr) * acos(xEr / er_mag)
R_ER_I = np.array([xEr, yEr, zEr, th_req])
mov_cm_UC = R3_Mat_Cords(yaw_dr, R_ER_I, gn_mat)
x_c, y_c, z_c, yaw_c = Check_Move_Sat(mov_cm_UC, Thr_vec)
drone(
moveBy(x_c, y_c, z_c, yaw_c) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait()
time.sleep(0.3)
#check if end of trajectory
if self.curWPindex == len(self.waypoints):
break
def test_3_fly_straight_2_seconds():
drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
drone.start_video_streaming()
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
sleep(5)
drone(
moveBy(dX=5, dY=0, dZ=0, dPsi=0) >> FlyingStateChanged(
state="hovering", _timeout=2)).wait()
sleep(5)
drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
drone.stop_video_streaming()
drone.disconnection()
def ManualMove(self,params):
if self._joystick_ctrl :
self.my_log.info("Mode mobile : commande {} ignorée ".format(params.to_string()))
return;
if isinstance(params, dict):
params = DroneCommandParams(**params)
if self._manual_unit :
if not self._on_test:
self._drone(moveBy(params.x,params.y,params.z,params.orientation))
else :
self.my_log.info("Mode simulation: informations reçues {} ".format(params.to_string()))
else :
self.my_log.info("Mode automatique : commande {} ignorée ".format(params.to_string()))
def test_10_reach_target():
drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
drone.start_video_streaming()
change_gimbal_angle(-85)
print("------take off-------")
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
sleep(2)
while detect() != 'blue':
drone(
moveBy(dX=1, dY=0, dZ=0, dPsi=0) >> FlyingStateChanged(
state="hovering", _timeout=20)).wait()
drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
drone.stop_video_streaming()
drone.disconnection()
def fly(self):
# Takeoff, fly, land, ...
print("Takeoff if necessary...")
self.drone(
FlyingStateChanged(state="hovering", _policy="check")
| FlyingStateChanged(state="flying", _policy="check")
| (
GPSFixStateChanged(fixed=1, _timeout=10, _policy="check_wait")
>> (
TakeOff(_no_expect=True)
& FlyingStateChanged(
state="hovering", _timeout=10, _policy="check_wait")
)
)
).wait()
self.drone(moveBy(0,-1.15,-0.5,-3.142)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
def function_2():
drone(
moveBy(1, 0, 0, 0)
>> FlyingStateChanged(state='hovering', _timeout=5, _policy='check_wait', _float_tol=(1e-07, 1e-09))
>> f.write("drone(")
>> f.write("\n")
>> f.write(" moveBy(1, 0, 0, 0)")
>> f.write("\n")
>> f.write(" >> FlyingStateChanged(state='hovering', _timeout=5)")
>> f.write("\n")
>> f.write(").wait()")
>> f.write("\n")
)
def fly(self):
# Takeoff, fly, land, ...
print("Takeoff")
self.drone(
TakeOff()
& FlyingStateChanged(
state="hovering", _timeout=10, _policy="check_wait")).wait()
#for i in range(2):
# print("Moving by ({}/2)...".format(i + 1))
self.drone(moveBy(1, 0, -0.5, 0, _timeout=20)).wait().success()
print("Landing...")
self.drone(
Orientation(pan=0, tilt=-82)
& StateOrientation(pan=0, tilt=-82)).wait().success()
self.land()
print("Landed\n")
def start(self):
# Connect the the drone
pygame.init()
W, H = 320, 240
fps=40
vx=0;
vy=0;
vz=0.0;
vr=0.0;
screen = pygame.display.set_mode((W, H))
self.drone.connection()
#clock = pygame.time.Clock()
running=True
while running:
self.drone.start_piloting()
for event in pygame.event.get():
if event.type==pygame.QUIT:
running=False
elif event.type==pygame.KEYUP:
self.drone.piloting_pcmd(0, 0, 0, 0, 0.0)
time.sleep(1)
elif event.type==pygame.KEYDOWN:
if event.key==pygame.K_ESCAPE:
self.drone.piloting_pcmd(0, 0, 0, 0, 0.0) # (roll, pitch, yaw, gaz, piloting_time)
time.sleep(1)
running=False
elif event.key==pygame.K_p:
self.drone(
TakeOff()
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
elif event.key==pygame.K_SPACE:
self.drone(
Landing()
>> FlyingStateChanged(state="landing", _timeout=5)
).wait()
elif event.key==pygame.K_w:
self.drone(moveBy(1.0,0,0,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
elif event.key==pygame.K_s:
self.drone(moveBy(-1.0,0,0,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
elif event.key==pygame.K_a:
self.drone(moveBy(0,-1,0,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
elif event.key==pygame.K_d:
self.drone(moveBy(0,1,0,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
elif event.key==pygame.K_r:
self.drone(moveBy(0,0,-0.5,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
elif event.key==pygame.K_f:
self.drone(moveBy(0,0,0.5,0)>> FlyingStateChanged(state="hovering",_timeout=10)).wait()
#self.drone.piloting_pcmd(vy, vx, 0, 0, 0.5) # (roll, pitch, yaw, gaz, piloting_time)
time.sleep(1)
pygame.display.flip()
def take_photo_burst_Move(drone, th_mov, th_R0, dxm):
# take a photo burst and get the associated media_id
xc, yc, zc = R3_Mat_Cords(th_R0, dxm)
photo_saved = drone(photo_progress(result="photo_saved", _policy="wait"))
drone(take_photo(cam_id=0) & moveBy(xc, yc, zc, th_mov)).wait()
photo_saved.wait()
media_id = photo_saved.received_events().last().args["media_id"]
# download the photos associated with this media id
media_info_response = requests.get(ANAFI_MEDIA_API_URL + media_id)
media_info_response.raise_for_status()
os.chdir("/home/rnallapu/code/Results")
download_dir = tempfile.mkdtemp()
Res_dir = filecreation()
#tempfile.gettempdir()
for resource in media_info_response.json()["resources"]:
image_response = requests.get(ANAFI_URL + resource["url"], stream=True)
download_path = os.path.join(download_dir, resource["resource_id"])
print('Hello')
print(download_path)
image_response.raise_for_status()
with open(download_path, "wb") as image_file:
shutil.copyfileobj(image_response.raw, image_file)
# parse the xmp metadata
with open(download_path, "rb") as image_file:
image_data = image_file.read()
image_xmp_start = image_data.find(b"<x:xmpmeta")
image_xmp_end = image_data.find(b"</x:xmpmeta")
image_xmp = ET.fromstring(image_data[image_xmp_start : image_xmp_end + 12])
for image_meta in image_xmp[0][0]:
xmp_tag = re.sub(r"{[^}]*}", "", image_meta.tag)
xmp_value = image_meta.text
# only print the XMP tags we are interested in
if xmp_tag in XMP_TAGS_OF_INTEREST:
print(resource["resource_id"], xmp_tag, xmp_value)
#
#shutil.copy2(download_path, "/home/rnallapu/code/Photos/")
shutil.copy2(download_path, Res_dir)
def take_Map_Video_Move(drone, th_mov, th_R0, dxm):
# take a photo burst and get the associated media_id
xc, yc, zc = R3_Mat_Cords(th_R0, dxm)
photo_saved = drone(recording_progress(result="stopped", _policy="wait"))
# drone(
# MaxRotationSpeed(1)
# >> MaxRotationSpeedChanged(current=1, _policy='wait')
# ).wait()
#drone(setAutonomousFlightMaxRotationSpeed(1e-7)).wait()
drone(start_recording(cam_id=0) & moveBy(xc, yc, zc, th_mov)).wait()
drone(stop_recording(cam_id=0)).wait()
print('Action Completed')
photo_saved.wait()
media_id = photo_saved.received_events().last().args["media_id"]
print(media_id)
# download the photos associated with this media id
os.chdir("/home/rnallapu/code/Results")
media_info_response = requests.get(ANAFI_MEDIA_API_URL + media_id)
media_info_response.raise_for_status()
download_dir = tempfile.mkdtemp()
Res_dir = filecreation()
#tempfile.gettempdir()
for resource in media_info_response.json()["resources"]:
image_response = requests.get(ANAFI_URL + resource["url"], stream=True)
download_path = os.path.join(download_dir, resource["resource_id"])
print('Hello')
print(download_path)
image_response.raise_for_status()
with open(download_path, "wb") as image_file:
shutil.copyfileobj(image_response.raw, image_file)
#shutil.copy2(download_path, "/home/rnallapu/code/Photos/")
shutil.copy2(download_path, Res_dir)
def test_asyncaction():
with olympe.Drone(DRONE_IP) as drone:
drone.connect()
# Start a flying action asynchronously
flyingAction = drone(
TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5) >>
moveBy(10, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5) >> Landing())
# Start video recording while the drone is flying
if not drone(start_recording(cam_id=0)).wait().success():
assert False, "Cannot start video recording"
# Send a gimbal pitch velocity target while the drone is flying
cameraAction = drone(
gimbal.set_target(
gimbal_id=0,
control_mode="velocity",
yaw_frame_of_reference="none",
yaw=0.0,
pitch_frame_of_reference="none",
pitch=0.1,
roll_frame_of_reference="none",
roll=0.0,
)).wait()
if not cameraAction.success():
assert False, "Cannot set gimbal velocity target"
# Wait for the end of the flying action
if not flyingAction.wait().success():
assert False, "Cannot complete the flying action"
# Stop video recording while the drone is flying
if not drone(stop_recording(cam_id=0)).wait().success():
assert False, "Cannot stop video recording"
# Leaving the with statement scope: implicit drone.disconnect() but that
# is still a good idea to perform the drone disconnection explicitly
drone.disconnect()
def test_9_calculate_drone_speed():
drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
# drone.start_video_streaming()
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
counter = 3
while counter < 6:
start_time = time.time()
distance = 1 * counter
drone(
moveBy(dX=distance, dY=0, dZ=0, dPsi=0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait()
speed = distance / (time.time() - start_time)
print("speed [m/s]: ", speed)
counter += 1
drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
drone.stop_video_streaming()
drone.disconnection()
def test_7_detect_and_move():
drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
# drone.start_video_streaming()
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
change_gimbal_angle(-80)
sleep(1)
start_time = time.time()
while time.time() - start_time < 500:
tc = detect.detect()
if tc > 0:
drone(
moveBy(dX=2, dY=0, dZ=0, dPsi=0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait()
break
reset_gimbal_orientation()
drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
drone.stop_video_streaming()
drone.disconnection()
def launch(self, hover_altitude):
self.bebop(
FlyingStateChanged(state='hovering', _policy='check')
| FlyingStateChanged(state='flying', _policy='check') | (
# GPSFixStateChanged(fixed=1, _timeout=10, _policy='check_wait')
# >> (
(TakeOff(_no_expect=True)
& FlyingStateChanged(
state='hovering', _timeout=10, _policy='check_wait')))
).wait()
self.drone_location = self.bebop.get_state(GpsLocationChanged)
if hover_altitude > 0:
self.bebop(
moveBy(0, 0, -hover_altitude, 0) # Move drone up
>> PCMD(1, 0, 0, 0, 0,
0) # Force drone to go into hovering state
>> FlyingStateChanged(state='hovering',
_timeout=5) # Set state to hovering
).wait().success()
def fly(self):
# Takeoff, fly, land, ...
print("Takeoff if necessary...")
self.drone(
FlyingStateChanged(state="hovering", _policy="check")
| FlyingStateChanged(state="flying", _policy="check")
|
(GPSFixStateChanged(fixed=1, _timeout=10, _policy="check_wait") >>
(TakeOff(_no_expect=True)
& FlyingStateChanged(
state="hovering", _timeout=10, _policy="check_wait")))).wait(
)
self.drone(MaxTilt(40)).wait().success()
for i in range(3):
print("Moving by ({}/3)...".format(i + 1))
self.drone(moveBy(10, 0, 0, math.pi, _timeout=20)).wait().success()
print("Landing...")
self.drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)
).wait()
print("Landed\n")
def goToEntrance():
print("Going to entrance")
##Go up
drone(
moveBy(0, 0, -5, 0) >> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
##Go to entrance
drone(
moveTo(latitude=entranceLatitude,
longitude=entranceLongitude,
altitude=5,
orientation_mode=0,
heading=0) >> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
drone(
moveTo(latitude=entranceLatitude,
longitude=entranceLongitude,
altitude=entranceAltitude,
orientation_mode=0,
heading=0) >> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
return
def test_subscriber():
drone = olympe.Drone(DRONE_IP, media_autoconnect=False)
# subscribe to all events during the drone connection
with drone.subscribe(lambda event, controller: print(f"{event}")):
drone.connect()
# Subscribe to FlyingStateChanged
# If you call `drone.subscribe` without using the `with` statement,
# you'll have to call `drone.unsubscribe()` later.
flying_sub = drone.subscribe(
lambda event, controller: print("Flyingstate =", event.args["state"]),
FlyingStateChanged(_policy="check"),
)
assert (drone(
FlyingStateChanged(state="hovering")
| (TakeOff()
& FlyingStateChanged(state="hovering"))).wait(5).success())
assert drone(moveBy(10, 0, 0, 0)).wait().success()
drone(Landing()).wait()
assert drone(FlyingStateChanged(state="landed")).wait().success()
# unsubscribe from FlyingStateChanged
drone.unsubscribe(flying_sub)
drone.disconnect()
def reset(self):
# Resets the drone back to start of simulation after completion of episode
self.pos_feedback() # Update state of the drone in self.agent_pos
# Random initialization(reset) for every episode
x_r = random.randrange(-5,5,1)
y_r = random.randrange(-5,5,1)
z_r = random.randrange(1,5,1)
# Random goal setting for every episode
x_d = random.randrange(-5,5,1)
y_d = random.randrange(-5,5,1)
z_d = random.randrange(1,5,1)
self.destination = [x_d,y_d,z_d]
print('------------RESET-------------',[x_r,y_r,z_r],'------------RESET-------------')
print('------------GOAL-------------',self.destination,'------------GOAL-------------')
# Move the drone to random initialization coordinate
self.drone(moveBy(x_r-self.agent_pos[0], self.agent_pos[1]-y_r, self.agent_pos[2]-z_r, 0)>> FlyingStateChanged(state="hovering", _timeout=5)).wait()
self.pos_feedback() # Update state of the drone in self.agent_pos
obs = [self.agent_pos[0]-self.destination[0],self.agent_pos[1]-self.destination[1], self.agent_pos[2]-self.destination[2]]
return np.array(obs).astype(np.float32) # reward, done, info can't be included
def test_listener():
drone = olympe.Drone(DRONE_IP)
# Explicit subscription to every event
every_event_listener = EveryEventListener(drone)
every_event_listener.subscribe()
drone.connect()
every_event_listener.unsubscribe()
# You can also subscribe/unsubscribe automatically using a with statement
with FlightListener(drone) as flight_listener:
for i in range(2):
get_state = drone(GetState(camera_id=0)).wait()
assert get_state.success()
assert drone(
FlyingStateChanged(state="hovering")
| (TakeOff() & FlyingStateChanged(state="hovering"))
).wait(5).success()
assert drone(moveBy(10, 0, 0, 0)).wait().success()
drone(Landing()).wait()
assert drone(FlyingStateChanged(state="landed")).wait().success()
print(f"Takeoff count = {flight_listener.takeoff_count}")
assert flight_listener.takeoff_count > 0
drone.disconnect()
def yuv_frame_cb(self, yuv_frame):
info = yuv_frame.info()
height, width = info["yuv"]["height"], info["yuv"]["width"]
cv2_cvt_color_flag = {
olympe.PDRAW_YUV_FORMAT_I420: cv2.COLOR_YUV2BGR_I420,
olympe.PDRAW_YUV_FORMAT_NV12: cv2.COLOR_YUV2BGR_NV12,
}[info["yuv"]["format"]]
img = cv2.cvtColor(yuv_frame.as_ndarray(), cv2_cvt_color_flag)
#lg = np.array([40, 62, 0])
#ug = np.array([96, 255, 255])
global lg
global ug
blurred = cv2.GaussianBlur(img, (15, 15), 0)
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, lg, ug)
mask = cv2.erode(mask, None, iterations=1) #remove noise
mask = cv2.dilate(mask, None, iterations=1) #remove noise
mask = cv2.GaussianBlur(mask, (15, 15), 2, 2) #window size 15x15
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
vx = 0.0
vy = 0.0
vz = 0.0
vr = 0.0
marker_y = 0.0
marker_x = 0.0
kp = 0.001
#movement speed (control gain): higher value higher speed
global largest
#define it one time, (allow user to modify the variable in current scope)
for cnt in contours:
lg = np.array([40, 62, 0])
ug = np.array([96, 255, 255])
approx = cv2.approxPolyDP(cnt, 0.01 * cv2.arcLength(cnt, True),
True)
print(len(approx))
if len(approx) == 4:
print("square")
cv2.drawContours(img, [cnt], 0, (0, 0, 255), -1)
elif len(approx) > 14:
cv2.drawContours(img, [cnt], 0, (0, 255, 255), -1)
area = cv2.contourArea(cnt)
print("Area Circle = %.2f" % area)
if largest is None or cv2.contourArea(cnt) > cv2.contourArea(
largest):
if cv2.contourArea(cnt) < 1000000 and cv2.contourArea(
cnt) > 5000:
largest = cnt
M = cv2.moments(largest)
marker_y = int(M["m01"] / M["m00"])
marker_x = int(M["m10"] / M["m00"])
vx = 0.5
vy = 0.0
vz = kp * (img.shape[0] / 2 - marker_y) #horizontal
vr = kp * (img.shape[1] / 2 - marker_x) #vertical
cv2.circle(img, (marker_x, marker_y), 2, (0, 0, 255),
-1)
self.drone(moveBy(vx, vy, -vz, -vr)).wait(10)
else:
circles = cv2.HoughCircles(gray,
cv2.HOUGH_GRADIENT,
5,
500,
param1=80,
param2=100,
minRadius=50,
maxRadius=150)
if circles is not None:
print("Circle There !")
circles = np.uint16(np.around(circles))
for i in circles[0, :]:
# draw the outer circle
cv2.circle(img, (i[0], i[1]), i[2], (0, 255, 0), 2)
# draw the center of the circle
cv2.circle(img, (i[0], i[1]), 2, (0, 0, 255), 3)
#for len(approx) != 4 and len(approx) < 14:
#cirles=cv2.HoughCircles(mask, cv2.HOUGH_GRADIENT, 1, 10)
#if cirles!=None:
#print ("Circle There !")
#cv2.drawContours(img,[cnt],0,(255,0,0),-1)
#area = cv2.contourArea(cnt)
#print (area)
#vx=0
#vy=3
#vz=0
#vr=-3.142
cv2.circle(img, (int(img.shape[1] / 2), int(img.shape[0] / 2)), 2,
(0, 0, 255), -1)
cv2.imshow('streaming', img)
cv2.waitKey(1)
def apriltag(self):
self.drone(
moveBy(0, 0, 0.5, 0) >> FlyingStateChanged(state="hovering",
_timeout=10)).wait()
# -*- coding: UTF-8 -*-
import olympe
from olympe.messages.ardrone3.Piloting import TakeOff, moveBy, Landing
drone = olympe.Drone("10.202.0.1")
drone.connect()
drone(TakeOff()).wait()
drone(moveBy(10, 0, 0, 0)).wait()
drone(Landing()).wait()
drone.disconnect()
# is optional for the default method, but you can use it to further
# restrict the event messages handled by this method or to limit the
# maximum size of it associated event queue (remember that the oldest
# events are dropped silently when the event queue is full).
@olympe.listen_event(queue_size=10)
def default(self, event, scheduler):
print_event(event)
if __name__ == "__main__":
drone = olympe.Drone(DRONE_IP)
# Explicit subscription to every event
every_event_listener = EveryEventListener(drone)
every_event_listener.subscribe()
drone.connect()
every_event_listener.unsubscribe()
# You can also subscribe/unsubscribe automatically using a with statement
with FlightListener(drone) as flight_listener:
for i in range(2):
assert drone(
FlyingStateChanged(state="hovering")
| (TakeOff() & FlyingStateChanged(state="hovering"))
).wait().success()
assert drone(moveBy(10, 0, 0, 0)).wait().success()
drone(Landing()).wait()
assert drone(FlyingStateChanged(state="landed")).wait().success()
assert flight_listener.has_observed_takeoff
drone.disconnect()
def goToDestination(destination):
if (destination == "A1"):
goToEntrance()
drone(
moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
elif (destination == "A2"):
goToEntrance()
drone(
moveBy(0, 2.3, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
drone(
moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
elif (destination == "A3"):
goToEntrance()
drone(
moveBy(0, 4.8, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
drone(
moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
elif (destination == "A4"):
goToEntrance()
drone(
moveBy(0, 7.1, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
drone(
moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
elif (destination == "A5"):
goToEntrance()
drone(
moveBy(0, 9.5, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
drone(
moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
elif (destination == "A6"):
goToEntrance()
drone(
moveBy(0, 12, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
drone(
moveBy(4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
elif (destination == "B1"):
goToEntrance()
drone(
moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
elif (destination == "B2"):
goToEntrance()
drone(
moveBy(0, 2.3, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
drone(
moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
elif (destination == "B3"):
goToEntrance()
drone(
moveBy(0, 4.8, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
drone(
moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
elif (destination == "B4"):
goToEntrance()
drone(
moveBy(0, 7.1, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
drone(
moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
elif (destination == "B5"):
goToEntrance()
drone(
moveBy(0, 9.5, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
drone(
moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
elif (destination == "B6"):
goToEntrance()
drone(
moveBy(0, 12, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
drone(
moveBy(-4.93, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
goToHome()
return
# Take-off
drone(
FlyingStateChanged(state="hovering", _policy="check")
| FlyingStateChanged(state="flying", _policy="check")
| (GPSFixStateChanged(fixed=1, _timeout=10, _policy="check_wait") >>
(TakeOff(_no_expect=True)
& FlyingStateChanged(
state="hovering", _timeout=10, _policy="check_wait")))).wait()
# Get the home position
drone_location = drone.get_state(GpsLocationChanged)
# Move 10m
drone(
moveBy(10, 0, 0, math.pi) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
# Go back home
drone(
moveTo(drone_location["latitude"], drone_location["longitude"],
drone_location["altitude"], MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> FlyingStateChanged(state="hovering", _timeout=5) >> moveToChanged(
latitude=drone_location["latitude"],
longitude=drone_location["longitude"],
altitude=drone_location["altitude"],
orientation_mode=MoveTo_Orientation_mode.TO_TARGET,
status='DONE',
_policy='wait') >> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
def yuv_frame_cb(self, yuv_frame):
"""
This function will be called by Olympe for each decoded YUV frame.
:type yuv_frame: olympe.VideoFrame
"""
# the VideoFrame.info() dictionary contains some useful informations
# such as the video resolution
info = yuv_frame.info()
height, width = info["yuv"]["height"], info["yuv"]["width"]
# convert pdraw YUV flag to OpenCV YUV flag
cv2_cvt_color_flag = {
olympe.PDRAW_YUV_FORMAT_I420: cv2.COLOR_YUV2BGR_I420,
olympe.PDRAW_YUV_FORMAT_NV12: cv2.COLOR_YUV2BGR_NV12,
}[info["yuv"]["format"]]
# yuv_frame.as_ndarray() is a 2D numpy array with the proper "shape"
# i.e (3 * height / 2, width) because it's a YUV I420 or NV12 frame
# Use OpenCV to convert the yuv frame to RGB
img = cv2.cvtColor(yuv_frame.as_ndarray(), cv2_cvt_color_flag)
blurred = cv2.GaussianBlur(img, (11, 11), 0)
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# Threshold the HSV image to get only certain colors
mask = cv2.inRange(hsv, greenLower, greenUpper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
mask = cv2.GaussianBlur(mask, (15, 15), 2, 2)
contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
#shape = cv2.drawContours(img, contours, -1, (0,255,0), 3)
vx = 0.0
vy = 0.0
vz = 0.0
vr = 0.0
marker_y = 0.0
marker_x = 0.0
kp = 0.001
global largest
for cnt in contours:
approx = cv2.approxPolyDP(cnt, 0.01 * cv2.arcLength(cnt, True),
True)
#print (len(approx))
if len(approx) == 4:
print("square")
cv2.drawContours(img, [cnt], 0, (0, 0, 255), -1)
elif len(approx) > 13:
print("circle")
cv2.drawContours(img, [cnt], 0, (0, 255, 255), -1)
if largest is None or cv2.contourArea(cnt) > cv2.contourArea(
largest):
if cv2.contourArea(cnt) < 1000000:
largest = cnt
if largest is not None:
cv2.drawContours(img, largest, -1, (0, 255, 0), 3)
M = cv2.moments(largest)
marker_y = int(M["m01"] / M["m00"])
marker_x = int(M["m10"] / M["m00"])
vx = 0.1
vy = 0.0
vz = kp * (img.shape[0] / 2 - marker_y)
vr = kp * (img.shape[1] / 2 - marker_x)
cv2.circle(img, (marker_x, marker_y), 2, (0, 0, 255), -1)
cv2.circle(img, (int(img.shape[1] / 2), int(img.shape[0] / 2)), 2,
(0, 0, 255), -1)
cv2.imshow('img', img)
self.drone(moveBy(vx, vy, -vz, -vr)).wait(10)
cv2.waitKey(1)
