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()
