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)
