mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[Python libs] Start python settings and flight plan libs. (#2592)
* [Python libs] Start python settings and flight plan libs. * [Python libs] Robustify XML attribute getters. * [Python libs] Adds WP number, improve getters. * [Python libs] Adds license headers.
This commit is contained in:
Fabien-B
GitHub
Executable
+174
@@ -0,0 +1,174 @@
|
||||
#!/usr/bin/env python3
|
||||
#
|
||||
# Copyright (C) 2020 Fabien Bonneval <fabien.bonneval@enac.fr>
|
||||
#
|
||||
# This file is part of paparazzi.
|
||||
#
|
||||
# paparazzi is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 2, or (at your option)
|
||||
# any later version.
|
||||
#
|
||||
# paparazzi is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with paparazzi; see the file COPYING. If not, see
|
||||
# <http://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
|
||||
from lxml import etree
|
||||
from typing import List, Union
|
||||
import sys
|
||||
from xml_utils import get_attrib, get_attrib_default
|
||||
|
||||
|
||||
class FlightPlan:
|
||||
|
||||
def __init__(self):
|
||||
self.header = "" # type: str
|
||||
self.waypoints = [] # type: List[Waypoint]
|
||||
self.exceptions = [] # type: List[Exc]
|
||||
self.blocks = [] # type: List[Block]
|
||||
self.name = None
|
||||
self.lat0 = None
|
||||
self.lon0 = None
|
||||
self.max_dist_from_home = None
|
||||
self.ground_alt = None
|
||||
self.security_height = None
|
||||
self.alt = None
|
||||
|
||||
@staticmethod
|
||||
def parse(fp_xml):
|
||||
fp = FlightPlan()
|
||||
fp_tree = etree.parse(fp_xml)
|
||||
fp_element = fp_tree.find("flight_plan")
|
||||
|
||||
fp.name = get_attrib(fp_element, "name")
|
||||
fp.lat0 = get_attrib(fp_element, "lat0")
|
||||
fp.lon0 = get_attrib(fp_element, "lon0")
|
||||
fp.max_dist_from_home = get_attrib(fp_element, "max_dist_from_home")
|
||||
fp.ground_alt = get_attrib(fp_element, "ground_alt")
|
||||
fp.security_height = get_attrib(fp_element, "security_height")
|
||||
fp.alt = get_attrib(fp_element, "alt")
|
||||
|
||||
if fp_element.find("header") is not None:
|
||||
fp.header = fp_element.find("header").text
|
||||
|
||||
ways_elt = fp_element.find("waypoints")
|
||||
fp.waypoints = FlightPlan.parse_waypoints(ways_elt)
|
||||
|
||||
blocks_elt = fp_element.find("blocks")
|
||||
fp.blocks = FlightPlan.parse_blocks(blocks_elt)
|
||||
|
||||
excs_elt = fp_element.find("exceptions")
|
||||
fp.exceptions = FlightPlan.parse_exceptions(excs_elt)
|
||||
return fp
|
||||
|
||||
@staticmethod
|
||||
def parse_waypoints(ways_elt: etree.Element):
|
||||
waypoints = []
|
||||
w_no = 1 # first waypoint number is 1.
|
||||
for way_e in ways_elt.findall("waypoint"):
|
||||
name = get_attrib(way_e, "name")
|
||||
x = get_attrib_default(way_e, "x", None, float)
|
||||
y = get_attrib_default(way_e, "y", None, float)
|
||||
lat = get_attrib_default(way_e, "lat", None)
|
||||
lon = get_attrib_default(way_e, "lon", None)
|
||||
alt = get_attrib_default(way_e, "alt", None, float)
|
||||
height = get_attrib_default(way_e, "height", None, float)
|
||||
waypoint = Waypoint(name, x, y, lat, lon, alt, height, w_no)
|
||||
waypoints.append(waypoint)
|
||||
w_no += 1
|
||||
return waypoints
|
||||
|
||||
@staticmethod
|
||||
def parse_blocks(blocks_elt):
|
||||
blocks = []
|
||||
for b_e in blocks_elt.findall("block"):
|
||||
name = get_attrib(b_e, "name")
|
||||
no = get_attrib(b_e, "no")
|
||||
block = Block(name, no, b_e)
|
||||
blocks.append(block)
|
||||
return blocks
|
||||
|
||||
@staticmethod
|
||||
def parse_exceptions(exs_elt):
|
||||
if exs_elt is None:
|
||||
return []
|
||||
excs = []
|
||||
for ex_e in exs_elt.findall("exception"):
|
||||
cond = get_attrib(ex_e, "cond")
|
||||
deroute = get_attrib(ex_e, "deroute")
|
||||
exc = Exc(cond, deroute)
|
||||
excs.append(exc)
|
||||
return excs
|
||||
|
||||
def get_waypoint(self, key: Union[str, int]):
|
||||
"""
|
||||
:param key: Waypoint name or number
|
||||
:type key: str or int
|
||||
"""
|
||||
if type(key) == str:
|
||||
for wp in self.waypoints:
|
||||
if wp.name == key:
|
||||
return wp
|
||||
elif type(key) == int:
|
||||
for wp in self.waypoints:
|
||||
if wp.no == key:
|
||||
return wp
|
||||
|
||||
def get_block(self, key: Union[str, int]):
|
||||
"""
|
||||
:param key: Block name or number
|
||||
:type key: str or int
|
||||
"""
|
||||
if type(key) == str:
|
||||
for block in self.blocks:
|
||||
if block.name == key:
|
||||
return block
|
||||
elif type(key) == int:
|
||||
for block in self.blocks:
|
||||
if block.no == key:
|
||||
return block
|
||||
|
||||
def get_block_groups(self):
|
||||
return list(set(filter(lambda x: x is not None,
|
||||
[get_attrib_default(block.xml, "group", None) for block in self.blocks])))
|
||||
|
||||
def get_blocks_from_group(self, groupname):
|
||||
return list(filter(lambda block: get_attrib_default(block.xml, "group", None) == groupname, self.blocks))
|
||||
|
||||
|
||||
class Waypoint:
|
||||
def __init__(self, name, x, y, lat, lon, alt, height, no):
|
||||
self.name = name
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.lat = lat
|
||||
self.lon = lon
|
||||
self.alt = alt
|
||||
self.height = height
|
||||
self.no = no
|
||||
|
||||
|
||||
class Block:
|
||||
def __init__(self, name, no, xml):
|
||||
self.name = name
|
||||
self.no = no
|
||||
self.xml = xml
|
||||
|
||||
|
||||
class Exc:
|
||||
def __init__(self, cond, deroute):
|
||||
self.cond = cond
|
||||
self.deroute = deroute
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
flight_plan = FlightPlan.parse(sys.argv[1])
|
||||
for b in flight_plan.blocks:
|
||||
print(b.name)
|
||||
Executable
+238
@@ -0,0 +1,238 @@
|
||||
#!/usr/bin/env python3
|
||||
#
|
||||
# Copyright (C) 2020 Fabien Bonneval <fabien.bonneval@enac.fr>
|
||||
#
|
||||
# This file is part of paparazzi.
|
||||
#
|
||||
# paparazzi is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 2, or (at your option)
|
||||
# any later version.
|
||||
#
|
||||
# paparazzi is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with paparazzi; see the file COPYING. If not, see
|
||||
# <http://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
from lxml import etree
|
||||
from typing import List
|
||||
import sys
|
||||
from os import path, getenv
|
||||
import time
|
||||
from xml_utils import get_attrib, get_attrib_default
|
||||
|
||||
PPRZ_HOME = getenv("PAPARAZZI_HOME", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../')))
|
||||
sys.path.append(PPRZ_HOME + "/var/lib/python")
|
||||
from pprzlink.ivy import IvyMessagesInterface
|
||||
from pprzlink.message import PprzMessage
|
||||
|
||||
|
||||
class PprzSettingsManager:
|
||||
def __init__(self, settings_xml_path, ac_id, ivy: IvyMessagesInterface):
|
||||
self.ac_id = ac_id
|
||||
self.ivy = ivy
|
||||
self.settings_grp = PprzSettingsParser.parse(settings_xml_path)
|
||||
self.ivy.subscribe(self.update_values, PprzMessage('ground', 'DL_VALUES'))
|
||||
|
||||
def update_values(self, sender, msg):
|
||||
if self.ac_id == msg.ac_id:
|
||||
for i, value in enumerate(msg.values):
|
||||
if value != '?':
|
||||
value = float(value)
|
||||
s = self.settings_grp[i] # type: PprzSetting
|
||||
s.set(value)
|
||||
|
||||
def __getitem__(self, item):
|
||||
return self.settings_grp.__getitem__(item)
|
||||
|
||||
def __setitem__(self, key, value):
|
||||
setting = self.settings_grp[key]
|
||||
msg = PprzMessage("ground", "DL_SETTING")
|
||||
msg['ac_id'] = self.ac_id
|
||||
msg['index'] = setting.index
|
||||
msg['value'] = value
|
||||
self.ivy.send(msg)
|
||||
|
||||
def __len__(self):
|
||||
return self.settings_grp.__len__()
|
||||
|
||||
|
||||
class PprzSettingsParser:
|
||||
def __init__(self):
|
||||
self.setting_nb = 0
|
||||
|
||||
@staticmethod
|
||||
def parse(settings_xml_path):
|
||||
parser = PprzSettingsParser()
|
||||
settings_tree = etree.parse(settings_xml_path)
|
||||
sets_element = settings_tree.find("dl_settings")
|
||||
return parser.parse_dl_settings(sets_element)
|
||||
|
||||
def parse_dl_setting(self, element):
|
||||
var = get_attrib(element, "var")
|
||||
min_v = get_attrib(element, "min", typ=float)
|
||||
max_v = get_attrib(element, "max", typ=float)
|
||||
shortname = get_attrib_default(element, "shortname", var)
|
||||
step = get_attrib_default(element, "step", 1, typ=float)
|
||||
values = get_attrib_default(element, "values", None)
|
||||
|
||||
if values is None:
|
||||
values = []
|
||||
else:
|
||||
values = values.split("|")
|
||||
count = int((max_v - min_v + step) / step)
|
||||
if len(values) != count:
|
||||
print("Warning: possibly wrong number of values ({}) for {} (expected {})"
|
||||
.format(len(values), shortname, count))
|
||||
setting = PprzSetting(var, self.setting_nb, shortname, min_v, max_v, step, values, element)
|
||||
|
||||
for e_button in element.findall("strip_button"):
|
||||
name = get_attrib(e_button, "name")
|
||||
value = get_attrib(e_button, "value")
|
||||
icon = get_attrib_default(e_button, "icon", None)
|
||||
group = get_attrib_default(e_button, "group", None)
|
||||
|
||||
button = StripButton(value, name, icon, group)
|
||||
setting.buttons.append(button)
|
||||
|
||||
for e_key in element.findall("key_press"):
|
||||
key = get_attrib(e_key, "key")
|
||||
value = get_attrib(e_key, "value")
|
||||
key_press = KeyPress(key, value)
|
||||
setting.key_press.append(key_press)
|
||||
|
||||
self.setting_nb += 1
|
||||
return setting
|
||||
|
||||
def parse_dl_settings(self, element):
|
||||
name = get_attrib_default(element, "name", None)
|
||||
group = PprzSettingsGrp(name)
|
||||
for child in element.getchildren():
|
||||
if child.tag == "dl_settings":
|
||||
setting_group = self.parse_dl_settings(child)
|
||||
group.groups_list.append(setting_group)
|
||||
elif child.tag == "dl_setting":
|
||||
setting = self.parse_dl_setting(child)
|
||||
group.settings_list.append(setting)
|
||||
else:
|
||||
raise Exception("tag {} not supported!".format(child.tag))
|
||||
return group
|
||||
|
||||
|
||||
class PprzSetting:
|
||||
"""Paparazzi Setting Class"""
|
||||
|
||||
def __init__(self, var, index, shortname, min_value, max_value, step, values, xml):
|
||||
self.var = var # type: str
|
||||
self.index = index # type: int
|
||||
self.shortname = shortname # type: str
|
||||
self.min_value = min_value # type: float
|
||||
self.max_value = max_value # type: float
|
||||
self.step = step # type: float
|
||||
self.values = values # type: List[str]
|
||||
self.value = None # type: float
|
||||
self.buttons = [] # type: List[StripButton]
|
||||
self.key_press = [] # type: List[KeyPress]
|
||||
self.xml = xml
|
||||
|
||||
def __str__(self):
|
||||
return "{{var: {}, shortname: {}, index: {}}}".format(self.var, self.shortname, self.index)
|
||||
|
||||
def value_from_name(self, name):
|
||||
"""Return the index in 'values' table matching a given name. Raise ValueError if the name is unknown."""
|
||||
if self.values is None:
|
||||
raise ValueError("No named values in this setting")
|
||||
return self.values.index(name) + self.min_value
|
||||
|
||||
def set(self, val):
|
||||
if isinstance(val, str):
|
||||
self.value = self.value_from_name(val)
|
||||
elif isinstance(val, float) or isinstance(val, int):
|
||||
self.value = val
|
||||
else:
|
||||
raise Exception("Bad type!")
|
||||
|
||||
|
||||
class StripButton:
|
||||
def __init__(self, value, name, icon, group):
|
||||
self.value = value
|
||||
self.name = name
|
||||
self.icon = icon
|
||||
self.group = group
|
||||
|
||||
|
||||
class KeyPress:
|
||||
def __init__(self, key, value):
|
||||
self.value = value
|
||||
self.key = key
|
||||
|
||||
|
||||
class PprzSettingsGrp:
|
||||
""""Paparazzi Setting Group Class"""
|
||||
|
||||
def __init__(self, name):
|
||||
self.name = name
|
||||
self.settings_list = [] # type: List[PprzSetting]
|
||||
self.groups_list = [] # type: List[PprzSettingsGrp]
|
||||
|
||||
def __str__(self):
|
||||
grp_names = [grp.name for grp in self.groups_list]
|
||||
settings = [str(setting) for setting in self.settings_list]
|
||||
return ", ".join(grp_names + settings)
|
||||
|
||||
def get_all_settings(self):
|
||||
all_settings = []
|
||||
all_settings += self.settings_list
|
||||
for group in self.groups_list:
|
||||
all_settings += group.get_all_settings()
|
||||
return sorted(all_settings, key=lambda s: s.index)
|
||||
|
||||
def __getitem__(self, item):
|
||||
if type(item) == int:
|
||||
for setting in self.get_all_settings():
|
||||
if item == setting.index:
|
||||
return setting
|
||||
raise IndexError("No setting #{}".format(item))
|
||||
elif type(item) == str:
|
||||
for setting in self.get_all_settings():
|
||||
if item == setting.shortname:
|
||||
return setting
|
||||
raise AttributeError('No such setting named "{}"'.format(item))
|
||||
|
||||
def __len__(self):
|
||||
return len(self.get_all_settings())
|
||||
|
||||
|
||||
def example1():
|
||||
# Usage: ./setting.py <path_var_AC>/settings.xml
|
||||
settings = PprzSettingsParser.parse(sys.argv[1])
|
||||
set_nav_radius = settings["nav_radius"]
|
||||
print(set_nav_radius)
|
||||
set_no_2 = settings[2]
|
||||
print(set_no_2)
|
||||
|
||||
|
||||
def example2():
|
||||
# Usage: ./setting.py <path_var_AC>/settings.xml <ac_id>
|
||||
ivy = IvyMessagesInterface("DemoSettings")
|
||||
try:
|
||||
setting_manager = PprzSettingsManager(sys.argv[1], sys.argv[2], ivy)
|
||||
while True:
|
||||
time.sleep(1)
|
||||
setting_manager["altitude"] = setting_manager["altitude"].value + 2
|
||||
except KeyboardInterrupt:
|
||||
ivy.shutdown()
|
||||
print("Stopping on request")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# example1 usage: ./setting.py <path_var_AC>/settings.xml
|
||||
example1()
|
||||
|
||||
# example2 usage: ./setting.py <path_var_AC>/settings.xml <ac_id>
|
||||
# example2()
|
||||
@@ -0,0 +1,47 @@
|
||||
#!/usr/bin/env python3
|
||||
#
|
||||
# Copyright (C) 2020 Fabien Bonneval <fabien.bonneval@enac.fr>
|
||||
#
|
||||
# This file is part of paparazzi.
|
||||
#
|
||||
# paparazzi is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 2, or (at your option)
|
||||
# any later version.
|
||||
#
|
||||
# paparazzi is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with paparazzi; see the file COPYING. If not, see
|
||||
# <http://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
from lxml import etree
|
||||
|
||||
|
||||
def get_attrib(node: etree.Element, tag: str, typ=None):
|
||||
"""XML Element attribute getter which is insensitive to case, handle default value and type conversion"""
|
||||
try:
|
||||
value = node.attrib[tag.lower()]
|
||||
except KeyError:
|
||||
value = node.attrib[tag.upper()]
|
||||
|
||||
if typ is None:
|
||||
return value
|
||||
else:
|
||||
return typ(value)
|
||||
|
||||
|
||||
def get_attrib_default(node: etree.Element, tag: str, default, typ=None):
|
||||
try:
|
||||
value = get_attrib(node, tag, typ=typ)
|
||||
except KeyError:
|
||||
value = default
|
||||
|
||||
if typ is not None and default is not None:
|
||||
return typ(value)
|
||||
else:
|
||||
return value
|
||||
Reference in New Issue
Block a user