class DroneClient(threading.Thread):
"""
Class representing a client which receive drone commands
and send drone odometry.
"""
running = True
def __init__(self, host="", port=9000):
threading.Thread.__init__(self)
self.host = host
self.port = port
self.drone = Drone()
self.socket = None
self.start()
def run(self):
"""
Main loop.
"""
print "DroneClient: Connecting.."
connected = False
while not connected:
try:
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.connect((self.host, self.port))
self.socket.settimeout(2)
connected = True
except socket.error:
continue
print "DroneClient: Connected to host."
while self.running:
try:
msg = self.socket.recv(1)
if len(msg) == 0:
print "Connection to server lost."
self.close()
elif msg == chr(10):
print "Emergency."
self.drone.emergency()
elif msg == chr(0):
print "Initialize."
self.drone.initialize()
self.socket.send(str(self.drone.getSize()))
self.socket.send("\n")
elif msg == chr(1):
print "Shutdown"
self.drone.shutdown()
self.close()
elif msg == chr(2):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
elif msg == chr(3):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
elif msg == chr(4):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
elif msg == chr(5):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
# Testing commands.
elif msg == chr(6):
self.drone.move(2)
elif msg == chr(7):
self.drone.move(3)
elif msg == chr(8):
self.drone.move(4)
elif msg == chr(9):
self.drone.move(5)
elif msg == chr(11):
self.drone.land()
elif msg == chr(12):
self.drone.initialize()
elif msg == chr(13):
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
except socket.timeout:
pass
except socket.error, err:
print err
self.close()
except KeyboardInterrupt:
print "Interrupted."
self.close()
class DroneClient(threading.Thread):
"""
Class representing a client which receive drone commands
and send drone odometry.
"""
running = True
def __init__(self, host="", port=9000):
threading.Thread.__init__(self)
self.host = host
self.port = port
self.drone = Drone()
self.socket = None
self.start()
def run(self):
"""
Main loop.
"""
print "DroneClient: Connecting.."
connected = False
while not connected:
try:
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.connect((self.host, self.port))
self.socket.settimeout(2)
connected = True
except socket.error:
continue
print "DroneClient: Connected to host."
while self.running:
try:
msg = self.socket.recv(1)
if len(msg) == 0:
print "Connection to server lost."
self.close()
elif msg == chr(10):
print "Emergency."
self.drone.emergency()
elif msg == chr(0):
print "Initialize."
self.drone.initialize()
self.socket.send(str(self.drone.getSize()))
self.socket.send("\n")
elif msg == chr(1):
print "Shutdown"
self.drone.shutdown()
self.close()
elif msg == chr(2):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
elif msg == chr(3):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
elif msg == chr(4):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
elif msg == chr(5):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
# Testing commands.
elif msg == chr(6):
self.drone.move(2)
elif msg == chr(7):
self.drone.move(3)
elif msg == chr(8):
self.drone.move(4)
elif msg == chr(9):
self.drone.move(5)
elif msg == chr(11):
self.drone.land()
elif msg == chr(12):
self.drone.initialize()
elif msg == chr(13):
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
except socket.timeout:
pass
except socket.error, err:
print err
self.close()
except KeyboardInterrupt:
print "Interrupted."
self.close()
def pso2(): #Main Control Loop (as prototyped on 2/26 in Glennan Lounge)
"""Main function that runs when the module is called from the command line.
Starts a ui_listener and a drone object, then runs a loop with updates every
50 ms. On each loop iteration, check the ui_state, update the PSO, and calculate
command (for waypoint mode) or pass gamepad command (for manual override mode).
"""
#This is secretly a ROS node. Init it.
rospy.init_node('pso')
# Create listener to receive info from UI
ui_listener = pso_network.UIListener()
ui_listener.daemon = True
ui_listener.start()
ui_state = ui_listener.get_ui()
# Create listener to recieve waypoints and corrections from planner.
planner_listener = pso_network.PlannerListener()
planner_listener.daemon = True
planner_listener.start()
waypoint = cv.CreateMat(4, 1, cv.CV_32FC1)
tf_listener = tf.TransformListener()
#Instatiate Drone Objects (defined in Drone.py)
myDrone = Drone("192.168.1.1")
nav = myDrone.get_navdata()
#Preset flags
running = True
wait_on_emergency = False
wait_on_liftoff = False
wait_on_land = False
#Create Kalman filter, state, and command vectors
kalman = PsoKalman()
u = cv.CreateMat(4, 1, cv.CV_32FC1)
z = cv.CreateMat(5, 1, cv.CV_32FC1)
sys_time = time.time()
#Create PID controllers for each axis
yaw_pid = pso_pid.PID()
yaw_pid.k = 1.5
yaw_pid.t_i = 1.
yaw_pid.angular = True
yaw_pid.deadband = .05
z_pid = pso_pid.PID()
z_pid.k = .00075
z_pid.i_enable = False
z_pid.t_i = 10.
z_pid.deadband = 150
roll_pid = pso_pid.PID()
roll_pid.k = .0002
roll_pid.i_enable = False
roll_pid.d_enable = True
roll_pid.t_d = 1
roll_pid.deadband = 50
pitch_pid = pso_pid.PID()
pitch_pid.k = .0002
pitch_pid.i_enable = False
pitch_pid.d_enable = True
pitch_pid.t_d = 1
pitch_pid.deadband = 50
#Logger puts state in csv for matlab-y goodness
#logger = debuglogger.Logger()
#Fig bucking loop
while not rospy.is_shutdown():
time.sleep(.05)
os.system("clear")
#Get command state from UI
prev_ui_state = ui_state
ui_state = ui_listener.get_ui()
if ui_state[EMERGENCY]:
myDrone.emergency()
if not prev_ui_state[EMERGENCY]:
rospy.loginfo("User ordered emergency");
if ui_state[SHUTDOWN]:
#UI has ordered shutdown
rospy.loginfo( "Shutting down control loop...")
ui_listener.stop()
myDrone.kill()
running = False
if ui_state[TRIM]:
myDrone.trim()
ui_listener.clear_flag(TRIM)
rospy.loginfo("Trimming Drone")
if ui_state[FLYING]:
myDrone.takeoff()
print "Taking Off/Flying"
if not prev_ui_state[FLYING]:
wait_on_liftoff = 10
else:
myDrone.land()
print "Landing/Landed"
if prev_ui_state[FLYING]:
wait_on_land = 5
if ui_state[RESET]:
rospy.loginfo("Resetting drone PSO and emergency state.")
myDrone.reset()
#myDrone.reset_emergency()
yaw_pid.flush()
z_pid.flush()
roll_pid.flush()
pitch_pid.flush()
ui_listener.clear_flag(RESET)
#Get navdata
prevnav = nav
nav = myDrone.get_navdata()
#Print out Drone State
if nav.check_state(navdata.EMERGENCY):
print "Emergency!"
if not prevnav.check_state(navdata.EMERGENCY):
if(ui_state[EMERGENCY]):
rospy.loginfo("Drone is in Emergency State")
else:
rospy.logerr("Unexpected drone emergency")
elif not nav.check_state(navdata.COM_WATCHDOG):
print "WATCHDOG"
elif nav.check_state(navdata.COMMAND):
print "Watchdog cleared. Not yet ready for commands."
else:
print "Ready to Fly\n"
print "\t\tECACAVNAPCUWAPTHLGCMBNTTTCUACVVF\n{0}\t\t{1:32b}".format(nav.seq,nav.state) #Print navdata state
rospy.loginfo("Navdata {0:4}: {1:32b}".format(nav.seq,nav.state))
#Update State (Kalman)
dt = time.time()-sys_time
print "dt:\t",dt
sys_time = time.time()
z[0, 0], z[1, 0], z[2, 0], z[3, 0], z[4, 0] = nav.vx, nav.vy, nav.z, nav.vz, nav.psi
#z and u need to be cv matrices!!!!
sys_state = myDrone.get_state()
print "\nDrone Kalman State:"
print "x:\t{0}".format(sys_state[0, 0])
print "y:\t{0}".format(sys_state[2, 0])
print "z:\t{0}".format(sys_state[4, 0])
print "vx:\t{0}".format(sys_state[1, 0])
print "vy:\t{0}".format(sys_state[3, 0])
print "vz:\t{0}".format(sys_state[5, 0])
print "theta:\t{0}".format(sys_state[6, 0])
print "vtheta:\t{0}".format(sys_state[7, 0])
print "\nNavdata Euler Angles:"
print "theta:\t",nav.theta
print "phi:\t",nav.phi
print "psi:\t",nav.psi
print "\nNavdata Stuff:"
print "z:\t",nav.z
print "vx:\t",nav.vx
print "vy:\t",nav.vy
ui_listener.set_state(sys_state, nav)
br = tf.TransformBroadcaster()
br.sendTransform((sys_state[0,0]/1000., sys_state[2,0]/1000., sys_state[4,0]/1000),
tf.transformations.quaternion_from_euler(nav.phi, nav.psi, -sys_state[6, 0]-math.pi/2),
rospy.Time.now(),
"/base_footprint",
"/odom")
#print "ROS Time:\t",rospy.Time.now()
#logger.log(sys_state)
if wait_on_liftoff>0:
print "Waiting for liftoff to finish"
wait_on_liftoff -= dt
u[0, 0], u[1, 0], u[2, 0], u[3, 0] = 0, 0, 1, 0#Assume drone goes full speed up when taking off
elif ui_state[FLYING]:
#If Drone is in waypoint mode, compute command
if not ui_state[OVERRIDE]:
#Get waypoint
if at_waypoint() and not planner_listener.waypoints.empty():
waypoint = planner_listener.waypoints.get()
rospy.loginfo("Next Waypoint: {0}, {1}, {2}, {3}".format(waypoint[0, 0], waypoint[1, 0], waypoint[2, 0], waypoint[3, 0]))
print "\nNext Waypoint:"
print "X:\t", waypoint[0, 0]
print "Y:\t", waypoint[1, 0]
print "Z:\t", waypoint[2, 0]
print "θ:\t", waypoint[3, 0]
#Compute command
(roll_des,pitch_des) = world2drone(waypoint[0, 0]-sys_state[0, 0], waypoint[1, 0]-sys_state[2, 0], sys_state[6, 0])
print "Desired Roll:\t", roll_des
print "Desired Pitch:\t", pitch_des
u[0, 0] = pitch_pid.update(pitch_des)
u[1, 0] = roll_pid.update(-roll_des)
u[2, 0] = 0#z_pid.update(sys_state[4, 0], waypoint[2, 0])
u[3, 0] = yaw_pid.update(sys_state[6,0], waypoint[3,0])
myDrone.go(u[0,0], u[1,0], u[3, 0], u[2,0])
else: #Manual override: Use command from UI state
print "\nManual override mode\n\n\n"
myDrone.go(ui_state[COMMAND][0], ui_state[COMMAND][1], ui_state[COMMAND][2], ui_state[COMMAND][3])
rospy.logdebug("Sent Command to Drone: {0}, {1}, {2}, {3}".format(ui_state[COMMAND][0], ui_state[COMMAND][1], ui_state[COMMAND][2], ui_state[COMMAND][3]))
u[0, 0], u[1, 0], u[2, 0], u[3, 0] = ui_state[COMMAND]
else:
print "\nLanded"
#Print out commands
print "\nCommands:\npitch:\t",u[0, 0]
print "roll:\t", u[1, 0]
print "gaz:\t", u[2, 0]
print "yaw:\t", u[3, 0]
class World(ShowBase):
fps_text = fps_text2 = fps_text3 = fps_text4 = None
room_dimentions = [0, 0]
camera_position = [290., 1609., 370, -1980, -15, 0] # x y z h p r
drone = None
drone_instance = None
markers = {}
marker_lines = {}
marker_lines_observed = {}
active_keys = {}
loop_callback = None
joy = None
simulation = True
drone_started = False
def __init__(self, width=6.78, length=5.82, simulation=True, video=True):
ShowBase.__init__(self)
width *= 100
length *= 100
self.room_dimentions = [width, length]
self.simulation = simulation
self.wall1 = self.wall_model(0, 0, 0, width, 0)
self.wall2 = self.wall_model(0, 0, 0, length, 90)
self.wall3 = self.wall_model(width, length, 0, width, 180)
self.wall4 = self.wall_model(width, length, 0, length, -90)
self.root_3d = self.marker_model(position=[0., 0., 0.],
orientation=[90, 90, 0])
self.drone = self.drone_model()
self.drone_instance = Drone(simulation=simulation, video=video)
self.lx_drone = LineNodePath(self.render2d, 'box', 2)
self.lx_drone.reparentTo(self.drone)
self.lx_drone.setColor(1., 0., 0., 1.)
self.lx_drone.setTransparency(TransparencyAttrib.MAlpha)
self.lx_drone.setAlphaScale(0.5)
self.lx_drone.drawLines([[(0., 0., 0.), (4., 0., 0.)]])
self.lx_drone.create()
self.ly_drone = LineNodePath(self.render2d, 'box', 2)
self.ly_drone.reparentTo(self.drone)
self.ly_drone.setColor(0., 1., 0., 1.)
self.ly_drone.setTransparency(TransparencyAttrib.MAlpha)
self.ly_drone.setAlphaScale(0.5)
self.ly_drone.drawLines([[(0., 0., 0.), (0., 0., 4.)]])
self.ly_drone.create()
self.lz_drone = LineNodePath(self.render2d, 'box', 2)
self.lz_drone.reparentTo(self.drone)
self.lz_drone.setColor(0., 0., 1., 1.)
self.lz_drone.setTransparency(TransparencyAttrib.MAlpha)
self.lz_drone.setAlphaScale(0.5)
self.lz_drone.drawLines([[(0., 0., 0.), (0., 4., 0.)]])
self.lz_drone.create()
try:
self.joy = xbox.Joystick()
joy_ready = False
if not self.joy.A():
joy_ready = True
if not joy_ready:
raise Exception("Joy not ready!")
else:
print("ready")
except:
pass
# Add the spinCameraTask procedure to the task manager.
self.tick_loop = self.taskMgr.add(self.tick, "tick_loop")
self.accept("space", self.control_drone, [" "])
self.accept("c", self.control_drone, ["c"])
self.accept("x", self.control_drone, ["x"])
self.accept("w", self.control_drone, ["w"])
self.accept("a", self.control_drone, ["a"])
self.accept("s", self.control_drone, ["s"])
self.accept("d", self.control_drone, ["d"])
self.accept("q", self.control_drone, ["q"])
self.accept("e", self.control_drone, ["e"])
self.accept("m", self.control_drone, ["m"])
self.accept("r", self.control_drone, ["r"])
self.accept("f", self.control_drone, ["f"])
self.keypress_repeat("4", self.move_camera, ["x", -1])
self.keypress_repeat("6", self.move_camera, ["x", 1])
self.keypress_repeat("8", self.move_camera, ["y", 1])
self.keypress_repeat("5", self.move_camera, ["y", -1])
self.keypress_repeat("1", self.move_camera, ["z", 1])
self.keypress_repeat("3", self.move_camera, ["z", -1])
self.keypress_repeat("7", self.move_camera, ["h", -1])
self.keypress_repeat("9", self.move_camera, ["h", 1])
self.keypress_repeat("arrow_up", self.move_camera, ["p", 1])
self.keypress_repeat("arrow_down", self.move_camera, ["p", -1])
def control_drone(self, key):
if key == " ":
if not self.drone_started:
self.drone_started = True
self.drone_instance.takeoff()
else:
self.drone_started = False
self.drone_instance.land()
if key == "x":
self.drone_instance.emergency()
elif key == "c":
self.drone_instance.move(0., 0., 0., 0.)
elif key == "w":
self.drone_instance.move(0., 0.08, 0., 0.)
elif key == "s":
self.drone_instance.move(0., -0.08, 0., 0.)
elif key == "d":
self.drone_instance.move(0.08, 0., 0., 0.)
elif key == "a":
self.drone_instance.move(-0.08, 0., 0., 0.)
elif key == "q":
self.drone_instance.move(0., 0., 0., 0.3)
elif key == "e":
self.drone_instance.move(0., 0., 0., -0.3)
elif key == "m":
self.drone_instance.mtrim()
elif key == "r":
self.drone_instance.move(0., 0., 0.14, 0.)
elif key == "f":
self.drone_instance.move(0., 0., -0.15, 0.)
def keypress_repeat(self, key, callback, parameter):
self.accept(key, self.keypress_start, [key, callback, parameter])
self.accept(key + "-up", self.keypress_stop, [key])
def keypress_start(self, key, callback, parameter):
self.active_keys[key] = [callback, parameter]
def keypress_stop(self, key):
self.active_keys[key] = None
def move_camera(self, parameter):
if parameter[0] == "x":
self.camera_position[0] += 10 * np.cos(
np.deg2rad(self.camera_position[3])) * parameter[1]
self.camera_position[1] += -10 * np.sin(
np.deg2rad(self.camera_position[3])) * parameter[1]
if parameter[0] == "y":
self.camera_position[0] += 10 * np.sin(
np.deg2rad(self.camera_position[3])) * parameter[1]
self.camera_position[1] += 10 * np.cos(
np.deg2rad(self.camera_position[3])) * parameter[1]
if parameter[0] == "z":
self.camera_position[2] += 10 * parameter[1]
if parameter[0] == "h":
self.camera_position[3] += parameter[1]
if parameter[0] == "p":
self.camera_position[4] += parameter[1]
def joy_block(self, xbox_key):
""" blocks the xbox key until it's released """
while xbox_key():
pass
def tick(self, task):
for key in self.active_keys:
if self.active_keys[key] is not None:
self.active_keys[key][0](self.active_keys[key][1])
if self.joy is not None:
if self.joy.Back():
self.closeWindow(self.win)
self.userExit()
self.shutdown()
self.destroy()
# takeoff:
if self.joy.A():
print "takeoff"
self.drone_instance.takeoff()
self.joy_block(self.joy.A)
# emergency:
if self.joy.X():
print "emergency"
self.drone_instance.emergency()
self.joy_block(self.joy.X)
# emergency:
if self.joy.B():
print "land"
self.drone_instance.land()
self.joy_block(self.joy.B)
(roll, throttle) = self.joy.leftStick()
(yaw, pitch) = self.joy.rightStick()
print roll, pitch, throttle, yaw
self.drone_instance.move(roll, pitch, throttle, yaw)
if self.loop_callback is not None:
self.loop_callback(self, task)
self.camera.setPos(self.camera_position[0], self.camera_position[1],
self.camera_position[2])
self.camera.setHpr(-self.camera_position[3], self.camera_position[4],
self.camera_position[5])
drone_position = self.convert_position(
self.drone_instance.get_position())
drone_orientation = self.drone_instance.get_orientation()
self.drone.setPos(drone_position[0], drone_position[1],
drone_position[2])
self.drone.setHpr(drone_orientation[0], drone_orientation[1],
drone_orientation[2])
if task.time > 0:
if self.fps_text is not None:
self.fps_text.destroy()
self.fps_text = OnscreenText(text="Tick-Rate: " +
str(int(task.frame / task.time)),
pos=(0.05, 0.05),
scale=0.05,
align=TextNode.ALeft,
parent=self.a2dBottomLeft)
if self.fps_text2 is not None:
self.fps_text2.destroy()
self.fps_text2 = OnscreenText(text="Camera Position: " +
str(self.camera_position),
pos=(0.05, 0.1),
scale=0.05,
align=TextNode.ALeft,
parent=self.a2dBottomLeft)
if self.fps_text3 is not None:
self.fps_text3.destroy()
self.fps_text3 = OnscreenText(
text="Drone Position: " +
str(self.drone_instance.get_position()),
pos=(0.05, 0.15),
scale=0.05,
align=TextNode.ALeft,
parent=self.a2dBottomLeft)
return Task.cont
def drone_model(self):
drone = self.loader.loadModel("resources/models_3d/drone")
drone.setScale(15, 15, 10)
drone.reparentTo(self.render)
return drone
def wall_model(self, x, y, z, length, orientation):
wall = self.loader.loadModel("resources/models_3d/plane")
wall.reparentTo(self.render)
wall.setScale(length, 0, 290)
wall.setPos(x, y, z)
wall.setH(orientation)
wall.setTransparency(TransparencyAttrib.MAlpha)
wall.setAlphaScale(0.1)
return wall
def marker_model(self, position, orientation):
marker = self.loader.loadModel("resources/models_3d/plane")
marker.reparentTo(self.render)
marker.setScale(21, 0, 21)
marker.setPos(position[0], position[1], position[2])
marker.setHpr(orientation[0], orientation[1], orientation[2])
marker.setTransparency(TransparencyAttrib.MAlpha)
marker.setAlphaScale(0.5)
marker.setColor(1., 0., 0., 1.)
return marker
def line_model(self, r, g, b):
line = LineNodePath(self.render2d, 'box', 2)
line.reparentTo(self.render)
line.setColor(r, g, b, 1.)
line.setTransparency(TransparencyAttrib.MAlpha)
line.setAlphaScale(0.5)
return line
def point_model(self, position, color):
marker = self.loader.loadModel("resources/models_3d/sphere")
marker.reparentTo(self.render)
marker.setScale(5, 5, 5)
marker.setPos(position[0], position[1], position[2])
marker.setTransparency(TransparencyAttrib.MAlpha)
marker.setAlphaScale(0.5)
marker.setColor(color[0], color[1], color[2], 1.)
def line_draw(self, line, from_position, to_position):
line.reset()
line.drawLines([[(from_position[0], from_position[1],
from_position[2]),
(to_position[0], to_position[1], to_position[2])]])
line.create()
def get_dimensions(self):
return [self.room_dimentions[0] / 100., self.room_dimentions[1] / 100.]
def get_drone(self):
return self.drone_instance
def set_markers(self, markers):
self.markers = {}
for key, marker in markers.iteritems():
self.markers[key] = self.marker_model(
self.convert_position(marker[0]), marker[1])
def set_default_markers(self):
dimensions = self.get_dimensions()
self.set_markers({
0: [[dimensions[0] / 2, 1.5, 0.01], [0, 0, 0]],
1: [[dimensions[0] / 2, 1.5, dimensions[1] - 0.01], [0, 0, 0]],
2: [[dimensions[0] - 0.01, 1.5, dimensions[1] / 2], [90, 0, 0]],
3: [[0.01, 1.5, dimensions[1] / 2], [90, 0, 0]]
})
def get_markers(self):
return self.markers
def convert_position(self, position):
return [position[0] * 100, position[2] * 100, position[1] * 100]
def hook_init(self, callback):
callback(self)
def hook_loop(self, callback):
self.loop_callback = callback
