def gesture_ctrl(self, scf, fc, g):
mc = MotionCommander(scf)
mc._reset_position_estimator()
print ('Enter to start (c to cancel)')
char = raw_input()
if char == 'c':
print('\nClosing link...')
scf.close_link()
print('Shutting down...')
os.kill(os.getpid(), signal.SIGINT)
try:
while True:
g_id = gesture.get_gesture()
if gesture is not None:
fc.perform_gesture(g_id, mc)
except Exception, e:
print (str(e))
scf.close_link()
def gesture_ctrl(self, scf, fc, g):
global gesture
mc = MotionCommander(scf)
mc._reset_position_estimator()
self.resetYawInit()
try:
while True:
g_id = gesture.get_gesture()
if g_id is not None:
#print("Gesture detected: ")
#print(g_id)
fc.perform_gesture(scf, g_id, mc)
except Exception, e:
print(str(e))
scf.close_link()
def key_ctrl(self, scf):
mc = MotionCommander(scf)
mc._reset_position_estimator()
print 'Spacebar to start'
raw_input()
pygame.display.set_mode((400, 300))
print 'WASD for throttle & yaw; arrow keys for left/right/forward/backward'
print 'Spacebar to land'
vel_x = 0
vel_y = 0
vel_z = 0
yaw_rate = 0
try:
mc.take_off()
#set inital Yaw value
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
while len(stbLines) == 0:
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
currAttitude = stbLines[len(stbLines) - 1]
need, to, currentYaw, test = currAttitude.split(',')
self.yawCurr = float(currentYaw)
Thread(target=self._updateYaw, args=(scf, )).start()
while True:
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_w:
vel_z = 0.3
if event.key == pygame.K_SPACE:
print 'Space pressed, landing'
mc.land()
time.sleep(2)
print 'Closing link...'
scf.close_link()
print 'Link closed'
pygame.quit()
sys.exit(0)
if event.key == pygame.K_a:
yaw_rate = -45
if event.key == pygame.K_s:
vel_z = -0.3
if event.key == pygame.K_d:
yaw_rate = 45
if event.key == pygame.K_UP:
#vel_x = 0.3
vel_x = 0.2 * self.fwCoef[0]
vel_y = 0.2 * self.fwCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.fwCoef[0],
vel_y * self.fwCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_DOWN:
#vel_x = -0.3
vel_x = 0.2 * self.bkCoef[0]
vel_y = 0.2 * self.bkCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.bkCoef[0],
vel_y * self.bkCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_LEFT:
#vel_y = 0.3
vel_x = 0.2 * self.lfCoef[0]
vel_y = 0.2 * self.lfCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.bkCoef[0],
vel_y * self.bkCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_RIGHT:
#vel_y = -0.3
vel_x = 0.2 * self.rtCoef[0]
vel_y = 0.2 * self.rtCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.bkCoef[0],
vel_y * self.bkCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_r:
#set recalibrate initial Yaw value
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
while len(stbLines) == 0:
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
print("yaw angle recalibrated :")
newInitAttitude = stbLines[len(stbLines) - 1]
need, to, initYaw, test = newInitAttitude.split(
',')
print(initYaw)
self.yawInit = float(initYaw)
if event.key == pygame.K_RCTRL:
mc.stop()
if event.type == pygame.KEYUP:
if event.key == pygame.K_w or event.key == pygame.K_s:
vel_z = 0
if event.key == pygame.K_a or event.key == pygame.K_d:
yaw_rate = 0
if event.key == pygame.K_UP or event.key == pygame.K_DOWN:
vel_x = 0
if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT:
vel_y = 0
mc._set_vel_setpoint(vel_x, vel_y, vel_z, yaw_rate)
except KeyboardInterrupt:
print('\nShutting down...')
scf.close_link()
except Exception, e:
print str(e)
scf.close_link()
