class IvyRequester(object):
def __init__(self, interface=None):
self._interface = interface
if interface is None:
self._interface = IvyMessagesInterface("ivy requester")
self.ac_list = []
def __del__(self):
self.shutdown()
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def get_aircrafts(self):
def aircrafts_cb(ac_id, msg):
self.ac_list = [int(a) for a in msg['ac_list'].split(',') if a]
print("aircrafts: {}".format(self.ac_list))
self._interface.subscribe(aircrafts_cb, "(.*AIRCRAFTS .*)")
sender = 'get_aircrafts'
request_id = '42_1' # fake request id, should be PID_index
self._interface.send("{} {} AIRCRAFTS_REQ".format(sender, request_id))
# hack: sleep briefly to wait for answer
sleep(0.1)
return self.ac_list
class IvyRequester(object):
def __init__(self, interface=None):
self._interface = interface
if interface is None:
self._interface = IvyMessagesInterface("ivy requester")
self.ac_list = []
def __del__(self):
self.shutdown()
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def get_aircrafts(self):
def aircrafts_cb(ac_id, msg):
self.ac_list = [int(a) for a in msg['ac_list'].split(',') if a]
print("aircrafts: {}".format(self.ac_list))
self._interface.subscribe(aircrafts_cb, "(.*AIRCRAFTS .*)")
sender = 'get_aircrafts'
request_id = '42_1' # fake request id, should be PID_index
self._interface.send("{} {} AIRCRAFTS_REQ".format(sender, request_id))
# hack: sleep briefly to wait for answer
sleep(0.1)
return self.ac_list
class RosIvyMessagesInterface(RosMessagesInterface):
def __init__(self,
agent_name=None,
start_ivy=True,
verbose=False,
ivy_bus=IVY_BUS):
self.interface = IvyMessagesInterface(agent_name, start_ivy, verbose,
ivy_bus)
RosMessagesInterface.__init__(self)
# only subscribe to telemetry messages eg. starting with AC_ID
self.interface.subscribe(callback=self.to_ros,
regex_or_msg='(^[0-9]+ .*)')
def from_ros(self, ros_msg):
pprz_msg = self.converter.ros2pprz(ros_msg)
self.interface.send(pprz_msg)
def to_ros(self, sender_id, pprz_msg):
ros_msg = self.converter.pprz2ros(sender_id, pprz_msg)
self.pub.publish(ros_msg)
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 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 Guidance(object):
def __init__(self, ac_id):
self.ac_id = ac_id
self._interface = None
self.ap_mode = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'] # try classic name
except Exception as e:
try:
self.ap_mode = settings.name_lookup[
'ap'] # in case it is a generated autopilot
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("sim_rc_4ch")
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 mode to GUIDED.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName(
'Guided') # AP_MODE_GUIDED
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'GUIDED') # AP_MODE_GUIDED
except ValueError:
msg['value'] = 19 # fallback to fixed index
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change mode to NAV.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName('Nav') # AP_MODE_NAV
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'NAV') # AP_MODE_NAV
except ValueError:
msg['value'] = 13 # fallback to fixed index
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def rc_4_channel_output(self,
mode=2,
throttle=0.0,
roll=0.0,
pitch=0.0,
yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "RC_4CH")
msg['ac_id'] = self.ac_id
msg['mode'] = mode
msg['throttle'] = throttle
msg['roll'] = roll
msg['pitch'] = pitch
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
class FormationControl:
def __init__(self, config, freq=10., verbose=False):
self.config = config
self.step = 1. / freq
self.verbose = verbose
self.ids = self.config['ids']
self.B = np.array(self.config['topology'])
self.Zdesired = np.array(self.config['desired_intervehicle_angles'])
self.k = np.array(self.config['gain'])
self.radius = np.array(self.config['desired_stationary_radius'])
self.aircraft = [Aircraft(i) for i in self.ids]
self.sigmas = np.zeros(len(self.aircraft))
for ac in self.aircraft:
settings = PaparazziACSettings(ac.id)
list_of_indexes = ['ell_a', 'ell_b']
for setting_ in list_of_indexes:
try:
index = settings.name_lookup[setting_].index
if setting_ == 'ell_a':
ac.a_index = index
if setting_ == 'ell_b':
ac.b_index = index
except Exception as e:
print(e)
print(setting_ + " setting not found, \
have you forgotten to check gvf.xml for your settings?")
# Start IVY interface
self._interface = IvyMessagesInterface("Circular Formation")
# bind to NAVIGATION message
def nav_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "NAVIGATION":
ac = self.aircraft[self.ids.index(ac_id)]
ac.XY[0] = float(msg.get_field(2))
ac.XY[1] = float(msg.get_field(3))
ac.initialized_nav = True
self._interface.subscribe(nav_cb, PprzMessage("telemetry", "NAVIGATION"))
def gvf_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "GVF":
if int(msg.get_field(1)) == 1:
ac = self.aircraft[self.ids.index(ac_id)]
param = msg.get_field(4)
ac.XYc[0] = float(param[0])
ac.XYc[1] = float(param[1])
ac.a = float(param[2])
ac.b = float(param[3])
ac.s = float(msg.get_field(2))
ac.initialized_gvf = True
self._interface.subscribe(gvf_cb, PprzMessage("telemetry", "GVF"))
def __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
def circular_formation(self):
'''
circular formation control algorithm
'''
ready = True
for ac in self.aircraft:
if (not ac.initialized_nav) or (not ac.initialized_gvf):
if self.verbose:
print("Waiting for state of aircraft ", ac.id)
ready = False
if not ready:
return
i = 0
for ac in self.aircraft:
ac.sigma = np.arctan2(ac.XY[1]-ac.XYc[1], ac.XY[0]-ac.XYc[0])
self.sigmas[i] = ac.sigma
i = i + 1
inter_sigma = self.B.transpose().dot(self.sigmas)
error_sigma = inter_sigma - self.Zdesired
if np.size(error_sigma) > 1:
for i in range(0, np.size(error_sigma)):
if error_sigma[i] > np.pi:
error_sigma[i] = error_sigma[i] - 2*np.pi
elif error_sigma[i] <= -np.pi:
error_sigma[i] = error_sigma[i] + 2*np.pi
else:
if error_sigma > np.pi:
error_sigma = error_sigma - 2*np.pi
elif error_sigma <= -np.pi:
error_sigma = error_sigma + 2*np.pi
u = -self.aircraft[0].s*self.k*self.B.dot(error_sigma)
if self.verbose:
print("Inter-vehicle errors: ", str(error_sigma*180.0/np.pi).replace('[','').replace(']',''))
i = 0
for ac in self.aircraft:
msga = PprzMessage("ground", "DL_SETTING")
msga['ac_id'] = ac.id
msga['index'] = ac.a_index
msga['value'] = self.radius + u[i]
msgb = PprzMessage("ground", "DL_SETTING")
msgb['ac_id'] = ac.id
msgb['index'] = ac.b_index
msgb['value'] = self.radius + u[i]
self._interface.send(msga)
self._interface.send(msgb)
i = i + 1
def run(self):
try:
# The main loop
while True:
# TODO: make better frequency managing
sleep(self.step)
# Run the formation algorithm
self.circular_formation()
except KeyboardInterrupt:
self.stop()
from pprzlink import messages_xml_map
ids = [200, 201, 202, 203, 204]
ivyInterface = IvyMessagesInterface()
time.sleep(0.5)
ac_id = 200
index = 1
msg = PprzMessage('datalink', 'MISSION_CUSTOM')
msg['ac_id'] = ac_id
msg['insert'] = 0
msg['index'] = 1
msg['type'] = 'LACE'
msg['duration'] = -1
msg['params'] = [1500.0, 900.0, 700.0, 0, 50.0, -7.0, -0.5, 3.0]
ivyInterface.send(msg)
msg['type'] = 'RSTT'
msg['index'] = 2
msg['params'] = [-1500.0, 900.0, 700.0, 0, 50.0, -7.0, -0.5, 3.0]
ivyInterface.send(msg)
msg['type'] = 'SPIR3'
msg['index'] = 3
msg['params'] = [0.0, 2400.0, 700.0, 900.0, 50.0, 200.0, -7.0, -0.5, 3.0]
ivyInterface.send(msg)
def all_start_lace():
for id in ids:
append_lace(id)
class RadioBridge:
def __init__(self, link_uri, msg_class='telemetry', verbose=False):
""" Initialize and run with the specified link_uri """
self.verbose = verbose
# Ivy interface and stream parser
self._ivy = IvyMessagesInterface("cf2ivy")
self._transport = PprzTransport(msg_class)
# Create a Crazyradio
self._driver = RadioDriver()
self._driver.connect(link_uri, self._link_quality_cb,
self._link_error_cb)
if self.verbose:
print('Connecting to %s' % link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
# Bind to all messages from ac_id
def _forward_to_cf(ac_id, msg):
try:
data = self._transport.pack_pprz_msg(0,
msg) # sender_id 0 = GCS
for i in range(0, len(data), 30):
pk = CRTPPacket()
pk.port = CRTP_PORT_PPRZLINK
pk.data = data[i:(i + 30)]
self._driver.send_packet(pk)
if self.verbose:
print('Forward message', msg.name)
except:
if self.verbose:
print('Forward error for', ac_id)
messages_datalink = messages_xml_map.get_msgs("datalink")
for msg in messages_datalink:
self._ivy.subscribe(_forward_to_cf, PprzMessage("datalink", msg))
def shutdown(self):
if self.verbose:
print('closing cf2ivy interfaces')
self._ivy.shutdown()
self._driver.close()
def run(self):
pk = self._driver.receive_packet(
0.1) # wait for next message with timeout
if pk is not None:
self._got_packet(pk)
def _got_packet(self, pk):
if pk.port == CRTP_PORT_PPRZLINK:
for c in pk.data:
if self._transport.parse_byte(bytes([c])):
(sender_id, _, _, msg) = self._transport.unpack()
if self.verbose:
print("Got message {} from {}".format(
msg.name, sender_id))
# Forward message to Ivy bus
if self.is_connected:
self._ivy.send(msg, sender_id=sender_id)
def _link_quality_cb(self, quality):
pass
def _link_error_cb(self, msg):
if self.verbose:
print("Link error: {}".format(msg))
class Guidance(object):
def __init__(self, verbose=False, interface=None, quad_ids=None):
self.verbose = verbose
self._interface = interface
self.auto2_index = None
self.ac_id = quad_ids[0]
self.ids = quad_ids
self.ap_mode = None
self.rotorcrafts = [Rotorcraft(i) for i in quad_ids]
try:
settings = PaparazziACSettings(
self.ac_id) # target and follower should be checked, FIX ME !
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'] # try classic name
except Exception as e:
try:
self.ap_mode = settings.name_lookup[
'ap'] # in case it is a generated autopilot
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface(
"Deep Guidance is on the way...")
# bind to INS message
def ins_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "INS":
rc = self.rotorcrafts[self.ids.index(ac_id)]
i2p = 1. / 2**8 # integer to position
i2v = 1. / 2**19 # integer to velocity
rc.X[0] = float(msg['ins_x']) * i2p
rc.X[1] = float(msg['ins_y']) * i2p
rc.X[2] = float(msg['ins_z']) * i2p
rc.V[0] = float(msg['ins_xd']) * i2v
rc.V[1] = float(msg['ins_yd']) * i2v
rc.V[2] = float(msg['ins_zd']) * i2v
rc.timeout = 0
rc.initialized = True
# self._interface.subscribe(ins_cb, PprzMessage("telemetry", "INS"))
#################################################################
def rotorcraft_fp_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "ROTORCRAFT_FP":
rc = self.rotorcrafts[self.ids.index(ac_id)]
i2p = 1. / 2**8 # integer to position
i2v = 1. / 2**19 # integer to velocity
i2w = 1. / 2**12 # integer to angle
rc.X[0] = float(msg['north']) * i2p
rc.X[1] = float(msg['east']) * i2p
rc.X[2] = float(msg['up']) * i2p
rc.V[0] = float(msg['vnorth']) * i2v
rc.V[1] = float(msg['veast']) * i2v
rc.V[2] = float(msg['vup']) * i2v
rc.W[2] = float(msg['psi']) * i2w
rc.timeout = 0
rc.initialized = True
# Un-comment this if the quadrotors are providing state information to use_deep_guidance.py
self._interface.subscribe(rotorcraft_fp_cb,
PprzMessage("telemetry", "ROTORCRAFT_FP"))
# bind to GROUND_REF message : ENAC Voliere is sending LTP_ENU
def ground_ref_cb(ground_id, msg):
ac_id = int(msg['ac_id'])
if ac_id in self.ids:
rc = self.rotorcrafts[self.ids.index(ac_id)]
# X and V in NED
rc.X[0] = float(msg['pos'][1])
rc.X[1] = float(msg['pos'][0])
rc.X[2] = float(msg['pos'][2])
rc.V[0] = float(msg['speed'][1])
rc.V[1] = float(msg['speed'][0])
rc.V[2] = float(msg['speed'][2])
rc.timeout = 0
rc.initialized = True
# Un-comment this if optitrack is being used for state information for use_deep_guidance.py **For use only in the Voliere**
#self._interface.subscribe(ground_ref_cb, PprzMessage("ground", "GROUND_REF"))
################################################################
# <message name="ROTORCRAFT_FP" id="147">
# <field name="east" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="north" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="up" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="veast" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="vnorth" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="vup" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="phi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
# <field name="theta" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
# <field name="psi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
# <field name="carrot_east" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="carrot_north" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="carrot_up" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="carrot_psi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
# <field name="thrust" type="int32"/>
# <field name="flight_time" type="uint16" unit="s"/>
# </message>
# <message name="INS" id="198">
# <field name="ins_x" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="ins_y" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="ins_z" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="ins_xd" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="ins_yd" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="ins_zd" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="ins_xdd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
# <field name="ins_ydd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
# <field name="ins_zdd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
# </message>
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, quad_id=None):
"""
change mode to GUIDED.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = quad_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName(
'Guided') # AP_MODE_GUIDED
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'GUIDED') # AP_MODE_GUIDED
except ValueError:
msg['value'] = 19 # fallback to fixed index
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self, quad_id=None):
"""
change mode to NAV.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = quad_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName('Nav') # AP_MODE_NAV
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'NAV') # AP_MODE_NAV
except ValueError:
msg['value'] = 13 # fallback to fixed index
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def goto_ned(self, north, east, down, heading=0.0, quad_id=None):
"""
goto a local NorthEastDown position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = quad_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'] = 0x0D
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'] = 0x0E
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_ned_vel(self,
north=0.0,
east=0.0,
down=0.0,
yaw=0.0,
quad_id=None):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = quad_id
msg['flags'] = 0xE0
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)
def move_at_body_vel(self, forward=0.0, right=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'] = 0x62
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
def accelerate(self, north=0.0, east=0.0, down=0.0, quad_id=2):
msg = PprzMessage("datalink", "DESIRED_SETPOINT")
msg['ac_id'] = quad_id
msg['flag'] = 0 # full 3D
msg['ux'] = north
msg['uy'] = east
msg['uz'] = down
self._interface.send(msg)
# <message name="GUIDED_SETPOINT_NED" id="40" link="forwarded">
# <description>
# Set vehicle position or velocity in NED.
# Frame can be specified with the bits 0-3
# Velocity of position setpoint can be specified with the bits 5-7
# Flags field definition:
# - bit 0: x,y as offset coordinates
# - bit 1: x,y in body coordinates
# - bit 2: z as offset coordinates
# - bit 3: yaw as offset coordinates
# - bit 4: free
# - bit 5: x,y as vel
# - bit 6: z as vel
# - bit 7: yaw as rate
# </description>
# <field name="ac_id" type="uint8"/>
# <field name="flags" type="uint8">bits 0-3: frame, bits 5-7: use as velocity</field>
# <field name="x" type="float" unit="m">X position/velocity in NED</field>
# <field name="y" type="float" unit="m">Y position/velocity in NED</field>
# <field name="z" type="float" unit="m">Z position/velocity in NED (negative altitude)</field>
# <field name="yaw" type="float" unit="rad" alt_unit="deg">yaw/rate setpoint</field>
# </message>
# class IvyRequester(object):
# def __init__(self, interface=None):
# self._interface = interface
# if interface is None:
# self._interface = IvyMessagesInterface("ivy requester")
# self.ac_list = []
# def __del__(self):
# self.shutdown()
# def shutdown(self):
# if self._interface is not None:
# print("Shutting down ivy interface...")
# self._interface.shutdown()
# self._interface = None
# def get_aircrafts(self):
# def aircrafts_cb(ac_id, msg):
# self.ac_list = [int(a) for a in msg['ac_list'].split(',') if a]
# print("aircrafts: {}".format(self.ac_list))
# self._interface.subscribe(aircrafts_cb, "(.*AIRCRAFTS .*)")
# sender = 'get_aircrafts'
# request_id = '42_1' # fake request id, should be PID_index
# self._interface.send("{} {} AIRCRAFTS_REQ".format(sender, request_id))
# # hack: sleep briefly to wait for answer
# sleep(0.1)
# return self.ac_list
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'] = 0x0D
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'] = 0x0E
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_ned_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'] = 0x60
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)
def move_at_body_vel(self, forward=0.0, right=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'] = 0x62
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
class FormationControl:
def __init__(self,
config,
freq=10.,
use_ground_ref=False,
ignore_geo_fence=False,
verbose=False):
self.config = config
self.step = 1. / freq
self.sens = self.config['sensitivity']
self.use_ground_ref = use_ground_ref
self.enabled = True # run control by default
self.ignore_geo_fence = ignore_geo_fence
self.verbose = verbose
self.ids = self.config['ids']
self.rotorcrafts = [FC_Rotorcraft(i) for i in self.ids]
self.joystick = FC_Joystick()
self.altitude = 2.0 # starts from 1 m high
self.scale = 1.0
self.B = np.array(self.config['topology'])
self.d = np.array(self.config['desired_distances'])
self.t1 = np.array(self.config['motion']['t1'])
self.t2 = np.array(self.config['motion']['t2'])
self.r1 = np.array(self.config['motion']['r1'])
self.r2 = np.array(self.config['motion']['r2'])
self.k = np.array(self.config['gains'])
if self.B.size == 2:
self.B.shape = (2, 1)
# Check formation settings
if len(self.ids) != np.size(self.B, 0):
print(
"The number of rotorcrafts in the topology and ids do not match"
)
return
if np.size(self.d) != np.size(self.B, 1):
print(
"The number of links in the topology and desired distances do not match"
)
return
#if np.size(self.d) != np.size(self.m,1):
# print("The number of (columns) motion parameters and relative vectors do not match")
# return
#if np.size(self.m, 0) != 8:
# print("The number of (rows) motion parameters must be eight")
# return
if self.config['3D'] == True:
print("3D formation is not supported yet")
return
# Start IVY interface
self._interface = IvyMessagesInterface("Formation Control Rotorcrafts")
# bind to INS message
def ins_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "INS":
rc = self.rotorcrafts[self.ids.index(ac_id)]
i2p = 1. / 2**8 # integer to position
i2v = 1. / 2**19 # integer to velocity
rc.X[0] = float(msg['ins_x']) * i2p
rc.X[1] = float(msg['ins_y']) * i2p
rc.X[2] = float(msg['ins_z']) * i2p
rc.V[0] = float(msg['ins_xd']) * i2v
rc.V[1] = float(msg['ins_yd']) * i2v
rc.V[2] = float(msg['ins_zd']) * i2v
rc.timeout = 0
rc.initialized = True
if not self.use_ground_ref:
self._interface.subscribe(ins_cb, PprzMessage("telemetry", "INS"))
# bind to GROUND_REF message
def ground_ref_cb(ground_id, msg):
ac_id = int(msg['ac_id'])
if ac_id in self.ids:
rc = self.rotorcrafts[self.ids.index(ac_id)]
# X and V in NED
rc.X[0] = float(msg['pos'][1])
rc.X[1] = float(msg['pos'][0])
rc.X[2] = -float(msg['pos'][2])
rc.V[0] = float(msg['speed'][1])
rc.V[1] = float(msg['speed'][0])
rc.V[2] = -float(msg['speed'][2])
rc.timeout = 0
rc.initialized = True
if self.use_ground_ref:
self._interface.subscribe(ground_ref_cb,
PprzMessage("ground", "GROUND_REF"))
# bind to JOYSTICK message
def joystick_cb(ac_id, msg):
self.joystick.trans = float(msg['axis1']) * self.sens['t1'] / 127.
self.joystick.trans2 = float(msg['axis2']) * self.sens['t2'] / 127.
self.joystick.rot = float(msg['axis3']) * self.sens['r1'] / 127.
self.altitude = self.sens['alt_min'] + float(msg['axis4']) * (
self.sens['alt_max'] - self.sens['alt_min']) / 127.
if msg['button1'] == '1' and not self.joystick.button1:
self.scale = min(self.scale + self.sens['scale'],
self.sens['scale_max'])
if msg['button2'] == '1' and not self.joystick.button2:
self.scale = max(self.scale - self.sens['scale'],
self.sens['scale_min'])
if msg['button4'] == '1' and not self.joystick.button4:
self.enabled = False
if msg['button3'] == '1' and not self.joystick.button3:
self.enabled = True
self.joystick.button1 = (msg['button1'] == '1')
self.joystick.button2 = (msg['button2'] == '1')
self.joystick.button3 = (msg['button3'] == '1')
self.joystick.button4 = (msg['button4'] == '1')
self._interface.subscribe(joystick_cb,
PprzMessage("ground", "JOYSTICK"))
def __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
def formation(self):
'''
formation control algorithm
'''
ready = True
for rc in self.rotorcrafts:
if not rc.initialized:
if self.verbose:
print("Waiting for state of rotorcraft ", rc.id)
sys.stdout.flush()
ready = False
if rc.timeout > 0.5:
if self.verbose:
print("The state msg of rotorcraft ", rc.id, " stopped")
sys.stdout.flush()
ready = False
if rc.initialized and 'geo_fence' in list(self.config.keys()):
geo_fence = self.config['geo_fence']
if not self.ignore_geo_fence:
if (rc.X[0] < geo_fence['x_min']
or rc.X[0] > geo_fence['x_max']
or rc.X[1] < geo_fence['y_min']
or rc.X[1] > geo_fence['y_max']
or rc.X[2] < geo_fence['z_min']
or rc.X[2] > geo_fence['z_max']):
if self.verbose:
print("The rotorcraft", rc.id,
" is out of the fence (", rc.X, ", ",
geo_fence, ")")
sys.stdout.flush()
ready = False
if not ready:
return
dim = 2 * len(self.rotorcrafts)
X = np.zeros(dim)
V = np.zeros(dim)
U = np.zeros(dim)
i = 0
for rc in self.rotorcrafts:
X[i] = rc.X[0]
X[i + 1] = rc.X[1]
V[i] = rc.V[0]
V[i + 1] = rc.V[1]
i = i + 2
Bb = la.kron(self.B, np.eye(2))
Z = Bb.T.dot(X)
Dz = rf.make_Dz(Z, 2)
Dzt = rf.make_Dzt(Z, 2, 1)
Dztstar = rf.make_Dztstar(self.d * self.scale, 2, 1)
Zh = rf.make_Zh(Z, 2)
E = rf.make_E(Z, self.d * self.scale, 2, 1)
# Shape and motion control
jmu_t1 = self.joystick.trans * self.t1[0, :]
jtilde_mu_t1 = self.joystick.trans * self.t1[1, :]
jmu_r1 = self.joystick.rot * self.r1[0, :]
jtilde_mu_r1 = self.joystick.rot * self.r1[1, :]
jmu_t2 = self.joystick.trans2 * self.t2[0, :]
jtilde_mu_t2 = self.joystick.trans2 * self.t2[1, :]
jmu_r2 = self.joystick.rot2 * self.r2[0, :]
jtilde_mu_r2 = self.joystick.rot2 * self.r2[1, :]
Avt1 = rf.make_Av(self.B, jmu_t1, jtilde_mu_t1)
Avt1b = la.kron(Avt1, np.eye(2))
Avr1 = rf.make_Av(self.B, jmu_r1, jtilde_mu_r1)
Avr1b = la.kron(Avr1, np.eye(2))
Avt2 = rf.make_Av(self.B, jmu_t2, jtilde_mu_t2)
Avt2b = la.kron(Avt2, np.eye(2))
Avr2 = rf.make_Av(self.B, jmu_r2, jtilde_mu_r2)
Avr2b = la.kron(Avr2, np.eye(2))
Avb = Avt1b + Avr1b + Avt2b + Avr2b
Avr = Avr1 + Avr2
if self.B.size == 2:
Zhr = np.array([-Zh[1], Zh[0]])
U = -self.k[1] * V - self.k[0] * Bb.dot(Dz).dot(Dzt).dot(
E) + self.k[1] * (Avt1b.dot(Zh) + Avt2b.dot(Zhr)) + np.sign(
jmu_r1[0]) * la.kron(
Avr1.dot(Dztstar).dot(self.B.T).dot(Avr1),
np.eye(2)).dot(Zhr)
else:
U = -self.k[1] * V - self.k[0] * Bb.dot(Dz).dot(Dzt).dot(
E) + self.k[1] * Avb.dot(Zh) + la.kron(
Avr.dot(Dztstar).dot(self.B.T).dot(Avr), np.eye(2)).dot(Zh)
if self.verbose:
#print "Positions: " + str(X).replace('[','').replace(']','')
#print "Velocities: " + str(V).replace('[','').replace(']','')
#print "Acceleration command: " + str(U).replace('[','').replace(']','')
print("Error distances: " +
str(E).replace('[', '').replace(']', ''))
sys.stdout.flush()
i = 0
for ac in self.rotorcrafts:
msg = PprzMessage("datalink", "DESIRED_SETPOINT")
msg['ac_id'] = ac.id
msg['flag'] = 0 # full 3D not supported yet
msg['ux'] = U[i]
msg['uy'] = U[i + 1]
msg['uz'] = self.altitude
self._interface.send(msg)
i = i + 2
def run(self):
try:
# The main loop
while True:
# TODO: make better frequency managing
sleep(self.step)
for rc in self.rotorcrafts:
rc.timeout = rc.timeout + self.step
# Run the formation algorithm
if self.enabled:
self.formation()
except KeyboardInterrupt:
self.stop()
class OpenSkyTraffic(object):
def __init__(self, period=10., margin=1., timeout=60., verbose=False):
self.period = period
self.margin = margin
self.timeout = timeout
self.last_receive = time()
self.verbose = verbose
self._interface = IvyMessagesInterface("OpenSkyTraffic")
self._opensky = OpenSkyApi()
self.ac_list = {}
self.intruder_list = {}
self.running = False
if self.verbose:
print("Starting opensky interface...")
# subscribe to FLIGHT_PARAM ground message
self._interface.subscribe(self.update_ac, PprzMessage("ground", "FLIGHT_PARAM"))
def stop(self):
if self.verbose:
print("Shutting down opensky interface...")
self.running = False
if self._interface is not None:
self._interface.shutdown()
def __del__(self):
self.stop()
def update_ac(self, ac_id, msg):
# update A/C info
self.last_receive = time()
self.ac_list[ac_id] = (self.last_receive, float(msg['lat']), float(msg['long']))
def filter_ac(self):
# purge timeout A/C
timeout = time() - self.timeout
for ac, (t, lat, lon) in self.ac_list.items():
if t < timeout:
del self.ac_list[ac]
def compute_bbox_list(self):
bbox = []
# for now assume that all UAVs are close enough, so merge all boxes into one
# future improvement could handle groups of UAVs far appart
lat_min, lat_max, lon_min, lon_max = None, None, None, None
for ac, (t, lat, lon) in self.ac_list.items():
if lat_min is None:
lat_min = lat
lat_max = lat
lon_min = lon
lon_max = lon
else:
lat_min = min(lat, lat_min)
lat_max = max(lat, lat_max)
lon_min = min(lon, lon_min)
lon_max = max(lon, lon_max)
if lat_min is not None:
bbox.append((lat_min-self.margin, lat_max+self.margin, lon_min-self.margin, lon_max+self.margin))
return bbox
def get_intruders(self):
self.filter_ac()
bbox = self.compute_bbox_list()
# request surounding aircraft to opensky-network
self.intruder_list = {}
for bb in bbox:
states = self._opensky.get_states(bbox=bb)
if states is not None:
for s in states.states:
self.intruder_list[s.callsign] = s
def send_intruder_msgs(self):
self.get_intruders()
def val_or_default(val, default):
if val is None:
return default
else:
return val
for i in self.intruder_list:
intruder = self.intruder_list[i]
if intruder.callsign is not None and len(intruder.callsign) > 0:
msg = PprzMessage("ground", "INTRUDER")
msg['id'] = intruder.icao24
msg['name'] = intruder.callsign.replace(" ", "")
msg['lat'] = int(intruder.latitude * 1e7)
msg['lon'] = int(intruder.longitude * 1e7)
msg['alt'] = int(val_or_default(intruder.geo_altitude, 0.) * 1000.)
msg['course'] = val_or_default(intruder.heading, 0.)
msg['speed'] = val_or_default(intruder.velocity, 0.)
msg['climb'] = val_or_default(intruder.vertical_rate, 0.)
msg['itow'] = intruder.time_position
if self.verbose:
print(msg)
self._interface.send(msg)
def run(self):
try:
self.running = True
t = 0.
while self.running:
t = t + 0.1 # increment timer until next update time is reach
if t > self.period:
self.send_intruder_msgs()
t = 0.
sleep(0.1) # wait a short time
except KeyboardInterrupt:
self.stop()
except Exception as e:
print("Failing with: "+str(e))
self.stop()
class FormationControl:
def __init__(self, config, freq=10., use_ground_ref=False, ignore_geo_fence=False, verbose=False):
self.config = config
self.step = 1. / freq
self.sens = self.config['sensitivity']
self.use_ground_ref = use_ground_ref
self.enabled = True # run control by default
self.ignore_geo_fence = ignore_geo_fence
self.verbose = verbose
self.ids = self.config['ids']
self.rotorcrafts = [FC_Rotorcraft(i) for i in self.ids]
self.joystick = FC_Joystick()
self.altitude = 2.0 # starts from 1 m high
self.scale = 1.0
self.B = np.array(self.config['topology'])
self.d = np.array(self.config['desired_distances'])
self.t1 = np.array(self.config['motion']['t1'])
self.t2 = np.array(self.config['motion']['t2'])
self.r1 = np.array(self.config['motion']['r1'])
self.r2 = np.array(self.config['motion']['r2'])
self.k = np.array(self.config['gains'])
if self.B.size == 2:
self.B.shape = (2,1)
# Check formation settings
if len(self.ids) != np.size(self.B, 0):
print("The number of rotorcrafts in the topology and ids do not match")
return
if np.size(self.d) != np.size(self.B, 1):
print("The number of links in the topology and desired distances do not match")
return
#if np.size(self.d) != np.size(self.m,1):
# print("The number of (columns) motion parameters and relative vectors do not match")
# return
#if np.size(self.m, 0) != 8:
# print("The number of (rows) motion parameters must be eight")
# return
if self.config['3D'] == True:
print("3D formation is not supported yet")
return
# Start IVY interface
self._interface = IvyMessagesInterface("Formation Control Rotorcrafts")
# bind to INS message
def ins_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "INS":
rc = self.rotorcrafts[self.ids.index(ac_id)]
i2p = 1. / 2**8 # integer to position
i2v = 1. / 2**19 # integer to velocity
rc.X[0] = float(msg['ins_x']) * i2p
rc.X[1] = float(msg['ins_y']) * i2p
rc.X[2] = float(msg['ins_z']) * i2p
rc.V[0] = float(msg['ins_xd']) * i2v
rc.V[1] = float(msg['ins_yd']) * i2v
rc.V[2] = float(msg['ins_zd']) * i2v
rc.timeout = 0
rc.initialized = True
if not self.use_ground_ref:
self._interface.subscribe(ins_cb, PprzMessage("telemetry", "INS"))
# bind to GROUND_REF message
def ground_ref_cb(ground_id, msg):
ac_id = int(msg['ac_id'])
if ac_id in self.ids:
rc = self.rotorcrafts[self.ids.index(ac_id)]
# X and V in NED
rc.X[0] = float(msg['pos'][1])
rc.X[1] = float(msg['pos'][0])
rc.X[2] = -float(msg['pos'][2])
rc.V[0] = float(msg['speed'][1])
rc.V[1] = float(msg['speed'][0])
rc.V[2] = -float(msg['speed'][2])
rc.timeout = 0
rc.initialized = True
if self.use_ground_ref:
self._interface.subscribe(ground_ref_cb, PprzMessage("ground", "GROUND_REF"))
# bind to JOYSTICK message
def joystick_cb(ac_id, msg):
self.joystick.trans = float(msg['axis1']) * self.sens['t1'] / 127.
self.joystick.trans2 = float(msg['axis2']) * self.sens['t2'] / 127.
self.joystick.rot = float(msg['axis3']) * self.sens['r1'] / 127.
self.altitude = self.sens['alt_min'] + float(msg['axis4']) * (self.sens['alt_max'] - self.sens['alt_min']) / 127.
if msg['button1'] == '1' and not self.joystick.button1:
self.scale = min(self.scale + self.sens['scale'], self.sens['scale_max'])
if msg['button2'] == '1' and not self.joystick.button2:
self.scale = max(self.scale - self.sens['scale'], self.sens['scale_min'])
if msg['button4'] == '1' and not self.joystick.button4:
self.enabled = False
if msg['button3'] == '1' and not self.joystick.button3:
self.enabled = True
self.joystick.button1 = (msg['button1'] == '1')
self.joystick.button2 = (msg['button2'] == '1')
self.joystick.button3 = (msg['button3'] == '1')
self.joystick.button4 = (msg['button4'] == '1')
self._interface.subscribe(joystick_cb, PprzMessage("ground", "JOYSTICK"))
def __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
def formation(self):
'''
formation control algorithm
'''
ready = True
for rc in self.rotorcrafts:
if not rc.initialized:
if self.verbose:
print("Waiting for state of rotorcraft ", rc.id)
sys.stdout.flush()
ready = False
if rc.timeout > 0.5:
if self.verbose:
print("The state msg of rotorcraft ", rc.id, " stopped")
sys.stdout.flush()
ready = False
if rc.initialized and 'geo_fence' in self.config.keys():
geo_fence = self.config['geo_fence']
if not self.ignore_geo_fence:
if (rc.X[0] < geo_fence['x_min'] or rc.X[0] > geo_fence['x_max']
or rc.X[1] < geo_fence['y_min'] or rc.X[1] > geo_fence['y_max']
or rc.X[2] < geo_fence['z_min'] or rc.X[2] > geo_fence['z_max']):
if self.verbose:
print("The rotorcraft", rc.id, " is out of the fence (", rc.X, ", ", geo_fence, ")")
sys.stdout.flush()
ready = False
if not ready:
return
dim = 2 * len(self.rotorcrafts)
X = np.zeros(dim)
V = np.zeros(dim)
U = np.zeros(dim)
i = 0
for rc in self.rotorcrafts:
X[i] = rc.X[0]
X[i+1] = rc.X[1]
V[i] = rc.V[0]
V[i+1] = rc.V[1]
i = i + 2
Bb = la.kron(self.B, np.eye(2))
Z = Bb.T.dot(X)
Dz = rf.make_Dz(Z, 2)
Dzt = rf.make_Dzt(Z, 2, 1)
Dztstar = rf.make_Dztstar(self.d * self.scale, 2, 1)
Zh = rf.make_Zh(Z, 2)
E = rf.make_E(Z, self.d * self.scale, 2, 1)
# Shape and motion control
jmu_t1 = self.joystick.trans * self.t1[0,:]
jtilde_mu_t1 = self.joystick.trans * self.t1[1,:]
jmu_r1 = self.joystick.rot * self.r1[0,:]
jtilde_mu_r1 = self.joystick.rot * self.r1[1,:]
jmu_t2 = self.joystick.trans2 * self.t2[0,:]
jtilde_mu_t2 = self.joystick.trans2 * self.t2[1,:]
jmu_r2 = self.joystick.rot2 * self.r2[0,:]
jtilde_mu_r2 = self.joystick.rot2 * self.r2[1,:]
Avt1 = rf.make_Av(self.B, jmu_t1, jtilde_mu_t1)
Avt1b = la.kron(Avt1, np.eye(2))
Avr1 = rf.make_Av(self.B, jmu_r1, jtilde_mu_r1)
Avr1b = la.kron(Avr1, np.eye(2))
Avt2 = rf.make_Av(self.B, jmu_t2, jtilde_mu_t2)
Avt2b = la.kron(Avt2, np.eye(2))
Avr2 = rf.make_Av(self.B, jmu_r2, jtilde_mu_r2)
Avr2b = la.kron(Avr2, np.eye(2))
Avb = Avt1b + Avr1b + Avt2b + Avr2b
Avr = Avr1 + Avr2
if self.B.size == 2:
Zhr = np.array([-Zh[1],Zh[0]])
U = -self.k[1]*V - self.k[0]*Bb.dot(Dz).dot(Dzt).dot(E) + self.k[1]*(Avt1b.dot(Zh) + Avt2b.dot(Zhr)) + np.sign(jmu_r1[0])*la.kron(Avr1.dot(Dztstar).dot(self.B.T).dot(Avr1), np.eye(2)).dot(Zhr)
else:
U = -self.k[1]*V - self.k[0]*Bb.dot(Dz).dot(Dzt).dot(E) + self.k[1]*Avb.dot(Zh) + la.kron(Avr.dot(Dztstar).dot(self.B.T).dot(Avr), np.eye(2)).dot(Zh)
if self.verbose:
#print "Positions: " + str(X).replace('[','').replace(']','')
#print "Velocities: " + str(V).replace('[','').replace(']','')
#print "Acceleration command: " + str(U).replace('[','').replace(']','')
print "Error distances: " + str(E).replace('[','').replace(']','')
sys.stdout.flush()
i = 0
for ac in self.rotorcrafts:
msg = PprzMessage("datalink", "DESIRED_SETPOINT")
msg['ac_id'] = ac.id
msg['flag'] = 0 # full 3D not supported yet
msg['ux'] = U[i]
msg['uy'] = U[i+1]
msg['uz'] = self.altitude
self._interface.send(msg)
i = i+2
def run(self):
try:
# The main loop
while True:
# TODO: make better frequency managing
sleep(self.step)
for rc in self.rotorcrafts:
rc.timeout = rc.timeout + self.step
# Run the formation algorithm
if self.enabled:
self.formation()
except KeyboardInterrupt:
self.stop()
class Aircraft(object):
def __init__(self, ac_id, freq=100.):
self.id = ac_id
self.pos = np.array([[0.0], [0.0], [0.0]])
self.attitude = np.array([[0.0], [0.0], [0.0]])
self.vel = np.array([[0.0], [0.0], [0.0]])
self.step = 1. / freq
self._interface = IvyMessagesInterface("Controller 1")
self.target_pos = np.array([[5.0], [7.0], [10.0]])
self.target_vel = np.array([[0.0], [0.0], [0.0]])
self.error_pos = np.array([[0.0], [0.0], [0.0]])
self.error_vel = np.array([[0.0], [0.0], [0.0]])
self.U = np.array([[0.0], [0.0]]) # Accelerations
# Circle
self.e = 0.0
self.grad = np.array([[0.0], [0.0]])
self.hess = np.array([[2.0, 0.0], [0.0, 2.0]])
self.rot = np.array([[0.0, -1.0], [1.0, 0.0]])
self.vel_d = np.array([[0.0], [0.0]])
self.s = 1.
self.c1 = 0.005
self.c2 = 7.
self.c3 = 1.25
def rotorcraft_fp_callback(_id, msg):
if msg.name == "ROTORCRAFT_FP":
field_coefs = msg.fieldcoefs
self.pos[0][0] = float(msg.get_field(0)) * field_coefs[0]
self.pos[1][0] = float(msg.get_field(1)) * field_coefs[1]
self.pos[2][0] = float(msg.get_field(2)) * field_coefs[2]
self.vel[0][0] = float(msg.get_field(3)) * field_coefs[3]
self.vel[1][0] = float(msg.get_field(4)) * field_coefs[4]
self.vel[2][0] = float(msg.get_field(5)) * field_coefs[5]
self.attitude[0][0] = float(msg.get_field(6)) * field_coefs[6]
self.attitude[1][0] = float(msg.get_field(7)) * field_coefs[7]
self.attitude[2][0] = float(msg.get_field(8)) * field_coefs[8]
self._interface.subscribe(rotorcraft_fp_callback,
PprzMessage("telemetry", "ROTORCRAFT_FP"))
def print(self):
print("\n\nID: \t" + str(self.id) + "\n\neast: \t" +
str(self.pos[0][0]) + "\nnorth: \t" + str(self.pos[1][0]) +
"\nup: \t" + str(self.pos[2][0]) + "\n\nRoll: \t" +
str(self.attitude[0][0]) + "\nPitch: \t" +
str(self.attitude[1][0]) + "\nYaw: \t" +
str(self.attitude[2][0]) + "\nU: \t" +
str(np.linalg.norm(self.U[0:2])) + "\nerror: \t" + str(self.U))
def send_accelerations(self):
msg = PprzMessage("datalink", "DESIRED_SETPOINT")
msg['ac_id'] = self.id
msg['flag'] = 0 # full 3D not supported yet
# Receiving position in ENU, but sending in NED:
msg['uy'] = self.U[0][0]
msg['ux'] = self.U[1][0]
msg['uz'] = 20 #self.U[2][0] # Sending altitude, not acceleration
self._interface.send(msg)
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
def run(self):
try:
# The main loop
while True:
#sleep(self.step)
F = -self.c1 * self.e * self.grad
L = -self.c2 * (self.vel[0:2] - self.vel_d)
origin = [0], [0] # origin point
plt.cla()
plt.quiver(*origin,
self.vel_d[0],
self.vel_d[1],
color=['r'],
angles='xy',
scale_units='xy',
scale=1)
plt.quiver(*origin,
F[0],
F[1],
color=['b'],
angles='xy',
scale_units='xy',
scale=1)
plt.quiver(*origin,
L[0],
L[1],
color=['g'],
angles='xy',
scale_units='xy',
scale=1)
#plt.quiver(*origin, self.vel[0], self.vel[1], color=['b'], angles='xy', scale_units='xy', scale=1)
#plt.quiver(*origin, self.U[0], self.U[1], color=['g'], angles='xy', scale_units='xy', scale=100)
#plt.autoscale(enable=True, axis='both', tight=None)
plt.xlim(-10, 10)
plt.ylim(-10, 10)
plt.pause(self.step)
self.circle_path()
self.calculate_accelerations_path()
self.send_accelerations()
#self.print()
except KeyboardInterrupt:
self.stop()
def calculate_accelerations_path(self):
xt = self.rot.dot(self.grad)
xt_dot = self.rot.dot(self.hess).dot(self.vel[0:2])
xt_norm = np.where(
np.linalg.norm(xt) != 0., np.linalg.norm(xt), 0.00001)
self.vel_d = self.s * xt / xt_norm
xt_dot_xt = np.where(
np.transpose(xt).dot(xt) != 0.,
np.transpose(xt).dot(xt)**(-3. / 2.), 1.)
acc_d = xt_dot / xt_norm + xt * (
-np.transpose(xt).dot(xt_dot)) * xt_dot_xt
self.U = -self.c1 * self.e * self.grad - self.c2 * (
self.vel[0:2] - self.vel_d) + self.c3 * acc_d
def circle_path(self):
radius = 10.
center = np.array([[0.0], [0.0]])
self.e = (self.pos[0][0] - center[0][0])**2 + (
self.pos[1][0] - center[1][0])**2 - radius**2
self.grad[0][0] = 2.0 * (self.pos[0][0] - center[0][0])
self.grad[1][0] = 2.0 * (self.pos[1][0] - center[1][0])
class PaparazziEnvironment(object):
"""
PaparazziEnvironment holds environment data (eg. wind)
and sends them to paparazzi.
"""
BIND_REGEX = "(^[0-9]* NPS_SPEED_POS .*)"
def __init__(self, nps_addr=None, time_scale=1):
"""
Initialize PaparazziEnvironment.
Arguments:
nps_addr: (optional) key-value pairs of ac_id and corresponding
(host, port). If None, use Ivy bus instead of UDP datagrams.
time_scale: simulation speed multiplier (default 1)
"""
# init ROS node
rospy.init_node('paparazzienvironment')
rospy.wait_for_service('/get_wind')
self.ivy_msg = None
self.nps_udp_socket = None
self.time_scale = time_scale
if nps_addr is not None:
self.use_udp = True
self.nps_addr = nps_addr
self.start_time = rospy.get_time()
self.wind_udp_packer = struct.Struct('d f f f f f I')
self.nps_udp_socket = socket.socket(socket.AF_INET,
socket.SOCK_DGRAM)
else:
self.use_udp = False
# Set up get_wind client
self.get_wind = rospy.ServiceProxy('/get_wind', GetWind)
self.get_wind.wait_for_service(timeout=10)
# initiate ivy message interface catching position messages
self.ivy_msg = IvyMessagesInterface(start_ivy=True, verbose=False)
self.ivy_msg.subscribe(self.on_ivy_msg_recv,
PaparazziEnvironment.BIND_REGEX)
self.init_time = rospy.Time.now()
self._last_log = 0
self._log_interval = 1 # in seconds
def on_ivy_msg_recv(self, ac_id, msg):
"""Handle state callback from ivy bus.
Arguments:
ac_id: aircraft id.
msg: PprzMessage.
"""
if msg.msg_class == "telemetry" and msg.name == "NPS_SPEED_POS":
x = float(msg.ltpp_x)
y = float(msg.ltpp_y)
z = float(msg.ltpp_z)
t = rospy.Time.now() - self.init_time
w = self.get_wind(ac_id, t, Point(x, y, z)).wind
if self.use_udp:
self.send_wind_udp(ac_id, w.east, w.north, w.up)
else:
self.send_wind_ivy(w.east, w.north, w.up, self.time_scale)
def send_wind_ivy(self, wind_east, wind_north, wind_up, time_scale):
"""Send wind to paparazzi in a ground.WORLD_ENV message using ivy."""
msg = PprzMessage('ground', 'WORLD_ENV')
msg['wind_east'] = wind_east
msg['wind_north'] = wind_north
# TODO: Check whether sign inversion below is correct or not
msg['wind_up'] = -wind_up
msg['ir_contrast'] = 400 # preset value
msg['time_scale'] = time_scale
msg['gps_availability'] = 1 # preset value
self.ivy_msg.send(msg)
# log wind message
if self._last_log + self._log_interval <= time.time():
self._last_log = time.time()
log_str = str(msg)
rospy.loginfo(log_str)
def send_wind_udp(self, ac_id, wind_east, wind_north, wind_up):
"""Send wind to paparazzi simulator using udp."""
# TODO: Allow time_scale control when using UDP
if ac_id not in self.nps_addr:
rospy.logerr("No (host, port) defined for ac_id {}".format(ac_id))
elapsed = rospy.get_time() - self.start_time
(speed, heading) = cmath.polar(wind_north + wind_east * 1j)
values = (elapsed, wind_north, wind_east, wind_up, heading, speed,
0x12345678)
message = self.wind_udp_packer.pack(*values)
rospy.loginfo(values)
rospy.loginfo(self.nps_addr[ac_id])
self.nps_udp_socket.sendto(message, self.nps_addr[ac_id])
if len(sys.argv) != 3:
print("Usage: dcfInitTables target_id aircraft_ids.txt")
interface.shutdown()
exit()
target_id = int(sys.argv[1])
ids = np.loadtxt(sys.argv[2])
list_ids = np.ndarray.tolist(ids)
time.sleep(2)
for i in list_ids:
msg_clean = PprzMessage("datalink", "CTC_CLEAN_TABLE")
msg_clean['ac_id'] = int(i)
print(msg_clean)
interface.send(msg_clean)
for i in list_ids :
msg = PprzMessage("datalink", "CTC_REG_TABLE")
msg['ac_id'] = int(i)
for ii in list_ids:
if (i != ii):
msg['nei_id'] = int(ii)
interface.send(msg)
print(msg)
time.sleep(2)
interface.shutdown()
class Guidance(object):
def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
self.ap_mode = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'].index
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("gb2ivy")
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 bind_flight_param(self, send_cb):
def bat_cb(ac_id, msg):
bat = float(msg['bat'])
# should not be more that 18 characters
send_cb('bat: '+str(bat)+' V')
self._interface.subscribe(bat_cb, regex=('(^ground ENGINE_STATUS '+str(self.ac_id)+' .*)'))
def set_guided_mode(self):
"""
change mode to GUIDED.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode
msg['value'] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change mode to NAV.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode
msg['value'] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def move_at_body_vel(self, forward=0.0, right=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'] = 0xE2
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
def command_callback(self, command):
"""
convert incoming command into velocity
"""
right = 0.0
forward = 0.0
down = 0.0
yaw = 0.0
command = int(command)
if command & J_RIGHT:
right += 2.0
if command & J_LEFT:
right -= 2.0
if command & J_UP:
forward += 2.0
if command & J_DOWN:
forward -= 2.0
if command & J_A:
down -= 1.0
if command & J_B:
down += 1.0
if command & J_START:
yaw += radians(20)
if command & J_SELECT:
yaw -= radians(20)
self.move_at_body_vel(forward, right, down, yaw)
class initTable:
def __init__(self, config, verbose=False):
self.verbose = verbose
self.config = config
self.ids = np.array(self.config['ids'])
self.B = np.array(self.config['topology'])
self.Zdesired = np.array(self.config['desired_intervehicle_angles'])
self.list_ids = np.ndarray.tolist(self.ids)
# Start IVY interface
self._interface = IvyMessagesInterface("Init DCF tables")
def __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
def init_dcftables(self):
time.sleep(2)
for count, column in enumerate(self.B.T):
index = np.nonzero(column)
i = index[0]
# nei_id = 0, special msg to clean the table onboard
msg_clean_a = PprzMessage("datalink", "DCF_REG_TABLE")
msg_clean_a['ac_id'] = int(self.list_ids[i[0]])
msg_clean_a['nei_id'] = 0
msg_clean_b = PprzMessage("datalink", "DCF_REG_TABLE")
msg_clean_b['ac_id'] = int(self.list_ids[i[1]])
msg_clean_b['nei_id'] = 0
self._interface.send(msg_clean_a)
self._interface.send(msg_clean_b)
if self.verbose:
print(msg_clean_a)
print(msg_clean_b)
for count, column in enumerate(self.B.T):
index = np.nonzero(column)
i = index[0]
msga = PprzMessage("datalink", "DCF_REG_TABLE")
msga['ac_id'] = int(self.list_ids[i[0]])
msga['nei_id'] = int(self.list_ids[i[1]])
if len(self.list_ids) == 2:
msga['desired_sigma'] = (column[index])[0] * int(self.Zdesired)
else:
msga['desired_sigma'] = (column[index])[0] * int(
self.Zdesired[count])
self._interface.send(msga)
msgb = PprzMessage("datalink", "DCF_REG_TABLE")
msgb['ac_id'] = int(self.list_ids[i[1]])
msgb['nei_id'] = int(self.list_ids[i[0]])
if len(self.list_ids) == 2:
msgb['desired_sigma'] = (column[index])[1] * int(self.Zdesired)
else:
msgb['desired_sigma'] = (column[index])[1] * int(
self.Zdesired[count])
self._interface.send(msgb)
if self.verbose:
print(msga)
print(msgb)
time.sleep(2)
class initTable:
def __init__(self, config, verbose=False):
self.verbose = verbose
self.config = config
self.ids = np.array(self.config['ids'])
self.B = np.array(self.config['topology'])
self.Zdesired = np.array(self.config['desired_intervehicle_angles'])
self.list_ids = np.ndarray.tolist(self.ids)
# Start IVY interface
self._interface = IvyMessagesInterface("Init DCF tables")
def __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
def init_dcftables(self):
time.sleep(2)
for count, column in enumerate(self.B.T):
index = np.nonzero(column)
i = index[0]
# nei_id = 0, special msg to clean the table onboard
msg_clean_a = PprzMessage("datalink", "DCF_REG_TABLE")
msg_clean_a['ac_id'] = int(self.list_ids[i[0]])
msg_clean_a['nei_id'] = 0
msg_clean_b = PprzMessage("datalink", "DCF_REG_TABLE")
msg_clean_b['ac_id'] = int(self.list_ids[i[1]])
msg_clean_b['nei_id'] = 0
self._interface.send(msg_clean_a)
self._interface.send(msg_clean_b)
if self.verbose:
print(msg_clean_a)
print(msg_clean_b)
for count, column in enumerate(self.B.T):
index = np.nonzero(column)
i = index[0]
msga = PprzMessage("datalink", "DCF_REG_TABLE")
msga['ac_id'] = int(self.list_ids[i[0]])
msga['nei_id'] = int(self.list_ids[i[1]])
if len(self.list_ids) == 2:
msga['desired_sigma'] = (column[index])[0]*int(self.Zdesired)
else:
msga['desired_sigma'] = (column[index])[0]*int(self.Zdesired[count])
self._interface.send(msga)
msgb = PprzMessage("datalink", "DCF_REG_TABLE")
msgb['ac_id'] = int(self.list_ids[i[1]])
msgb['nei_id'] = int(self.list_ids[i[0]])
if len(self.list_ids) == 2:
msgb['desired_sigma'] = (column[index])[1]*int(self.Zdesired)
else:
msgb['desired_sigma'] = (column[index])[1]*int(self.Zdesired[count])
self._interface.send(msgb)
if self.verbose:
print(msga)
print(msgb)
time.sleep(2)
print("You need to define an aircraft ID")
exit()
# start ivy interface
if args.ivy_bus is not None:
ivy = IvyMessagesInterface("herelink2ivy", ivy_bus=args.ivy_bus)
else:
ivy = IvyMessagesInterface("herelink2ivy")
# Start a connection listening to a UDP port
conn = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
print("Starting Herelink RSSI for ac_id %d" % (args.ac_id))
try:
# Start up the streaming client.
while True:
msg = conn.recv_match(type='RADIO_STATUS', blocking=True)
if not msg or msg.get_type() == "BAD_DATA":
continue
pmsg = PprzMessage("telemetry", "RSSI_COMBINED")
pmsg['remote_rssi'] = msg.remrssi
pmsg['tx_power'] = 0
pmsg['local_rssi'] = msg.rssi
pmsg['local_noise'] = msg.noise
pmsg['remote_noise'] = msg.remnoise
ivy.send(pmsg, args.ac_id)
except (KeyboardInterrupt, SystemExit):
print("Shutting down ivy interface...")
ivy.shutdown()
class Guidance(object):
def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
self.ap_mode = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'] # try classic name
except Exception as e:
try:
self.ap_mode = settings.name_lookup[
'ap'] # in case it is a generated autopilot
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("gb2ivy")
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 bind_flight_param(self, send_cb):
def bat_cb(ac_id, msg):
bat = float(msg['bat'])
# should not be more that 18 characters
send_cb('bat: ' + str(bat) + ' V')
self._interface.subscribe(bat_cb,
regex=('(^ground ENGINE_STATUS ' +
str(self.ac_id) + ' .*)'))
def set_guided_mode(self):
"""
change mode to GUIDED.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName(
'Guided') # AP_MODE_GUIDED
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'GUIDED') # AP_MODE_GUIDED
except ValueError:
msg['value'] = 19 # fallback to fixed index
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change mode to NAV.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName('Nav') # AP_MODE_NAV
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'NAV') # AP_MODE_NAV
except ValueError:
msg['value'] = 13 # fallback to fixed index
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def move_at_body_vel(self, forward=0.0, right=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'] = 0xE2
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
def command_callback(self, command):
"""
convert incoming command into velocity
"""
right = 0.0
forward = 0.0
down = 0.0
yaw = 0.0
command = int(command)
if command & J_RIGHT:
right += 2.0
if command & J_LEFT:
right -= 2.0
if command & J_UP:
forward += 2.0
if command & J_DOWN:
forward -= 2.0
if command & J_A:
down -= 1.0
if command & J_B:
down += 1.0
if command & J_START:
yaw += radians(20)
if command & J_SELECT:
yaw -= radians(20)
self.move_at_body_vel(forward, right, down, yaw)
class CommandSender(IvyMessagesInterface):
def __init__(self, verbose=False, callback = None):
self.verbose = verbose
self.callback = callback
self._interface = IvyMessagesInterface("Mission Commander", start_ivy=False)
self._interface.subscribe(self.message_recv)
self._interface.start()
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_dict(self, ac_id, insert, msg_id, msgs):
msg = PprzMessage("datalink", msg_id)
msg['ac_id'] = ac_id
msg['insert'] = insert
msg['duration'] = msgs.get('duration')
msg['index'] = msgs.get('index')
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_shape(self, status, obstacle_id, obmsg):
msg = PprzMessage("ground", "SHAPE")
msg['id'] = int(obstacle_id)
msg['fillcolor'] = "red" if obstacle_id > 19 else "orange"
msg['linecolor'] = "red" if obstacle_id > 19 else "orange"
msg['status'] = 0 if status=="create" else 1
msg['shape'] = 0
msg['latarr'] = [int(obmsg.get("latitude")*10000000.),int(obmsg.get("latitude")*10000000.)]
msg['lonarr'] = [int(obmsg.get("longitude")*10000000.),int(obmsg.get("longitude")*10000000.)]
msg['radius'] = int(obmsg.get("sphere_radius") if "sphere_radius" in obmsg else obmsg.get("cylinder_radius"))
msg['text'] = "NULL"
msg['opacity'] = 2
self._interface.send(msg)
class MessageInterface:
"""
MessageInterface
This is an abstraction layer used to simplify the use of paparazzi
message interface, especially for time measurement and casting of
message payload data (which sometimes stays in ascii)
"""
def prettify_message(msg):
"""
Sometimes IvyMessageInterface does not cast data to their binary types.
This function cast all fields to their binary types. (supposed to be
done by PprzMessage.payload_to_binay but does not seem to be working)
It also measure reception time.
"""
timestamp = time.time()
fieldValues = []
for fieldValue, fieldType in zip(msg.fieldvalues, msg.fieldtypes):
if "int" in fieldType:
castType = int
elif "float" in fieldType:
castType = float
elif "string" in fieldType:
castType = str
elif "char" in fieldType:
castType = int
else:
# Could not indentify type, leave field as is
fieldValues.append(fieldValue)
# Checking if is a list
if '[' in fieldType:
fieldValues.append([castType(value) for value in fieldValue])
else:
fieldValues.append(castType(fieldValue))
msg.set_values(fieldValues)
msg.timestamp = timestamp
return msg
def parse_pprz_msg(msg):
"""
Alias to IvyMessageInterface.parse_pprz_msg, but with prettify_message
called at the end to ensure all data are in binary format.
"""
class Catcher:
"""
This is a type specifically to catch result from
IvyMessageInterface.parse_pprz_msg which only outputs result via a
callback.
"""
def set_message(self, aircraftId, message):
self.message = message
self.aircraftId = str(aircraftId)
catcher = Catcher()
IvyMessagesInterface.parse_pprz_msg(catcher.set_message, msg)
return [MessageInterface.prettify_message(catcher.message),
catcher.aircraftId]
def __init__(self, ivyBusAddress=None):
if ivyBusAddress is None:
ivyBusAddress = os.getenv('IVY_BUS')
if ivyBusAddress is None:
ivyBusAddress == ""
self.messageInterface = IvyMessagesInterface(ivy_bus=ivyBusAddress)
def bind(self, callback, ivyRegex):
return self.messageInterface.subscribe(
lambda sender, msg: callback(MessageInterface.prettify_message(msg)),
ivyRegex)
def bind_raw(self, callback, ivyRegex):
return self.messageInterface.bind_raw(callback, ivyRegex)
def unbind(self, bindId):
self.messageInterface.unbind(bindId)
def send(self, msg):
return self.messageInterface.send(msg)
class FormationControl:
def __init__(self, config, freq=10., verbose=False):
self.config = config
self.step = 1. / freq
self.verbose = verbose
self.ids = self.config['ids']
self.B = np.array(self.config['topology'])
self.Zdesired = np.array(self.config['desired_intervehicle_angles_degrees'])*np.pi/180
self.k = np.array(self.config['gain'])
self.radius = np.array(self.config['desired_stationary_radius_meters'])
self.aircraft = [Aircraft(i) for i in self.ids]
self.sigmas = np.zeros(len(self.aircraft))
for ac in self.aircraft:
settings = PaparazziACSettings(ac.id)
list_of_indexes = ['ell_a', 'ell_b']
for setting_ in list_of_indexes:
try:
index = settings.name_lookup[setting_].index
if setting_ == 'ell_a':
ac.a_index = index
if setting_ == 'ell_b':
ac.b_index = index
except Exception as e:
print(e)
print(setting_ + " setting not found, \
have you forgotten to check gvf.xml for your settings?")
# Start IVY interface
self._interface = IvyMessagesInterface("Circular Formation")
# bind to NAVIGATION message
def nav_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "NAVIGATION":
ac = self.aircraft[self.ids.index(ac_id)]
ac.XY[0] = float(msg.get_field(2))
ac.XY[1] = float(msg.get_field(3))
ac.initialized_nav = True
self._interface.subscribe(nav_cb, PprzMessage("telemetry", "NAVIGATION"))
def gvf_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "GVF":
if int(msg.get_field(1)) == 1:
ac = self.aircraft[self.ids.index(ac_id)]
param = msg.get_field(4)
ac.XYc[0] = float(param[0])
ac.XYc[1] = float(param[1])
ac.a = float(param[2])
ac.b = float(param[3])
ac.s = float(msg.get_field(2))
ac.initialized_gvf = True
self._interface.subscribe(gvf_cb, PprzMessage("telemetry", "GVF"))
def __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
def circular_formation(self):
'''
circular formation control algorithm
'''
ready = True
for ac in self.aircraft:
if (not ac.initialized_nav) or (not ac.initialized_gvf):
if self.verbose:
print("Waiting for state of aircraft ", ac.id)
ready = False
if not ready:
return
i = 0
for ac in self.aircraft:
ac.sigma = np.arctan2(ac.XY[1]-ac.XYc[1], ac.XY[0]-ac.XYc[0])
self.sigmas[i] = ac.sigma
i = i + 1
inter_sigma = self.B.transpose().dot(self.sigmas)
error_sigma = inter_sigma - self.Zdesired
if np.size(error_sigma) > 1:
for i in range(0, np.size(error_sigma)):
if error_sigma[i] > np.pi:
error_sigma[i] = error_sigma[i] - 2*np.pi
elif error_sigma[i] <= -np.pi:
error_sigma[i] = error_sigma[i] + 2*np.pi
else:
if error_sigma > np.pi:
error_sigma = error_sigma - 2*np.pi
elif error_sigma <= -np.pi:
error_sigma = error_sigma + 2*np.pi
u = -self.aircraft[0].s*self.k*self.B.dot(error_sigma)
if self.verbose:
print("Inter-vehicle errors: ", str(error_sigma*180.0/np.pi).replace('[','').replace(']',''))
i = 0
for ac in self.aircraft:
msga = PprzMessage("ground", "DL_SETTING")
msga['ac_id'] = ac.id
msga['index'] = ac.a_index
msga['value'] = self.radius + u[i]
msgb = PprzMessage("ground", "DL_SETTING")
msgb['ac_id'] = ac.id
msgb['index'] = ac.b_index
msgb['value'] = self.radius + u[i]
self._interface.send(msga)
self._interface.send(msgb)
i = i + 1
def run(self):
try:
# The main loop
while True:
# TODO: make better frequency managing
sleep(self.step)
# Run the formation algorithm
self.circular_formation()
except KeyboardInterrupt:
self.stop()
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)
return msg_shape
def line(shape_id):
msg_shape = PprzMessage("ground", "SHAPE")
msg_shape['id'] = shape_id
msg_shape['linecolor'] = 'purple'
msg_shape['shape'] = Shape.LINE.value
msg_shape['status'] = Status.CREATE.value
msg_shape['latarr'] = [int(43.46296 * 1e7), int(43.46216 * 1e7)]
msg_shape['lonarr'] = [int(1.272 * 1e7), int(1.2744 * 1e7)]
msg_shape['text'] = "runway"
return msg_shape
if __name__ == '__main__':
ivy = IvyMessagesInterface("Shape drawer")
# wait for ivy to be correctly started
time.sleep(0.1)
msg = polygon(1)
ivy.send(msg)
msg = circle(2)
ivy.send(msg)
msg = line(3)
ivy.send(msg)
ivy.shutdown()
