def fly(self):
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(0.1, 0, 0, math.pi, _timeout=20)).wait().success()
print("Landing...")
self.drone(
Landing()
>> FlyingStateChanged(state="landed", _timeout=5)
).wait()
print("Landed\n")
def casting_with_moveby(tc, casting_memory):
"""
If no new plume is detected, we use CASTING. Pick a random angle between 30 to 120 degrees, turn CCW to face this degree and fly for tc seconds.
Continue this pattern until a new plume is detected or until a miximum ammout of turns which we will define as a "failed attempt".
There is a need to "remember" every counterturn the Drone makes in order to fly back "upwind" (degree 0) once a plume is detected.
:param tc - time in plume [sec]
:return: True if plume detected
"""
degree = random.randint(30, 120)
rad = degree * math.pi / 180
print("degree: ", degree)
print("rad: ", rad)
casting_memory.append(rad)
# turn:
# olympe.messages.ardrone3.Piloting.moveBy(dPsi=rad).wait()
print("casting turn: ", degree)
drone(
moveBy(dX=0, dY=0, dZ=0, dPsi=rad) >> FlyingStateChanged(
state="hovering", _timeout=20)).wait()
# print("we turned: ", rad)
print("casting fly: ", dx(tc))
drone(
moveBy(dX=dx(tc), dY=0, dZ=0, dPsi=0) >> FlyingStateChanged(
state="hovering", _timeout=20)).wait()
# print(" moved after casting for time tc = ", tc)
return casting_memory
class FlightListener(olympe.EventListener):
# This set a default queue size for every listener method
default_queue_size = 100
def __init__(self, *args, **kwds):
super().__init__(*args, **kwds)
self.has_observed_takeoff = False
@olympe.listen_event(PositionChanged())
def onPositionChanged(self, event, scheduler):
casey(moveTo(event.args["latitude"], event.args["longitude"], 0.86, MoveTo_Orientation_mode.TO_TARGET, 0.0))
print(
"latitude = {latitude} longitude = {longitude} altitude = {altitude}".format(
**event.args
)
)
@olympe.listen_event(AttitudeChanged())
def onAttitudeChanged(self, event, scheduler):
print("roll = {roll} pitch = {pitch} yaw = {yaw}".format(**event.args))
@olympe.listen_event(AltitudeAboveGroundChanged())
def onAltitudeAboveGroundChanged(self, event, scheduler):
print("height above ground = {altitude}".format(**event.args))
@olympe.listen_event(SpeedChanged())
def onSpeedChanged(self, event, scheduler):
print("speedXYZ = ({speedX}, {speedY}, {speedZ})".format(**event.args))
# You can also handle multiple message types with the same method
@olympe.listen_event(
FlyingStateChanged() | AlertStateChanged() | NavigateHomeStateChanged()
)
def onStateChanged(self, event, scheduler):
# Here, since every "*StateChanged" message has a `state` argument
# we can handle them uniformly to print the current associated state
print("{} = {}".format(event.message.name, event.args["state"]))
# You can also monitor a sequence of event using the complete Olympe DSL syntax
@olympe.listen_event(
FlyingStateChanged(state="takingoff", _timeout=1.)
>> FlyingStateChanged(state="hovering", _timeout=5.)
)
def onTakeOff(self, event, scheduler):
# This method will be called once for each completed sequence of event
# FlyingStateChanged: motor_ramping -> takingoff -> hovering
caseyTakeOff()
print("The drone has taken off!")
self.has_observed_takeoff = True
# The `default` listener method is only called when no other method
# matched the event message The `olympe.listen_event` decorator usage
# 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)
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()
# Setup your callback functions to do some live video processing
self.drone.set_streaming_callbacks(raw_cb=self.yuv_frame_cb)
# Start video streaming
self.drone.start_video_streaming()
def move6(self): #tag=7
self.drone(
moveBy(-0.8, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
self.drone(Landing() >> FlyingStateChanged(state="hovering",
_timeout=10)).wait()
time.sleep(1)
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(
)
df = pd.read_csv('route.csv')
#self.drone(moveBy(4, 2, 0, 0)>> FlyingStateChanged(state="hovering", _timeout=5)).wait()
while (True):
for i in range(len(df)):
print('-----------------------------------',
(df.X[i] / 4) - 10.125, (df.Y[i] / 4) - 10.125,
'------------------------------------------')
self.positive_control((df.X[i] / 4) - 10.125,
(df.Y[i] / 4) - 10.125)
self.drone(
PCMD(1, 0, 0, 0, 0, timestampAndSeqNum=0, _timeout=10))
#self.positive_control(-7.2,-7.2)
#self.drone(PCMD(1, 0, 0, 0, 0, timestampAndSeqNum=0, _timeout=10))
self.drone(Landing())
self.drone.disconnection()
def perform(operation, distance):
for action in operation:
if action == location.UP:
log.log(log.INFO, "Action sent: UP")
if action == location.DOWN:
log.log(log.INFO, "Action sent: DOWN")
if action == location.LEFT:
log.log(log.INFO, "Action sent: LEFT")
drone(
moveBy(0, 1 * distance.x, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait()
if action == location.RIGHT:
log.log(log.INFO, "Action sent: RIGHT")
drone(
moveBy(0, -1 * distance.x, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait()
if action == location.FORWARD:
log.log(log.INFO, "Action sent: FWD")
drone(
moveBy(1 * distance.y, 0, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait()
if action == location.BACKWARD:
log.log(log.INFO, "Action sent: BACKWARD")
drone(
moveBy(-1 * distance.y, 0, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait()
return
def takeOff(self, client):
print("Taking off")
if (self.drone.get_state(FlyingStateChanged)['state'] ==
FlyingStateChanged_State.landed):
self.drone(
FlyingStateChanged(state="hovering", _policy="check")
| FlyingStateChanged(state="flying", _policy="check")
| (GPSFixStateChanged(
fixed=1, _timeout=self.timeout, _policy="check_wait") >>
(TakeOff(_no_expect=True)
& FlyingStateChanged(state="hovering",
_timeout=self.timeout,
_policy="check_wait")))).wait()
messageToSend = {
"op": "takeOff",
"userId": client.userId,
"response": 1
}
self.flying = True
else:
messageToSend = {
"op": "takeOff",
"userId": client.userId,
"response": 0
}
client.send((json.dumps(messageToSend) + "\n"))
def take_off_turn_and_land(drone):
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
sleep(5)
# moveBy((1, 0, 0, 0) >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
# moveBy((0, 0, 0, math.pi) >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
drone.disconnection()
def start(self):
self.drone.connection()
self.drone.start_piloting()
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(3.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.
# 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, )
# Start video streaming
self.drone.start_video_streaming()
while True:
cv2.waitKey(1)
def main():
drone = olympe.Drone(DRONE_IP)
drone.connect()
drone(
TakeOff()
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
drone(
moveBy(3, 0, 0, 0)
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
drone(
moveBy(0, 4, 0, 0)
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
drone(
moveBy(-3, 0, 0, 0)
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
drone(
moveBy(0, -4, 0, 0)
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait().success()
drone(Landing()).wait()
drone.disconnect()
def Drone_Actn(xa, drone):
print(colored(('Your Message: ', xa), "blue"))
X_Ref = []
X_Home = [1, 2.5, 0.75, 0, 0, 0, 0, 0, 3*pi/2]
if xa==1:
if (drone.get_state(FlyingStateChanged)["state"] is not FlyingStateChanged_State.hovering):
print(colored(('Initating Drone Take off !'), "green"))
drone(TakeOff(_no_expect=True) & FlyingStateChanged(state="hovering", _policy="wait", _timeout=5)
).wait()
# drone.start_piloting()
else:
print(colored(('Drone has already taken off !'), "green"))
if xa == 2:
X_Ref = X_Home
X_Ref[0] = 1
X_Ref[2] = 0.75
elif xa==3:
X_Ref = [0, 0, 1.5, 0, 0, 0, 0, 0, 0]
elif xa==4:
X_Ref = X_Home
elif xa==5:
X_Ref = [1.2, -2.5, 1.3, 0, 0, 0, 0, 0, 0]
elif xa==6:
X_Ref = [1, 0, 1.5, 0, 0, 0, 0, 0, pi]
elif xa==7:
X_Ref = X_Home
elif xa==8:
X_Ref = X_Home
elif xa==9:
X_Ref = X_Home
elif xa==10:
drone(moveBy(0, 0, 0, -5*pi/180)
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
elif xa==11:
print(colored(('Starting to Land !'), "green"))
drone.stop_piloting()
drone(Landing()).wait()
time.sleep(5)
drone(Emergency()).wait()
print(colored(('Drone Landed !'), "green"))
else:
print('Currently Not programmed')
return X_Ref
def test_0_take_off_turn_and_land(drone):
drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
drone.start_video_streaming()
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
sleep(5)
drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
drone.stop_video_streaming()
drone.disconnection()
def takeOff(drone):
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()
def test_enums():
drone = olympe.Drone(DRONE_IP)
drone.connect()
assert drone(FlyingStateChanged(state="landed")).wait(5).success()
# is equivalent to
assert drone(
FlyingStateChanged(state=FlyingState.landed)).wait(5).success()
drone.disconnect()
def takeOff(self):
print('<< takeOff() >>')
self.drone(
TakeOff() >> FlyingStateChanged("hovering", _timeout=5)).wait()
self.drone(
moveBy(0, 0, (1.0 - self.flight_altitude), 0) >>
FlyingStateChanged("hovering", _timeout=5)).wait()
def fly(self):
print("TakingOff")
self.drone(TakeOff() >> FlyingStateChanged(state="hovering",
_timeout=10)).wait()
print("Rotate heading by -180 degrees")
self.drone(
moveBy(0, -1.15, -0.5, -3.142) >> FlyingStateChanged(
state="hovering", _timeout=10)).wait()
def flight_algorithm():
"""
Flight algorithm. Run flight.py from the terminal to initiate drone.
"""
# test maximum run time until considered failure [sec]:
test_max_time = 6000
casting_memory = []
# set drone param:
drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
drone.start_video_streaming()
change_gimbal_angle(-60)
sleep(1)
# take off:
print("------take off-------")
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
sleep(1)
# wait for first plume detection, while not detected stay in place, if detected move forward:
wait_to_start()
tc = count_time_and_fly_straight(casting_memory)
# initialize test start time, and casting angle memory so we can fly "straight" once detect() is True.
# in the future the drone will need to be able to analyze where the wind is coming from and fly in that direction.
casting_memory = []
test_start_time = time.time()
########### main algorithm: ###########
# while the drone hasn't reached the source (='success')
while read_detect_func_output() != 'success' and (
time.time() - test_start_time) < test_max_time:
print("----started flight algorithm loop------")
if read_detect_func_output() is True:
tc = count_time_and_fly_straight(casting_memory)
casting_memory = []
if read_detect_func_output() is False:
print(
"----------didnt find next plume going into casting mode----------"
)
if wait_for_plume_or_until_t(tc, casting_memory) is False:
print("----------start casting----------")
casting_memory = casting_with_moveby(tc, casting_memory)
######## end of run #########
print("...landing...")
reset_gimbal_orientation()
drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
drone.stop_video_streaming()
drone.disconnection()
def test_moveby2():
drone = olympe.Drone(DRONE_IP)
drone.connect()
assert drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)
).wait().success()
assert drone(
moveBy(10, 0, 0, 0) >> FlyingStateChanged(state="hovering", _timeout=5)
).wait().success()
assert drone(Landing()).wait().success()
drone.disconnect()
def move3(self): #tag=2 change
self.drone(
moveBy(-0.25, 0, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait() #change
self.drone(
moveBy(0, -0.4, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait() #change
self.drone(
moveBy(0, 0, 0, -1.571) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
def move7(self): #tag=3
self.drone(
moveBy(0, 0, -0.2, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait() #change
self.drone(
moveBy(0.9, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
self.drone(
moveBy(0, 0, 0, -1.571) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
def move(index, coords):
print(route[index + 1][0])
april(
moveTo(route[index + 1][0], route[index +
1][1], 1, MoveTo_Orientation_mode.
TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
casey(
moveTo(route[index][0], route[index][1], 0.8, MoveTo_Orientation_mode.
TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
def _takeoff(self):
""""""
print("Takeoff if necessary...")
self.drone(
FlyingStateChanged(state="hovering", _policy="check")
| FlyingStateChanged(state="flying", _policy="check")
| (GPSFixStateChanged(fixed=1, _timeout=5, _policy="check_wait") >>
(TakeOff(_no_expect=True)
& FlyingStateChanged(
state="hovering", _timeout=5, _policy="check_wait")))
).wait()
def start(self):
# Connect the the drone
self.drone.connection()
self.drone.start_piloting()
self.drone(TakeOff()>> FlyingStateChanged(state="hovering", _timeout=10)).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()
self.drone.set_streaming_callbacks(
raw_cb=self.yuv_frame_cb,
h264_cb=self.h264_frame_cb
)
self.drone.start_video_streaming()
vxx=0;
vyy=0;
while True:
for event in pygame.event.get():
if event.type==pygame.QUIT:
running=False
elif event.type==pygame.KEYUP:
vxx=0
vyy=0
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:
vxx=20
elif event.key==pygame.K_s:
vxx=-20
elif event.key==pygame.K_a:
vyy=-20
elif event.key==pygame.K_d:
vyy=20
self.drone.piloting_pcmd(vyy, vxx, 0, 0, 0.5) # (roll, pitch, yaw, gaz, piloting_time)
#time.sleep(1)
pygame.display.flip()
cv2.waitKey(1)
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 move2(self): #tag=1
self.drone(
moveBy(-0.5, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
self.drone(
moveBy(0, -1, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
self.drone(
moveBy(1, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
self.drone(
moveBy(0, 1, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
def move4(self): #tag=4
self.drone(
moveBy(-0.5, 0, 0, 0) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
self.drone(
moveBy(0, -0.9, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait() #change
self.drone(
moveBy(2.8, 0, 0, 0) >> FlyingStateChanged(
state="hovering", _timeout=5)).wait() #change
self.drone(
moveBy(0, 0, 0, 1.571) >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
def test_1_fly_straight_1_meter():
drone.set_streaming_callbacks(raw_cb=yuv_frame_cb)
drone.start_video_streaming()
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
#
# drone(
# moveBy(dX=2, dY=0, dZ=0, dPsi=0)
# >> FlyingStateChanged(state="hovering", _timeout=5)
# ).wait()
sleep(100)
drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
drone.stop_video_streaming()
drone.disconnection()
def fly(self):
# Takeoff, fly, land, ...
"""
Uncomment the code snippet below, enabiling the user to use the Sky Remote on the drone
while the drone can still returns Video feed with Operation predictions on Screen.
"""
"""
# 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()
"""
# This landing condition is used as a redundancy incase of stop() failing.
if read_navigation_output == 'stop':
print("Landing...")
self.drone(Landing() >> FlyingStateChanged(state="landed",
_timeout=5)).success()
print("Landed\n")
print("***** Takeoff if necessary *****")
self.drone(
TakeOff(_no_expect=True) >> FlyingStateChanged(
state="hovering", _timeout=10, _policy="check_wait")).wait(
).success()
# If the UAV has not stopped
while read_navigation_output() != 'stop':
print("**** Navigation has started ****")
if read_navigation_output() == 'left':
self.drone(moveBy(0, 0, 0, -0.2)).wait().success()
print('LEFT')
if read_navigation_output() == 'right':
self.drone(moveBy(0, 0, 0, 0.2)).wait().success()
print('RIGHT')
if read_navigation_output() == 'centre':
self.drone(moveBy(0, 0, 0, 0)).success()
print('CENTRE')
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()
