def onTakeOff(self, event, scheduler):
# This method will be called once for each completed sequence of event
# FlyingStateChanged: motor_ramping -> takingoff -> hovering
# followerTakeOff(casey)
# followerTakeOff(donatello)
# followerTakeOff(leonardo)
# followerTakeOff(michelangelo)
casey(TakeOff())
donatello(TakeOff())
leonardo(TakeOff())
michelangelo(TakeOff())
print("The drone has taken off!")
self.has_observed_takeoff = True
def _takeoff(self):
""""""
print("Takeoff if necessary...")
self.drone(
TakeOff(_no_expect=True)
& FlyingStateChanged(
state="hovering", _timeout=5, _policy="check_wait")).wait()
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 main():
april = olympe.Drone(april_ip)
casey = olympe.Drone(casey_ip)
donatello = olympe.Drone(donatello_ip)
leonardo = olympe.Drone(leonardo_ip)
michelangelo = olympe.Drone(michelangelo_ip)
raphael = olympe.Drone(raphael_ip)
splinter = olympe.Drone(splinter_ip)
swarm = [
april, casey, donatello, leonardo, michelangelo, raphael, splinter
]
for drone in swarm:
drone.connect()
for drone in swarm:
drone(TakeOff()).wait().success()
for drone in swarm:
drone(moveBy(0, 0, -2, 0)).wait()
for drone in swarm: # x = back/forward z = right/left y = up/down
drone(moveBy(0, -2, 0, 0)).wait() # drone moves up left/-Z by 3
for drone in swarm: # x = back/forward z = right/left y = up/down
drone(moveBy(2, 0, 0, 0)).wait() # drone moves up foreward by 3
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 init():
global drone
drone = olympe.Drone("10.202.0.1")
drone.connection()
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
return
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 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 main():
drone = olympe.Drone(DRONE_IP)
drone.connect()
assert drone(TakeOff()).wait().success()
time.sleep(10)
assert drone(Landing()).wait().success()
drone.disconnect()
def test_moveby():
drone = olympe.Drone(DRONE_IP)
drone.connect()
assert drone(TakeOff()).wait().success()
drone(moveBy(10, 0, 0, 0)).wait()
assert drone(Landing()).wait().success()
drone.disconnect()
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 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 CommonTakeOff(self):
if not self._on_test:
if (self._drone.get_state(FlyingStateChanged)["state"] is not FlyingStateChanged_State.hovering):
self._drone(GPSFixStateChanged(fixed=1, _timeout=10, _policy="check_wait")).wait()
self._drone( TakeOff(_no_expect=True) & FlyingStateChanged(state="hovering", _policy="wait", _timeout=5) ).wait()
else :
self.my_log.info("Mode simulation: informations reçues")
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 takeoff(self):
assert self.drone(TakeOff() >> FlyingStateChanged(
state="hovering", _timeout=5)).wait().success()
print(
"---------------------------------------------------SUCCESSFUL TAKEOFF---------------------------------------------------------------------"
)
return
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 start(self):
self.drone.connection()
self.drone.start_piloting()
self.drone(TakeOff()>> FlyingStateChanged(state="hovering", _timeout=10)).wait()
time.sleep(1)
self.drone.set_streaming_callbacks(raw_cb=self.yuv_frame_cb,)
self.drone.start_video_streaming()
def main():
# start polling thread for xbox controller
js = SimpleJoystick.SimpleJoystick()
js_thread = threading.Thread(target=js.poll)
js_thread.start()
print('controller connected')
# connect to drone
drone = olympe.Drone("10.202.0.1")
drone.connection()
print('drone connected')
# Take off and then wait for a maximum of 5 seconds for the drone to start hovering
print('taking off')
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait()
# start piloting interface
print('starting piloting')
olympe.Drone.start_piloting(drone)
#olympe.Drone.piloting_pcmd(drone,50,0,0,0,5.0)
continue_flying = True
while continue_flying:
# poll from the xbox controller
if js.button_states['b']:
continue_flying = False
# get roll,pitch,yaw,gaz values from controller
roll = int(js.axis_states['rx'] * 100.0)
pitch = int(js.axis_states['ry'] * -100.0)
yaw = int(js.axis_states['x'] * 100.0)
gaz = int(js.axis_states['y'] * -100.0)
if abs(roll) <= 5:
roll = 0
if abs(pitch) <= 5:
pitch = 0
if abs(yaw) <= 5:
yaw = 0
if abs(gaz) <= 5:
gaz = 0
# send commands to drone
olympe.Drone.piloting_pcmd(drone, roll, pitch, yaw, gaz, 1.0)
# stop piloting interface
print('stop piloting')
#drone(stop_piloting()).wait()
olympe.Drone.stop_piloting(drone)
# land drone and disconnect
print('drone landing')
drone(Landing()).wait()
drone.disconnection()
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 takeoff(self):
# Take off and then wait for a maximum of 5 seconds for the drone to start hovering
print('taking off')
self.drone(TakeOff() >> FlyingStateChanged(state="hovering",
_timeout=5)).wait()
# start piloting
print('starting piloting')
olympe.Drone.start_piloting(self.drone)
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 test_log():
drone = olympe.Drone(DRONE_IP, name="toto")
drone.connect()
if DRONE_RTSP_PORT is not None:
drone.streaming.server_addr = f"{DRONE_IP}:{DRONE_RTSP_PORT}"
assert drone(TakeOff()).wait().success()
assert drone.streaming.play()
time.sleep(10)
assert drone.streaming.stop()
assert drone(Landing()).wait().success()
drone.disconnect()
def ManualTakeOff(self):
if self._manual_unit :
if not self._on_test:
if (self._drone.get_state(FlyingStateChanged)["state"] is not FlyingStateChanged_State.hovering
and self._drone.get_state(FlyingStateChanged)["state"] is not FlyingStateChanged_State.flying):
self._drone(TakeOff(_no_expect=True)).wait()
self._brain_client.emit('on_manual_command', {"name":self.my_name, "command":self.ManualTakeOff.__name__})
else :
self.my_log.info("Mode simulation: informations reçues")
else :
self.my_log.info("Mode automatique : commande ManualTakeOff ignorée ")
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 AutomaticTakeOff(self):
if not self._manual_unit :
if not self._on_test:
if (self._drone.get_state(FlyingStateChanged)["state"] is not FlyingStateChanged_State.hovering
and self._drone.get_state(FlyingStateChanged)["state"] is not FlyingStateChanged_State.flying):
self._drone(TakeOff(_no_expect=True) & FlyingStateChanged(state="hovering", _policy="wait", _timeout=5) ).wait()
else :
self.my_log.info("Mode simulation: informations reçues ")
time.sleep(1)
self._brain_client.emit('on_command_success', {"name":self.my_name, "command":"AutomaticTakeOff"})
else :
self.my_log.info("Mode manuel : commande ignorée ")
def main():
drone = olympe.Drone(DRONE_IP)
drone.connect()
assert drone(TakeOff()).wait().success()
pub = rospy.Publisher('chatter', String, queue_size=10)
rospy.init_node('talker', anonymous=True)
hello_str = "hello world %s"
pub.publish(hello_str)
#print("Take off")
time.sleep(10)
assert drone(Landing()).wait().success()
#print("Landing")
drone.disconnect()
