|
| 1 | +import os, os.path |
| 2 | +import simplejson |
| 3 | + |
| 4 | +from pymavlink import mavutil |
| 5 | +import droneapi.lib |
| 6 | +from droneapi.lib import VehicleMode, Location |
| 7 | + |
| 8 | +import cherrypy |
| 9 | +from cherrypy.process import wspbus, plugins |
| 10 | +from jinja2 import Environment, FileSystemLoader |
| 11 | + |
| 12 | +cherrypy_conf = { |
| 13 | + '/': { |
| 14 | + 'tools.sessions.on': True, |
| 15 | + 'tools.staticdir.root': local_path |
| 16 | + }, |
| 17 | + '/static': { |
| 18 | + 'tools.staticdir.on': True, |
| 19 | + 'tools.staticdir.dir': './html/assets' |
| 20 | + } |
| 21 | +} |
| 22 | + |
| 23 | +class Drone(object): |
| 24 | + def __init__(self, home_coords, server_enabled=True): |
| 25 | + self.api = local_connect() |
| 26 | + self.gps_lock = False |
| 27 | + self.altitude = 30.0 |
| 28 | + self.vehicle = self.api.get_vehicles()[0] |
| 29 | + self.commands = self.vehicle.commands |
| 30 | + self.current_coords = [] |
| 31 | + self.home_coords = home_coords |
| 32 | + self.webserver_enabled = server_enabled |
| 33 | + self._log("DroneDelivery Start") |
| 34 | + |
| 35 | + # Register observers |
| 36 | + self.vehicle.add_attribute_observer('armed', self.armed_callback) |
| 37 | + self.vehicle.add_attribute_observer('location', self.location_callback) |
| 38 | + #self.vehicle.add_attribute_observer('mode', self.mode_callback) |
| 39 | + self.vehicle.add_attribute_observer('gps_0', self.gps_callback) |
| 40 | + |
| 41 | + self._log("Waiting for GPS Lock") |
| 42 | + |
| 43 | + def takeoff(self, alt): |
| 44 | + self._log("Taking off") |
| 45 | + self.commands.takeoff(30.0) |
| 46 | + self.vehicle.flush() |
| 47 | + |
| 48 | + def arm(self, toggle=True): |
| 49 | + if toggle: |
| 50 | + self._log("Arming") |
| 51 | + else: |
| 52 | + self._log("Disarming") |
| 53 | + self.vehicle.armed = True |
| 54 | + self.vehicle.flush() |
| 55 | + |
| 56 | + def run(self): |
| 57 | + self._log('Running initial boot sequence') |
| 58 | + self.arm() |
| 59 | + self.takeoff() |
| 60 | + self.change_mode('GUIDED') |
| 61 | + |
| 62 | + if self.webserver_enabled is True: |
| 63 | + self._run_server() |
| 64 | + |
| 65 | + def _run_server(): |
| 66 | + # Start web server if enabled |
| 67 | + cherrypy.tree.mount(DroneDelivery(self), '/', config=cherrypy_conf) |
| 68 | + |
| 69 | + cherrypy.config.update({ |
| 70 | + 'server.socket_port': 8080, |
| 71 | + 'server.socket_host': '0.0.0.0', |
| 72 | + 'log.screen': None |
| 73 | + }) |
| 74 | + |
| 75 | + cherrypy.engine.start() |
| 76 | + |
| 77 | + def change_mode(self, mode): |
| 78 | + self._log("Mode: {0}".format(mode)) |
| 79 | + |
| 80 | + self.vehicle.mode = VehicleMode(mode) |
| 81 | + self.vehicle.flush() |
| 82 | + |
| 83 | + def goto(self, location, relative=None): |
| 84 | + self._log("Goto: {0}, {1}".format(location, self.altitude)) |
| 85 | + |
| 86 | + self.commands.goto( |
| 87 | + Location( |
| 88 | + float(location[0]), float(location[1]), |
| 89 | + float(self.altitude), |
| 90 | + is_relative=relative |
| 91 | + ) |
| 92 | + ) |
| 93 | + self.vehicle.flush() |
| 94 | + |
| 95 | + def get_location(self): |
| 96 | + return [self.current_location.lat, self.current_location.lon] |
| 97 | + |
| 98 | + def location_callback(self, location): |
| 99 | + location = self.vehicle.location |
| 100 | + |
| 101 | + if location.alt is not None: |
| 102 | + self.altitude = location.alt |
| 103 | + |
| 104 | + self.current_location = location |
| 105 | + |
| 106 | + def armed_callback(self, armed): |
| 107 | + self._log("DroneDelivery Armed Callback") |
| 108 | + self.vehicle.remove_attribute_observer('armed', self.armed_callback) |
| 109 | + |
| 110 | + def mode_callback(self, mode): |
| 111 | + self._log("Mode: {0}".format(self.vehicle.mode)) |
| 112 | + |
| 113 | + def gps_callback(self, gps): |
| 114 | + self._log("GPS: {0}".format(self.vehicle.gps_0)) |
| 115 | + if self.gps_lock is False: |
| 116 | + self.gps_lock = True |
| 117 | + self.vehicle.remove_attribute_observer('gps_0', self.gps_callback) |
| 118 | + self.run() |
| 119 | + |
| 120 | + def _log(self, message): |
| 121 | + print "[DEBUG]: {0}".format(message) |
| 122 | + |
| 123 | +class Templates: |
| 124 | + def __init__(self, home_coords): |
| 125 | + self.home_coords = home_coords |
| 126 | + self.options = self.get_options() |
| 127 | + self.environment = Environment(loader=FileSystemLoader( local_path + '/html')) |
| 128 | + |
| 129 | + def get_options(self): |
| 130 | + return { |
| 131 | + 'width': 670, |
| 132 | + 'height': 470, |
| 133 | + 'zoom': 13, |
| 134 | + 'format': 'png', |
| 135 | + 'access_token': 'pk.eyJ1IjoibXJwb2xsbyIsImEiOiJtUG0tRk9BIn0.AqAiefUV9fFYRo-w0jFR1Q', |
| 136 | + 'mapid': 'mrpollo.kfbnjbl0', |
| 137 | + 'home_coords': self.home_coords, |
| 138 | + 'menu': [ |
| 139 | + {'name': 'Home', 'location': '/'}, |
| 140 | + {'name': 'Track', 'location': '/track'}, |
| 141 | + {'name': 'Command', 'location': '/command'} |
| 142 | + ], |
| 143 | + 'current_url': '/', |
| 144 | + 'json': '' |
| 145 | + } |
| 146 | + |
| 147 | + def index(self): |
| 148 | + self.options = self.get_options() |
| 149 | + self.options['current_url'] = '/' |
| 150 | + return self.get_template('index') |
| 151 | + |
| 152 | + def track(self, current_coords): |
| 153 | + self.options = self.get_options() |
| 154 | + self.options['current_url'] = '/track' |
| 155 | + self.options['current_coords'] = current_coords |
| 156 | + self.options['json'] = simplejson.dumps(self.options) |
| 157 | + return self.get_template('track') |
| 158 | + |
| 159 | + def command(self, current_coords): |
| 160 | + self.options = self.get_options() |
| 161 | + self.options['current_url'] = '/command' |
| 162 | + self.options['current_coords'] = current_coords |
| 163 | + return self.get_template('command') |
| 164 | + |
| 165 | + def get_template(self, file_name): |
| 166 | + template = self.environment.get_template( file_name + '.html') |
| 167 | + return template.render(options=self.options) |
| 168 | + |
| 169 | +class DroneDelivery(object): |
| 170 | + def __init__(self, drone): |
| 171 | + self.drone = drone |
| 172 | + self.templates = Templates(self.drone.home_coords) |
| 173 | + |
| 174 | + @cherrypy.expose |
| 175 | + def index(self): |
| 176 | + return self.templates.index() |
| 177 | + |
| 178 | + @cherrypy.expose |
| 179 | + def command(self): |
| 180 | + return self.templates.command(self.drone.get_location()) |
| 181 | + |
| 182 | + @cherrypy.expose |
| 183 | + @cherrypy.tools.json_out() |
| 184 | + def vehicle(self): |
| 185 | + return dict(position=self.drone.get_location()) |
| 186 | + |
| 187 | + @cherrypy.expose |
| 188 | + def track(self, lat=None, lon=None): |
| 189 | + # Process POST request from Command |
| 190 | + # Sending MAVLink packet with goto instructions |
| 191 | + if(lat is not None and lon is not None): |
| 192 | + self.drone.goto([lat, lon], True) |
| 193 | + |
| 194 | + return self.templates.track(self.drone.get_location()) |
| 195 | + |
| 196 | +Drone([32.5738, -117.0068]) |
| 197 | +cherrypy.engine.block() |
0 commit comments