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