class WaypointMover(object): def __init__(self, verbose=False): self.verbose = verbose self._interface = IvyMessagesInterface(self.message_recv) def message_recv(self, ac_id, msg): if self.verbose: print("Got msg %s" % msg.name) def shutdown(self): print("Shutting down ivy interface...") self._interface.shutdown() def __del__(self): self.shutdown() def move_waypoint(self, ac_id, wp_id, lat, lon, alt): msg = PprzMessage("ground", "MOVE_WAYPOINT") msg['ac_id'] = ac_id msg['wp_id'] = wp_id msg['lat'] = lat msg['long'] = lon msg['alt'] = alt print("Sending message: %s" % msg) self._interface.send(msg)
class WaypointMover(object): def __init__(self, verbose=False): self.verbose = verbose self._interface = IvyMessagesInterface(self.message_recv) def message_recv(self, ac_id, msg): if self.verbose: print("Got msg %s" % msg.name) def shutdown(self): print("Shutting down ivy interface...") self._interface.shutdown() def __del__(self): self.shutdown() def move_waypoint(self, ac_id, wp_id, lat, lon, alt): msg = PprzMessage("ground", "MOVE_WAYPOINT") msg["ac_id"] = ac_id msg["wp_id"] = wp_id msg["lat"] = lat msg["long"] = lon msg["alt"] = alt print("Sending message: %s" % msg) self._interface.send(msg)
class ObstacleAdder(object): def __init__(self, verbose=False): self.verbose = verbose self._interface = IvyMessagesInterface(self.message_recv) def message_recv(self, ac_id, msg): if self.verbose: print("Got msg %s" % msg.name) def shutdown(self): print("Shutting down ivy interface...") self._interface.shutdown() def __del__(self): self.shutdown() def add_obstacle(self, obstacle_id, color, status, lat, lon, radius, alt): msg = PprzMessage("ground", "OBSTACLE") msg['id'] = obstacle_id msg['color'] = color msg['status'] = status msg['lat'] = lat msg['lon'] = lon msg['radius'] = radius msg['alt'] = alt print("Sending message: %s" % msg) self._interface.send(msg)
class myQRParser(): def signal_handler(self, signal, frame): print('You pressed Ctrl+C!') self.interface.shutdown() sys.exit(0) def myCrapThread(self): time.sleep(1) myCode.decode_webcam(callback=self.codeRecognized, device='/dev/video0') #print('Finished, please ctrl+c') self.interface.shutdown() sys.exit(0) def __init__(self, ac_id, waypoint_id): self.ac_id = ac_id[0] self.waypoint_id = waypoint_id[0] self.interface = IvyMessagesInterface(self, self.emptyFun) t = threading.Thread(target=self.myCrapThread) t.start() signal.signal(signal.SIGINT, self.signal_handler) print('Press Ctrl+C') signal.pause() self.interface.shutdown() def emptyFun(ac_id, msg): print "Never" def codeRecognized (self, data): print data if data == '1': coords = 1 print "Dropzone 1" elif data == '2': coords = 2 print "Dropzone 2" elif data == '3': coords = 3 print "Dropzone 3" else: print data return print('Going to: ', coords) pprzmsg = PprzMessage("ground", "DL_SETTING") pprzmsg.set_values([self.ac_id, self.waypoint_id, coords]) self.interface.send(pprzmsg) #pprzmsg.set_values([self.ac_id, (self.waypoint_id+1), coords[0], coords[1], 10]) #self.interface.send(pprzmsg) def __call__(a, b, c): print ""
class WaypointMover(object): def __init__(self, verbose=False): self.verbose = verbose self._interface = IvyMessagesInterface("WaypointMover") def shutdown(self): print("Shutting down ivy interface...") self._interface.shutdown() def __del__(self): self.shutdown() def move_waypoint(self, ac_id, wp_id, lat, lon, alt): msg = PprzMessage("ground", "MOVE_WAYPOINT") msg['ac_id'] = ac_id msg['wp_id'] = wp_id msg['lat'] = lat msg['long'] = lon msg['alt'] = alt print("Sending message: %s" % msg) self._interface.send(msg)
class IntruderAdder(object): def __init__(self, verbose=False): self.verbose = verbose self._interface = IvyMessagesInterface(self.message_recv) def message_recv(self, ac_id, msg): if self.verbose: print("Got msg %s" % msg.name) def shutdown(self): print("Shutting down ivy interface...") self._interface.shutdown() def __del__(self): self.shutdown() def add_intruder(self, intruder_id, name, lat, lon, alt, course, speed, climb, itow): msg = PprzMessage("ground", "INTRUDER") msg['id'] = intruder_id msg['name'] = name msg['lat'] = lat msg['lon'] = lon msg['alt'] = alt msg['course'] = course msg['speed'] = speed msg['climb'] = climb msg['itow'] = 0 print("Sending message: %s" % msg) self._interface.send(msg) def new_intruder(self, intruder_id, name): msg = PprzMessage("ground", "INTRUDER") msg['id'] = intruder_id msg['name'] = name msg['itow'] = 0 print("Sending message: %s" % msg) self._interface.send(msg)
class Guidance(object): def __init__(self, ac_id, verbose=False): self.ac_id = ac_id self.verbose = verbose self._interface = None self.auto2_index = None try: settings = PaparazziACSettings(self.ac_id) except Exception as e: print(e) return try: self.auto2_index = settings.name_lookup['auto2'].index except Exception as e: print(e) print("auto2 setting not found, mode change not possible.") self._interface = IvyMessagesInterface(self.message_recv) def message_recv(self, ac_id, msg): if self.verbose: print("Got msg %s" % msg.name) def shutdown(self): if self._interface is not None: print("Shutting down ivy interface...") self._interface.shutdown() def __del__(self): self.shutdown() def set_guided_mode(self): """ change auto2 mode to GUIDED. """ if self.auto2_index is not None: msg = PprzMessage("ground", "DL_SETTING") msg['ac_id'] = self.ac_id msg['index'] = self.auto2_index msg['value'] = 19 # AP_MODE_GUIDED print("Setting mode to GUIDED: %s" % msg) self._interface.send(msg) def set_nav_mode(self): """ change auto2 mode to NAV. """ if self.auto2_index is not None: msg = PprzMessage("ground", "DL_SETTING") msg['ac_id'] = self.ac_id msg['index'] = self.auto2_index msg['value'] = 13 # AP_MODE_NAV print("Setting mode to NAV: %s" % msg) self._interface.send(msg) def goto_ned(self, north, east, down, heading=0.0): """ goto a local NorthEastDown position in meters (if already in GUIDED mode) """ msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg['ac_id'] = self.ac_id msg['flags'] = 0x00 msg['x'] = north msg['y'] = east msg['z'] = down msg['yaw'] = heading print("goto NED: %s" % msg) # embed the message in RAW_DATALINK so that the server can log it self._interface.send_raw_datalink(msg) def goto_ned_relative(self, north, east, down, yaw=0.0): """ goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode) """ msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg['ac_id'] = self.ac_id msg['flags'] = 0x01 msg['x'] = north msg['y'] = east msg['z'] = down msg['yaw'] = yaw print("goto NED relative: %s" % msg) self._interface.send_raw_datalink(msg) def goto_body_relative(self, forward, right, down, yaw=0.0): """ goto to a position relative to current position and heading in meters (if already in GUIDED mode) """ msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg['ac_id'] = self.ac_id msg['flags'] = 0x03 msg['x'] = forward msg['y'] = right msg['z'] = down msg['yaw'] = yaw print("goto body relative: %s" % msg) self._interface.send_raw_datalink(msg) def move_at_vel(self, north=0.0, east=0.0, down=0.0, yaw=0.0): """ move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode) """ msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg['ac_id'] = self.ac_id msg['flags'] = 0x70 msg['x'] = north msg['y'] = east msg['z'] = down msg['yaw'] = yaw print("move at vel NED: %s" % msg) self._interface.send_raw_datalink(msg)
class Guidance(object): def __init__(self, ac_id, verbose=False): self.ac_id = ac_id self.verbose = verbose self._interface = None self.auto2_index = None try: settings = PaparazziACSettings(self.ac_id) except Exception as e: print(e) return try: self.auto2_index = settings.name_lookup['auto2'].index except Exception as e: print(e) print("auto2 setting not found, mode change not possible.") self._interface = IvyMessagesInterface("guided mode example") def shutdown(self): if self._interface is not None: print("Shutting down ivy interface...") self._interface.shutdown() self._interface = None def __del__(self): self.shutdown() def set_guided_mode(self): """ change auto2 mode to GUIDED. """ if self.auto2_index is not None: msg = PprzMessage("ground", "DL_SETTING") msg['ac_id'] = self.ac_id msg['index'] = self.auto2_index msg['value'] = 19 # AP_MODE_GUIDED print("Setting mode to GUIDED: %s" % msg) self._interface.send(msg) def set_nav_mode(self): """ change auto2 mode to NAV. """ if self.auto2_index is not None: msg = PprzMessage("ground", "DL_SETTING") msg['ac_id'] = self.ac_id msg['index'] = self.auto2_index msg['value'] = 13 # AP_MODE_NAV print("Setting mode to NAV: %s" % msg) self._interface.send(msg) def goto_ned(self, north, east, down, heading=0.0): """ goto a local NorthEastDown position in meters (if already in GUIDED mode) """ msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg['ac_id'] = self.ac_id msg['flags'] = 0x00 msg['x'] = north msg['y'] = east msg['z'] = down msg['yaw'] = heading print("goto NED: %s" % msg) # embed the message in RAW_DATALINK so that the server can log it self._interface.send_raw_datalink(msg) def goto_ned_relative(self, north, east, down, yaw=0.0): """ goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode) """ msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg['ac_id'] = self.ac_id msg['flags'] = 0x01 msg['x'] = north msg['y'] = east msg['z'] = down msg['yaw'] = yaw print("goto NED relative: %s" % msg) self._interface.send_raw_datalink(msg) def goto_body_relative(self, forward, right, down, yaw=0.0): """ goto to a position relative to current position and heading in meters (if already in GUIDED mode) """ msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg['ac_id'] = self.ac_id msg['flags'] = 0x03 msg['x'] = forward msg['y'] = right msg['z'] = down msg['yaw'] = yaw print("goto body relative: %s" % msg) self._interface.send_raw_datalink(msg) def move_at_vel(self, north=0.0, east=0.0, down=0.0, yaw=0.0): """ move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode) """ msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg['ac_id'] = self.ac_id msg['flags'] = 0x70 msg['x'] = north msg['y'] = east msg['z'] = down msg['yaw'] = yaw print("move at vel NED: %s" % msg) self._interface.send_raw_datalink(msg)
class Guidance(object): def __init__(self, ac_id, verbose=False): self.ac_id = ac_id self.verbose = verbose self._interface = None self.auto2_index = None try: settings = PaparazziACSettings(self.ac_id) except Exception as e: print(e) return try: self.auto2_index = settings.name_lookup["auto2"].index except Exception as e: print(e) print("auto2 setting not found, mode change not possible.") self._interface = IvyMessagesInterface(self.message_recv) def message_recv(self, ac_id, msg): if self.verbose: print("Got msg %s" % msg.name) def shutdown(self): if self._interface is not None: print("Shutting down ivy interface...") self._interface.shutdown() def __del__(self): self.shutdown() def set_guided_mode(self): """ change auto2 mode to GUIDED. """ if self.auto2_index is not None: msg = PprzMessage("ground", "DL_SETTING") msg["ac_id"] = self.ac_id msg["index"] = self.auto2_index msg["value"] = 19 # AP_MODE_GUIDED print("Setting mode to GUIDED: %s" % msg) self._interface.send(msg) def set_nav_mode(self): """ change auto2 mode to NAV. """ if self.auto2_index is not None: msg = PprzMessage("ground", "DL_SETTING") msg["ac_id"] = self.ac_id msg["index"] = self.auto2_index msg["value"] = 13 # AP_MODE_NAV print("Setting mode to NAV: %s" % msg) self._interface.send(msg) def goto_ned(self, north, east, down, heading=0.0): """ goto a local NorthEastDown position in meters (if already in GUIDED mode) """ msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg["ac_id"] = self.ac_id msg["flags"] = 0x00 msg["x"] = north msg["y"] = east msg["z"] = down msg["yaw"] = heading print("goto NED: %s" % msg) # embed the message in RAW_DATALINK so that the server can log it self._interface.send_raw_datalink(msg) def goto_ned_relative(self, north, east, down, yaw=0.0): """ goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode) """ msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg["ac_id"] = self.ac_id msg["flags"] = 0x01 msg["x"] = north msg["y"] = east msg["z"] = down msg["yaw"] = yaw print("goto NED relative: %s" % msg) self._interface.send_raw_datalink(msg) def goto_body_relative(self, forward, right, down, yaw=0.0): """ goto to a position relative to current position and heading in meters (if already in GUIDED mode) """ msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") msg["ac_id"] = self.ac_id msg["flags"] = 0x03 msg["x"] = forward msg["y"] = right msg["z"] = down msg["yaw"] = yaw print("goto body relative: %s" % msg) self._interface.send_raw_datalink(msg)
class CommandSender(object): def __init__(self, verbose=False, callback = None): self.verbose = verbose self.callback = callback self._interface = IvyMessagesInterface(self.message_recv) def message_recv(self, ac_id, msg): if (self.verbose and self.callback != None): self.callback(ac_id, msg) def shutdown(self): print("Shutting down ivy interface...") self._interface.shutdown() def __del__(self): self.shutdown() def add_mission_command(self, msg_id = "", ac_id = 5 , insert = "APPEND" , wp_lat = "", wp_lon = "", wp_alt = "", duration = "60", center_lat = "", center_lon = "", center_alt = "", radius = "60",segment_lat_1 = "", segment_lat_2 = "", segment_lon_1 = "", segment_lon_2 = "", segment_alt = "", point_lat_1 = "", point_lon_1 = "", point_lat_2 = "", point_lon_2 = "", point_lat_3 = "", point_lon_3 = "", point_lat_4 = "", point_lon_4 = "", point_lat_5 = "", point_lon_5 = "", path_alt = "", nb = "", survey_lat_1 = "", survey_lon_1 = "", survey_lat_2 = "", survey_lon_2 = "", survey_alt = ""): msg = PprzMessage("datalink", msg_id) msg['ac_id'] = ac_id msg['insert'] = insert msg['duration'] = duration if msg_id == MISSION_GOTO_WP_LLA: msg['wp_lat'] = wp_lat msg['wp_lon'] = wp_lon msg['wp_alt'] = wp_alt elif msg_id == MISSION_CIRCLE_LLA: msg['center_lat'] = center_lat msg['center_lon'] = center_lon msg['center_alt'] = center_alt msg['radius'] = radius elif msg_id == MISSION_SEGMENT_LLA: msg['segment_lat_1'] = segment_lat_1 msg['segment_lon_1'] = segment_lon_1 msg['segment_lat_2'] = segment_lat_2 msg['segment_lon_2'] = segment_lon_2 elif msg_id == MISSION_PATH_LLA: msg['point_lat_1'] = point_lat_1 msg['point_lon_1'] = point_lon_1 msg['point_lat_2'] = point_lat_2 msg['point_lon_2'] = point_lon_2 msg['point_lat_3'] = point_lat_3 msg['point_lon_3'] = point_lon_3 msg['point_lat_4'] = point_lat_4 msg['point_lon_4'] = point_lon_4 msg['point_lat_5'] = point_lat_5 msg['point_lon_5'] = point_lon_5 msg['path_alt'] = path_alt msg['nb'] = nb elif msg_id == MISSION_SURVEY_LLA: msg['survey_lat_1'] = survey_lat_1 msg['survey_lon_1'] = survey_lon_1 msg['survey_lat_2'] = survey_lat_2 msg['survey_lon_2'] = survey_lon_2 msg['survey_alt'] = survey_alt print("Sending message: %s" % msg) self._interface.send(msg) def add_mission_command_dict(self, ac_id, insert, msg_id, msgs): print('hello') msg = PprzMessage("datalink", msg_id) print(msgs) print(msgs.keys) print(msgs.get('duration')) msg['ac_id'] = ac_id msg['insert'] = insert msg['duration'] = msgs.get('duration') if msg_id == 'MISSION_GOTO_WP_LLA': msg['wp_lat'] = msgs.get('wp_lat') msg['wp_lon'] = msgs.get('wp_lon') msg['wp_alt'] = msgs.get('wp_alt') elif msg_id == 'MISSION_CIRCLE_LLA': msg['center_lat'] = msgs.get('center_lat') msg['center_lon'] = msgs.get('center_lon') msg['center_alt'] = msgs.get('center_alt') msg['radius'] = msgs.get('radius') elif msg_id == 'MISSION_SEGMENT_LLA': msg['segment_lat_1'] = msgs.get('segment_lat_1') msg['segment_lon_1'] = msgs.get('segment_lon_1') msg['segment_lat_2'] = msgs.get('segment_lat_2') msg['segment_lon_2'] = msgs.get('segment_lon_2') elif msg_id == 'MISSION_PATH_LLA': msg['point_lat_1'] = msgs.get('point_lat_1') msg['point_lon_1'] = msgs.get('point_lon_1') msg['point_lat_2'] = msgs.get('point_lat_2') msg['point_lon_2'] = msgs.get('point_lon_2') msg['point_lat_3'] = msgs.get('point_lat_3') msg['point_lon_3'] = msgs.get('point_lon_3') msg['point_lat_4'] = msgs.get('point_lat_4') msg['point_lon_4'] = msgs.get('point_lon_4') msg['point_lat_5'] = msgs.get('point_lat_5') msg['point_lon_5'] = msgs.get('point_lon_5') msg['path_alt'] = msgs.get('path_alt') msg['nb'] = msgs.get('nb') elif msg_id == 'MISSION_SURVEY_LLA': msg['survey_lat_1'] = msgs.get('survey_lat_1') msg['survey_lon_1'] = msgs.get('survey_lon_1') msg['survey_lat_2'] = msgs.get('survey_lat_2') msg['survey_lon_2'] = msgs.get('survey_lon_2') msg['survey_alt'] = msgs.get('survey_alt') print("Sending message: %s" % msg) self._interface.send(msg) def add_obstacle(self, obstacle_id, color, status, lat, lon, radius, alt): msg = PprzMessage("ground", "OBSTACLE") msg['id'] = obstacle_id msg['color'] = color msg['status'] = status msg['lat'] = lat msg['lon'] = lon msg['radius'] = radius msg['alt'] = alt print("Sending message: %s" % msg) self._interface.send(msg)