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 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 move(index, coords): april( moveTo(route[index + 6][0], route[index + 6][1], 0.7, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=1)).wait().success() casey( moveTo(route[index][0], route[index][1], 1, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=2)).wait().success() donatello( moveTo(route[index + 6][0], route[index + 6][1], 0.7, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=3)).wait().success() leonardo( moveTo(route[index][0], route[index][1], 1, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=4)).wait().success() michelangelo( moveTo(route[index + 6][0], route[index + 6][1], 0.7, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() raphael( moveTo(route[index][0], route[index][1], 1, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() splinter( moveTo(route[index + 6][0], route[index + 6][1], 0.7, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success()
def roll_right(): drone(PCMD( 1, 10, 0, 0, 0, 10, ))
def roll_left(): drone(PCMD( 1, -10, 0, 0, 0, 10, ))
def decrease_throttle(): drone(PCMD( 0, 0, 0, 0, -10, 10, ))
def increase_throttle(): drone(PCMD( 0, 0, 0, 0, 10, 10, ))
def pitch_back(): drone(PCMD( 1, 0, -10, 0, 0, 10, ))
def pitch_fwd(): drone(PCMD( 1, 0, 10, 0, 0, 10, ))
def turn_right(): display_message('Turning right...') drone(PCMD( 1, 0, 0, 10, 0, 10, ))
def turn_left(): display_message('Turning left...') drone(PCMD( 1, 0, 0, -10, 0, 10, ))
def moveSwarm(poi): casey( moveTo(poi["latitude"], poi["longitude"], 0.9, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() donatello( moveTo(poi["latitude"], poi["longitude"], 0.8, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() leonardo( moveTo(poi["latitude"], poi["longitude"], 1.1, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() michelangelo( moveTo(poi["latitude"], poi["longitude"], 0.7, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() raphael( moveTo(poi["latitude"], poi["longitude"], 1.2, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() splinter( moveTo(poi["latitude"], poi["longitude"], 0.6, MoveTo_Orientation_mode. TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success()
def ManualTilt(self, params): #if not self._joystick_ctrl : # self.my_log.info("Mode DESKTOP : commande ignorée ") # return; if isinstance(params, dict): params = DroneCommandParams(**params) if self._manual_unit : if not self._on_test: self._drone(PCMD(params.flag,params.roll,params.pitch,params.yaw,params.throttle, timestampAndSeqNum=0)) else : self.my_log.info("Mode simulation: informations reçues") else : self.my_log.info("Mode automatique : commande ManualTilt ignorée ")
def step(self, action): # Takes action within set boundary limits with PCMD command, and updates state of the drone. self.pos_feedback() x=self.agent_pos[0] y=self.agent_pos[1] z=self.agent_pos[2] y_act = action[0] x_act = action[1] z_act = action[2] # Define bounded action if x>5.0: x_act = min(0,action[1]) if y<-5.0: y_act = min(0,action[0]) if x<-5: x_act = max(0,action[1]) if y>5: y_act = max(0,action[0]) if z<1: z_act = max(0,action[2]) if z>5: z_act = min(0,action[2]) self.drone(PCMD(1, y_act, x_act, 0, z_act, timestampAndSeqNum=0, _timeout=10)>> 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]] d = self.distance([obs[0],obs[1],obs[2]]) #Terminating Condition and reward design done = bool(d < 0.5) if bool(d < 0.5): reward = +100 else: reward = -1*(d) print('------------STEPS-------------',self.counter,'------------STEPS-------------') print('------------REWARD----------',reward,'------------REWARD----------') info = {} self.counter += 1 row = [self.counter,reward] with open('reward.csv', 'a', newline='') as csvFile: writer = csv.writer(csvFile) writer.writerow(row) csvFile.close() print('------------STATE-------------',self.agent_pos,'------------STATE-------------') return np.array(obs).astype(np.float32), reward, done, info
def rpyt_callback(self, msg): self.drone( PCMD( # https://developer.parrot.com/docs/olympe/arsdkng_ardrone3_piloting.html#olympe.messages.ardrone3.Piloting.PCMD flag=1, roll=int(self.bound_percentage( msg.roll / self.max_tilt * 100)), # roll [-100, 100] (% of max tilt) pitch=int( self.bound_percentage( msg.pitch / self.max_tilt * 100)), # pitch [-100, 100] (% of max tilt) yaw=int( self.bound_percentage( -msg.yaw / self.max_rotation_speed * 100)), # yaw rate [-100, 100] (% of max yaw rate) gaz=int( self.bound_percentage(msg.gaz / self.max_vertical_speed * 100) ), # vertical speed [-100, 100] (% of max vertical speed) timestampAndSeqNum=0)) # for debug only
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 performPilotingCommands(self, dX, dY, dZ, mX, mY, mZ): if self.detection_enabled == True: yaw = 0 gaz = 0 roll = 0 pitch = 0 if self.detection_mode == 'cars' and dX == 'HOLD' and dY == 'HOLD': return elif self.detection_mode == 'drones' and dX == 'HOLD' and dY == 'HOLD' and dZ == 'HOLD': return if dX == 'FORWARD': pitch = int(mX) elif dX == 'BACKWARD': pitch = int(-1 * mX) if dY == 'LEFT': roll = int(-1 * mY) elif dY == 'RIGHT': roll = int(mY) if dZ == 'UP': gaz = int(mZ) elif dZ == 'DOWN': gaz = int(-1 * mZ) # for i in range(0, 1): if self.frameCount % 10 == 0: for i in range(0, 2): print('pcmd - {}({}), {}({}), {}({})'.format( dX, pitch, dY, roll, dZ, gaz)) self.drone(PCMD(1, roll, pitch, yaw, gaz, int(time.time())))
def set_control_magnitude(self, yaw_correction, pitch_correction, roll_correction, throttle_correction): yaw_mag = round(yaw_correction) pitch_mag = round(pitch_correction) roll_mag = round(roll_correction) throttle_mag = round(throttle_correction) if yaw_mag > 100: yaw_mag = 100 elif yaw_mag < -100: yaw_mag = -100 if pitch_mag > 100: pitch_mag = 100 elif pitch_mag < -100: pitch_mag = -100 if roll_mag > 100: roll_mag = 100 elif roll_mag < -100: roll_mag = -100 if throttle_mag > 100: throttle_mag = 100 elif throttle_mag < -100: throttle_mag = -100 #self.drone.start_piloting() self.drone( PCMD(1, roll_mag, pitch_mag, yaw_mag, throttle_mag, timestampAndSeqNum=0, _timeout=10))
def positive_control(self, x, y): print( '-----------------------------------hi_4------------------------------------------' ) pid_pitch = pidcontroller.PID(50, 0.0, 0) #2.8, 1.8 level 2 pid_roll = pidcontroller.PID(50, 0.0, 0) #5.0, 2.0 level 2 #target_yaw = 0 # 0 is north target_x = x target_y = y start_time_pure = time.gmtime() start_time = start_time_pure.tm_sec '''heading = ["X-coord", "Y-coord", "Z-coord", "x", "y", "z", "count", "position"] with open('XYZ_data.csv', 'w', newline='') as csvFile: writer = csv.writer(csvFile) writer.writerow(heading) csvFile.close() heading = ["pitch", "roll", "yaw", "throttle"] with open('Control_data.csv', 'w', newline='') as csvFile: writer = csv.writer(csvFile) writer.writerow(heading) csvFile.close() heading = ["pitch", "roll", "yaw", "throttle"] with open('Control_raw_data.csv', 'w', newline='') as csvFile: writer = csv.writer(csvFile) writer.writerow(heading) csvFile.close() heading = ["error_x_forward", "error_y_sideward", "error_z_vertical", "error_yaw"] with open('Error_data.csv', 'w', newline='') as csvFile: writer = csv.writer(csvFile) writer.writerow(heading) csvFile.close() ''' while (True): #print('--------time----------',time.time(),'------------time--------------') self.pos_feedback() current_x = self.agent_pos[0] current_y = self.agent_pos[1] print('================================', current_x, '=======================================') print('================================', current_y, '=======================================') #current_altitude = -Y + bottom_to_checker_origin - bottom_to_drone_camera error_x = target_x - current_x error_y = target_y - current_y print('================================', error_x, '=======================================') print('================================', error_y, '=======================================') pitch_correction = pid_pitch.Update(error_x) roll_correction = -pid_roll.Update(error_y) print('================================', pitch_correction, '=======================================') print('================================', roll_correction, '=======================================') if abs(error_x) < 0.05 and abs(error_y) < 0.05: #self.drone(Landing()) #self.drone.disconnection() self.drone( PCMD(1, 0, 0, 0, 0, timestampAndSeqNum=0, _timeout=10)) break else: self.set_control_magnitude(0, int(round(pitch_correction)), int(round(roll_correction)), 0) time.sleep(1) print('Current x: %f' % current_x) print('Current y: %f' % current_y) print('Error_x %f' % error_x) print('Error_y %f' % error_y) print('Setting the pitch command to %f' % pitch_correction) print('Setting the roll command to %f' % roll_correction)
except subprocess.CalledProcessError: pass else: if keyboard_variant == "azerty": ctrl_keys = AZERTY_CTRL_KEYS return ctrl_keys if __name__ == "__main__": with olympe.Drone(DRONE_IP) as drone: drone.connection() control = KeyboardCtrl() while not control.quit(): if control.takeoff(): drone(TakeOff()) elif control.landing(): drone(Landing()) if control.has_piloting_cmd(): drone( PCMD( 1, control.roll(), control.pitch(), control.yaw(), control.throttle(), timestampAndSeqNum=0, )) else: drone(PCMD(0, 0, 0, 0, 0, timestampAndSeqNum=0)) time.sleep(0.05)
import math import time import olympe from olympe.messages.ardrone3.Piloting import TakeOff, Landing, PCMD from olympe.messages.ardrone3.PilotingState import FlyingStateChanged with olympe.Drone("10.202.0.1", loglevel=3) as drone: drone.connection() print("\n\n\n ---------- \n\n\n") drone(TakeOff() >> FlyingStateChanged(state="hovering", _timeout=5)).wait() print("\n\n\n -----TakeOff complete----- \n\n\n") while True: drone(PCMD(1, roll=20, pitch=100, yaw=50, gaz=20, timestampAndSeqNum=0)).wait() time.sleep(0.2) print("\n\n\n ---- Circle finished ---- \n\n\n") drone(Landing()).wait() print("\n\n\n ---- Drone landed ---- \n\n\n") #Leaving the with statement and disconnecting the drone.
def move(coords): april( moveTo(coords[0], coords[1], 1, MoveTo_Orientation_mode.TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() updateSwarm()
def on_press(self, key): print('\r{0}% Manual mode (\'esc\' to exit) '.format( self.getBatteryPercentage()), end='', flush=True) try: # print('alphanumeric key {0} pressed'.format(key.char)) # Moving Forward / Back / Left / Right # w if key.char == 'w': # print('Moving forward') # self.bebop(moveBy(0.2, 0, 0, 0)) # PCMD(flag, roll, pitch, yaw, gaz, timestampAndSeqNum, _timeout=10, _no_expect=False, _float_tol=(1e-07, 1e-09)) self.bebop(PCMD(1, 0, 100, 0, 0, 50)) if key.char == 's': # print('Moving backward') # self.bebop(moveBy(-0.2, 0, 0, 0)) self.bebop(PCMD(1, 0, -100, 0, 0, 50)) if key.char == 'q': # print('Strafe left') # self.bebop(moveBy(0, -0.2, 0, 0)) self.bebop(PCMD(1, -100, 0, 0, 0, 50)) if key.char == 'e': # print('Strafe right') # self.bebop(moveBy(0, 0.2, 0, 0)) self.bebop(PCMD(1, 100, 0, 0, 0, 50)) # FLIPS!!!!!! # shift+w if key.char == 'W': # print('forward flip') self.bebop( Flip(olympe.enums.ardrone3.Animations.Flip_Direction.front) ).wait() # shift+q if key.char == 'Q': # print('left flip') self.bebop( Flip(olympe.enums.ardrone3.Animations.Flip_Direction.left) ).wait() # shift+e if key.char == 'E': # print('right flip') self.bebop( Flip(olympe.enums.ardrone3.Animations.Flip_Direction.right) ).wait() if key.char == 'S': # print('backward flip') self.bebop( Flip(olympe.enums.ardrone3.Animations.Flip_Direction.back) ).wait() # Rotating if key.char == 'a': # print('Rotating left') # self.bebop(moveBy(0, 0, 0, -math.radians(30))) self.bebop(PCMD(1, 0, 0, -100, 0, 0)) if key.char == 'd': # print('Rotating right') # self.bebop(moveBy(0, 0, 0, math.radians(30))) self.bebop(PCMD(1, 0, 0, 100, 0, 0)) except AttributeError: # print('special key {0} pressed'.format(key)) # Vertical controls # ctrl if key == keyboard.Key.ctrl: # print('Moving down') # self.bebop(moveBy(0, 0, 1, 0)).wait() self.bebop(PCMD(1, 0, 0, 0, -50, 50)) # space if key == keyboard.Key.space: # print('Moving up') # self.bebop(moveBy(0, 0, -1, 0)).wait() self.bebop(PCMD(1, 0, 0, 0, 50, 50))
april( FlyingStateChanged(state="hovering") | (TakeOff() & FlyingStateChanged(state="hovering"))).wait().success() # casey( # FlyingStateChanged(state="hovering") # | (TakeOff() & FlyingStateChanged(state="hovering")) # ).wait().success() # donatello( # FlyingStateChanged(state="hovering") # | (TakeOff() & FlyingStateChanged(state="hovering")) # ).wait().success() april( moveBy(0, -40, 0, math.pi) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() april( moveBy(-40, 0, 0, math.pi) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() april( moveBy(0, 40, 0, math.pi) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() april( moveBy(40, 0, 0, math.pi) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success() # april(
# 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 moveCasey(coords): casey( moveTo(coords[0], coords[1], 0.8, MoveTo_Orientation_mode.TO_TARGET, 0.0) >> PCMD(1, 0, 0, 0, 0, 0) >> FlyingStateChanged( state="hovering", _timeout=5)).wait().success()