From f6b662b8cb429c057feb2ae286fe398c0b01a4f1 Mon Sep 17 00:00:00 2001 From: Hector Garcia de Marina Date: Sun, 19 Mar 2017 14:41:28 +0100 Subject: [PATCH] Circular formation script (#2038) * now we process only the msgs from the ac_id aircraft * now we process only the msgs from the ac_id aircraft * Circular formation Python script --- sw/ground_segment/python/gvf/gvfFormation.py | 161 +++++++++++++++++++ 1 file changed, 161 insertions(+) create mode 100644 sw/ground_segment/python/gvf/gvfFormation.py diff --git a/sw/ground_segment/python/gvf/gvfFormation.py b/sw/ground_segment/python/gvf/gvfFormation.py new file mode 100644 index 0000000000..74eba6c721 --- /dev/null +++ b/sw/ground_segment/python/gvf/gvfFormation.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python + +import time +import sys +import wx +import numpy as np +import sys +from os import path, getenv +PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../'))) +sys.path.append(PPRZ_SRC + "/sw/lib/python") +sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python") +from pprzlink.ivy import IvyMessagesInterface +from pprzlink.message import PprzMessage +from settings_xml_parse import PaparazziACSettings + +class aircraft: + def __init__(self, ac_id): + self.id = ac_id + self.XY = np.array([-999, -999]) + self.XYc = np.array([-999, -999]) + self.a = -999 + self.b = -999 + + self.sigma = -999 + + self.a_index = -999 + self.b_index = -999 + + self.time = -999 + +list_ids = [] +list_aircraft = [] +interface = IvyMessagesInterface("GVF Formation") + +def message_recv(ac_id, msg): + if ac_id in list_ids: + ac = list_aircraft[list_ids.index(ac_id)] + + if msg.name == 'NAVIGATION': + ac.XY[0] = float(msg.get_field(2)) + ac.XY[1] = float(msg.get_field(3)) + + if msg.name == 'GVF': + if int(msg.get_field(1)) == 1: + param = [float(x) for x in msg.get_field(3).split(',')] + ac.XYc[0] = param[0] + ac.XYc[1] = param[1] + ac.a = param[2] + ac.b = param[3] + + if msg.name == 'BAT': + ac.time = float(msg.get_field(3)) + return + +def formation(B, ds, radius, k): + no_telemetry = 0 + for ac in list_aircraft: + if ac.a == -999 or ac.XY[0] == -999: + print "Waiting for telemetry of aircraft ", ac.id + no_telemetry = 1 + + if no_telemetry: + return + + sigma = np.zeros(len(list_aircraft)) + i = 0 + for ac in list_aircraft: + ac.sigma = np.arctan2(ac.XY[1]-ac.XYc[1], ac.XY[0]-ac.XYc[0]) + sigma[i] = ac.sigma + i = i + 1 + + inter_sigma = B.transpose().dot(sigma) + error_sigma = inter_sigma - ds + + 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 = -k*B.dot(error_sigma) + + print list_aircraft[0].time, " ", error_sigma[0], " ", error_sigma[1] + + i = 0 + for ac in list_aircraft: + msga = PprzMessage("ground", "DL_SETTING") + msga['ac_id'] = ac.id + msga['index'] = ac.a_index + msga['value'] = radius + u[i] + msgb = PprzMessage("ground", "DL_SETTING") + msgb['ac_id'] = ac.id + msgb['index'] = ac.b_index + msgb['value'] = radius + u[i] + + interface.send(msga) + interface.send(msgb) + + i = i + 1 + return + +def main(): + if len(sys.argv) != 6: + print "Usage: gvfFormationApp topology.txt desired_sigma.txt ids.txt radius k" + interface.shutdown() + return + + B = np.loadtxt(sys.argv[1]) + desired_sigmas = np.loadtxt(sys.argv[2]) + ids = np.loadtxt(sys.argv[3]) + radius = float(sys.argv[4]) + k = float(sys.argv[5]) + + global list_ids + list_ids = np.ndarray.tolist(ids) + map(int, list_ids) + + if np.size(ids) != np.size(B,0): + print "The ammount of aircrafts in the topology and ids does not match" + return + + for i in range(0, len(ids)): + list_aircraft.append(aircraft(int(ids[i]))) + + + # Ivy + interface.subscribe(message_recv) + + for ac in list_aircraft: + settings = PaparazziACSettings(ac.id) + list_of_indexes = ['a', 'b'] + + for setting_ in list_of_indexes: + try: + index = settings.name_lookup[setting_].index + if setting_ == 'a': + ac.a_index = index + if setting_ == 'b': + ac.b_index = index + except Exception as e: + print(e) + print(setting_ + " setting not found, \ + have you forgotten gvf.xml in your settings?") + + try: + while True: + time.sleep(0.5) + formation(B, desired_sigmas, radius, k) + + except KeyboardInterrupt: + interface.shutdown() + + +if __name__ == '__main__': + main()