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(