def onPositionChanged(self, event, scheduler):
lat = event.args["latitude"]
lon = event.args["longitude"]
c_poi = (lat, lon)
poi = (21.371518, -157.71161)
print('\n ------->')
print('\n ------->')
print('\n ------->')
meters = haversine(poi, c_poi, unit='m')
print(meters)
print(meters < 2)
print('\n ------->')
print('\n ------->')
print('\n ------->')
two_lat = event.args["latitude"]
two_lon = event.args["longitude"]
if meters < 8:
casey(moveToChanged(status="CANCELED")
>> moveTo(21.370950, -157.709998, 10, MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> moveToChanged(status="DONE")
)
donatello(moveToChanged(status="CANCELED")
>> moveTo(21.370950, -157.709998, 20, MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> moveToChanged(status="DONE")
)
leonardo(moveToChanged(status="CANCELED")
>> moveTo(21.370950, -157.709998, 15, MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> moveToChanged(status="DONE")
)
two_lat = 21.370950
two_lon = -157.709998
casey_coords = findOffset(two_lat,two_lon,-5,0)
donatello_coords = findOffset(two_lat,two_lon,5,0)
leonardo_coords = findOffset(two_lat,two_lon,0,5)
michelangelo_coords = findOffset(event.args["latitude"],event.args["longitude"],0,-10)
raphael_coords = findOffset(event.args["latitude"],event.args["longitude"],10,10)
splinter_coords = findOffset(event.args["latitude"],event.args["longitude"],-10,-10)
casey(moveTo(casey_coords[0], casey_coords[1], 10, MoveTo_Orientation_mode.TO_TARGET, 0.0))
donatello(moveTo(donatello_coords[0], donatello_coords[1], 15, MoveTo_Orientation_mode.TO_TARGET, 0.0))
leonardo(moveTo(leonardo_coords[0], leonardo_coords[1], 20, MoveTo_Orientation_mode.TO_TARGET, 0.0))
michelangelo(moveTo(michelangelo_coords[0], michelangelo_coords[1], 25, MoveTo_Orientation_mode.TO_TARGET, 0.0))
raphael(moveTo(raphael_coords[0], raphael_coords[1], 30, MoveTo_Orientation_mode.TO_TARGET, 0.0))
splinter(moveTo(splinter_coords[0], splinter_coords[1], 35, MoveTo_Orientation_mode.TO_TARGET, 0.0))
print(
"latitude = {latitude} longitude = {longitude} altitude = {altitude}".format(
**event.args
)
)
def goHome(self):
print('ET going home...', end='', flush=True)
# Start navigating home
self.bebop(
moveTo(self.drone_location['latitude'],
self.drone_location['longitude'],
self.drone_location['altitude'], 1, 0.0) >>
FlyingStateChanged(state='hovering', _timeout=5) >> moveToChanged(
latitude=self.drone_location['latitude'],
longitude=self.drone_location['longitude'],
altitude=self.drone_location['altitude'],
orientation_mode=MoveTo_Orientation_mode.TO_TARGET,
status='DONE',
_policy='wait') >> FlyingStateChanged(state='hovering',
_timeout=5)).wait()
print('Done!')
def newCenterLocation(self, lat, lon):
# print('Flying state:', self.getFlyingState())
if self.getFlyingState() != 'landed':
self.bebop(
moveTo(lat, lon, self.drone_location['altitude'],
MoveTo_Orientation_mode.TO_TARGET, 0.0) >>
FlyingStateChanged(
state='hovering', _timeout=5) >> moveToChanged(
latitude=lat,
longitude=lon,
altitude=self.drone_location['altitude'],
orientation_mode=MoveTo_Orientation_mode.TO_TARGET,
status='DONE',
_policy='wait') >> FlyingStateChanged(state='hovering',
_timeout=5))
def moveToCoords(self):
self.bebop(FlyingStateChanged(state='hovering', _timeout=5))
print(self.bebop.get_state(moveToChanged))
# -25.766915, 28.251963 - Francois pool
lat = -25.766915
lon = 28.251963
# alt = self.drone_location['altitude']
alt = 4
print('Moving to coords...', end='', flush=True)
self.bebop(
moveTo(lat, lon, alt, MoveTo_Orientation_mode.TO_TARGET, 0.0) >>
FlyingStateChanged(state='hovering', _timeout=5) >> moveToChanged(
latitude=lat,
longitude=lon,
altitude=alt,
orientation_mode=MoveTo_Orientation_mode.TO_TARGET,
status='DONE',
_policy='wait') >> FlyingStateChanged(state='hovering',
_timeout=5))
print('Done!')
def flyTo(self, longitude, latitude, altitude=-1):
drone_location = self.drone.get_state(GpsLocationChanged)
if (altitude == -1):
altitude = drone_location["altitude"]
if (longitude == 0.0 and latitude == 0.0):
longitude = drone_location["longitude"]
latitude = drone_location["latitude"]
if (self.drone.get_state(FlyingStateChanged)['state'] !=
FlyingStateChanged_State.landed):
self.drone(
moveTo(latitude, longitude, altitude,
MoveTo_Orientation_mode.TO_TARGET, 0.0) >>
FlyingStateChanged(
state="hovering", _timeout=self.timeout) >> moveToChanged(
latitude=latitude,
longitude=longitude,
altitude=2,
orientation_mode=MoveTo_Orientation_mode.TO_TARGET,
status='DONE',
_policy='wait') >> FlyingStateChanged(
state="hovering", _timeout=self.timeout)).wait()
return True
else:
return False
| 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()
# Landing
drone(Landing() >> FlyingStateChanged(state="landed", _timeout=5)).wait()
drone.disconnection()
drone.connect()
casey.connect()
donatello.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(
FlyingStateChanged(state="hovering", _timeout=5) >> moveTo(
21.371518652227294, -157.71239, 15, MoveTo_Orientation_mode
.TO_TARGET, 0.0) >> moveToChanged(status="DONE") >> moveTo(
21.371518652227294, -157.7116182859902, 15,
MoveTo_Orientation_mode.TO_TARGET, 0.0) >>
moveToChanged(status="DONE") >> moveTo(
21.3708, -157.7116182859902, 15, MoveTo_Orientation_mode.
TO_TARGET, 0.0) >> moveToChanged(status="DONE") >> moveTo(
21.370081347772704, -157.7116182859902, 15,
MoveTo_Orientation_mode.TO_TARGET, 0.0) >>
moveToChanged(status="DONE") >> moveTo(
21.370081347772704, -157.71239, 15, MoveTo_Orientation_mode
.TO_TARGET, 0.0) >> moveToChanged(status="DONE") >> moveTo(
21.370081347772704, -157.7131617140098, 15,
MoveTo_Orientation_mode.TO_TARGET, 0.0) >>
moveToChanged(status="DONE") >> moveTo(
21.3708, -157.7131617140098, 15, MoveTo_Orientation_mode.
TO_TARGET, 0.0) >> moveToChanged(status="DONE") >> moveTo(
from olympe.messages.camera import start_recording, stop_recording
from olympe.messages import gimbal
from olympe.enums.ardrone3.Piloting import MoveTo_Orientation_mode
DRONE_IP = "10.202.0.1"
casey = olympe.Drone("10.202.1.1")
drone = olympe.Drone(DRONE_IP)
# if __name__ == "__main__":
# with olympe.Drone(DRONE_IP) as drone:
drone.connect()
casey.connect()
# Start a flying action asynchronously
drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5) >> moveTo(
21.37313, -157.7123, 15, MoveTo_Orientation_mode.NONE,
0.0) >> moveToChanged(status="DONE") >> moveTo(
21.37313, -157.7098, 15, MoveTo_Orientation_mode.NONE, 0.0) >>
moveToChanged(status="DONE") >> Landing())
casey(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5) >> moveTo(
21.3708, -157.709881, 15, MoveTo_Orientation_mode.NONE,
0.0) >> moveToChanged(status="DONE") >> moveTo(
21.368464, -157.71239, 15, MoveTo_Orientation_mode.NONE, 0.0) >>
moveToChanged(status="DONE") >> Landing()).wait().success()
# t:21.3708,lng:-157.709881 - east
# at:21.368464380261287,lng:-157.71239 - south
# Wait for the end of the flying action
# if not flyingAction.wait().success():
# assert False, "Cannot complete the flying action"
donatello.connect()
leonardo.connect()
michelangelo.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(
FlyingStateChanged(state="hovering", _timeout=5)
>> moveTo(21.371518652227294, -157.71239, 15, MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> moveToChanged(status="DONE")
>> moveTo(21.371518652227294, -157.7116182859902, 15, MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> moveToChanged(status="DONE")
>> moveTo(21.3708, -157.7116182859902, 15, MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> moveToChanged(status="DONE")
>> moveTo(21.370081347772704, -157.7116182859902, 15, MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> moveToChanged(status="DONE")
>> moveTo(21.370081347772704, -157.71239, 15, MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> moveToChanged(status="DONE")
>> moveTo(21.370081347772704, -157.7131617140098, 15, MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> moveToChanged(status="DONE")
>> moveTo(21.3708, -157.7131617140098, 15, MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> moveToChanged(status="DONE")
>> moveTo(21.371518652227294, -157.7131617140098, 15, MoveTo_Orientation_mode.TO_TARGET, 0.0)
>> moveToChanged(status="DONE")
).wait().success()
class FlightListener(olympe.EventListener):
def set_logger(self, logger):
self.my_log = logger;
def set_socket(self, websocket):
self._brain_client = websocket;
def set_relay(self, relay):
self._relay = relay
def set_drone_coordinates(self, coordinates):
self._drone_coordinates = coordinates
def set_initialized_socket(self, initialized):
self._socket_initialized = initialized;
def set_my_name(self, name):
self.my_name = name;
def on_signal_lost (self, func):
self._on_signal_lost = func;
@olympe.listen_event(NavigateHomeStateChanged())
def on_navigation_changed(self, event, scheduler):
global navigate_home_status_lookup
try :
if hasattr(self, "my_log"):
self.my_log.info("{} = {}".format(event.message.name, event.args["state"]))
self.emit_status("navigate_home_status_changed", navigate_home_status_lookup.index(event.args["state"]))
except(Exception) as err :
if hasattr(self, "my_log"):
self.my_log.info(err)
@olympe.listen_event(FlyingStateChanged())
def on_state_changed(self, event, scheduler):
global flying_status_lookup
try :
if hasattr(self, "my_log"):
self.my_log.info("{} = {}".format(event.message.name, event.args["state"]))
self.emit_status("flight_status_changed", flying_status_lookup.index(event.args["state"]))
except(Exception) as err :
if hasattr(self, "my_log"):
self.my_log.info(err)
@olympe.listen_event(AlertStateChanged() )
def on_alert_raised(self, event, scheduler):
global alert_lookup
try :
if hasattr(self, "my_log"):
self.my_log.info("{} = {}".format(event.message.name, event.args["state"]))
self.emit_status("on_alert_changed", alert_lookup.index(event.args["state"]))
except(Exception) as err :
if hasattr(self, "my_log"):
self.my_log.info(err)
@olympe.listen_event(PositionChanged() | moveToChanged())
def on_position_changed(self, event, scheduler):
if hasattr(self, "my_log"):
self.my_log.info(
"latitude = {} longitude = {} altitude = {}".format(
event.args["latitude"],
event.args["longitude"],
event.args["altitude"]
)
)
if hasattr(self, "_drone_coordinates"):
self._drone_coordinates.update_position(event.args["latitude"], event.args["longitude"], event.args["altitude"])
self.on_element_position_update('position_update', event)
@olympe.listen_event(HomeChanged())
def on_home_changed(self, event, scheduler):
if hasattr(self, "my_log"):
self.my_log.info(
"latitude = {} longitude = {} altitude = {}".format(
event.args["latitude"],
event.args["longitude"],
event.args["altitude"]
)
)
if hasattr(self, "_relay"):
self._relay.update_position(event.args["latitude"], event.args["longitude"], event.args["altitude"])
self.on_element_position_update('home_position_update', event)
"""
@olympe.listen_event(
FlyingStateChanged(state="motor_ramping")
>> FlyingStateChanged(state="takingoff", _timeout=1.)
>> FlyingStateChanged(state="hovering", _timeout=5.)
)
def on_take_off(self, event, scheduler):
if hasattr(self, "my_log"):
self.my_log.info("Décollage du drone")
#event.message.name
self.emit_status("takeoff", event.args["success"])
"""
@olympe.listen_event(rssi_changed())
def on_signal_changed(self, event, scheduler):
if hasattr(self, "my_log"):
self.my_log.info(event.args)
self.on_internal_status_changed(event.message.name, event.args["rssi"])
if hasattr(self, "_on_signal_lost") and event.args["rssi"] <= -120:
self._on_signal_lost()
@olympe.listen_event(NumberOfSatelliteChanged())
def on_satellite_changed(self, event, scheduler):
if hasattr(self, "my_log"):
self.my_log.info(event);
self.on_internal_status_changed(event.message.name, event.args["numberOfSatellite"])
@olympe.listen_event(BatteryStateChanged())
def on_battery_update(self, event, scheduler):
if hasattr(self, "my_log"):
self.my_log.info(event);
self.on_internal_status_changed(event.message.name, event.args["percent"])
@olympe.listen_event(GPSFixStateChanged())
def on_gps_changed(self, event, scheduler):
if hasattr(self, "my_log"):
self.my_log.info(event);
self.on_internal_status_changed(event.message.name, event.args["fixed"])
def on_internal_status_changed(self, name, value):
try :
if hasattr(self, "_socket_initialized") and hasattr(self, "my_name") and hasattr(self,"_brain_client") and self._socket_initialized:
self._brain_client.emit('internal_status_changed', {"id":self.my_name, "status":name, "result":value})
except(Exception) as error :
if hasattr(self, "my_log"):
self.my_log.error("Une erreur est survenue lors de la mise à jour des états interne")
self.my_log.error(error)
self.my_log.error("{} = {}".format(value.message.name, value.args))
def on_element_position_update(self, event, value):
try :
if hasattr(self, "_socket_initialized") and hasattr(self, "my_name") and hasattr(self,"_brain_client") and self._socket_initialized:
self._brain_client.emit(event, {"id":self.my_name,"latitude": value.args["latitude"], "longitude": value.args["longitude"], "altitude": value.args["altitude"] })
except(Exception) as error :
if hasattr(self, "my_log"):
self.my_log.error("Une erreur est survenue lors de la mise à jour des positions")
self.my_log.error(error)
self.my_log.error("{} = {}".format(value.message.name, value.args))
def emit_status(self, name, value):
try :
if hasattr(self, "_socket_initialized") and hasattr(self, "my_name") and hasattr(self,"_brain_client") and self._socket_initialized:
self._brain_client.emit(name, {"drone_name":self.my_name, "status":value})
except(Exception) as error :
if hasattr(self, "my_log"):
self.my_log.error("Une erreur est survenue lors de la mise à jour des états interne")
self.my_log.error(error)
self.my_log.error("{} = {}".format(value.message.name, value.args))
print("Drone moving "+str(FW*math.cos(DIRECTION*math.pi/180))+" m forward and "+str(FW*math.sin(DIRECTION*math.pi/180))+" up.")
drone(
moveBy(FW*math.cos(DIRECTION*math.pi/180), 0, -FW*math.sin(DIRECTION*math.pi/180), 0)
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait()
saver = threading.Thread(target=save_packets, args=(seconds, paths_CR1[STEP], CR1, paths_CR2[STEP], CR2))
saver.start()
saver.join()
print("\n\n\n ---- Experience finished, Back to home ---- \n\n\n")
drone(
moveTo(drone_home['latitude'], drone_home['longitude'], drone_home['altitude'], MoveTo_Orientation_mode.NONE, 0.0)
>> moveToChanged(status='DONE')
).wait()
print("\n\n\n ---- Landing... ---- \n\n\n")
drone(
Landing()
).wait()
print("\n\n\n ---- Drone landed ---- \n\n\n")
#Leaving the with statement and disconnecting the drone.
print("Ending thread:", threading.current_thread().name)
str(FW * math.cos(DIRECTION * math.pi / 180)) +
" m forward and " +
str(FW * math.sin(DIRECTION * math.pi / 180)) + " up.")
drone(
moveBy(FW * math.cos(DIRECTION * math.pi / 180), 0, -FW *
math.sin(DIRECTION * math.pi / 180), 0) >>
FlyingStateChanged(state="hovering", _timeout=5)).wait()
saver = threading.Thread(target=save_packets,
args=(seconds, paths[STEP]))
saver.start()
saver.join()
print("\n\n\n ---- Experience finished, Back to home ---- \n\n\n")
drone(
moveTo(drone_home['latitude'], drone_home['longitude'],
drone_home['altitude'], MoveTo_Orientation_mode.NONE, 0.0)
>> moveToChanged(status='DONE')).wait()
print("\n\n\n ---- Landing... ---- \n\n\n")
drone(Landing()).wait()
print("\n\n\n ---- Drone landed ---- \n\n\n")
#Leaving the with statement and disconnecting the drone.
print("Ending thread:", threading.current_thread().name)
raphael.connect()
splinter.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(
FlyingStateChanged(state="hovering", _timeout=5) >> moveTo(
21.43378258952731, -157.7882350477468, 1,
MoveTo_Orientation_mode.TO_TARGET, 0.0) >> moveToChanged(
status="DONE") >> moveTo(
21.43378258971239, -157.7882350475352, 11,
MoveTo_Orientation_mode.TO_TARGET, 0.0) >>
moveToChanged(status="DONE") >> moveTo(
21.43378258989746, -157.7882350473236, 21,
MoveTo_Orientation_mode.TO_TARGET, 0.0) >> moveToChanged(
status="DONE") >> moveTo(
21.43378259008253, -157.7882350471119, 31,
MoveTo_Orientation_mode.TO_TARGET,
0.0) >> moveToChanged(status="DONE") >> moveTo(
21.4337825902676, -157.7882350469003, 41,
MoveTo_Orientation_mode.TO_TARGET, 0.0) >>
moveToChanged(status="DONE") >> moveTo(
21.43378259045268, -157.7882350466886, 51,
MoveTo_Orientation_mode.TO_TARGET, 0.0) >> moveToChanged(
status="DONE") >> moveTo(