[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.

sw/lib/python/flight_plan.py

@@ -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)

sw/lib/python/settings.py

@@ -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()

sw/lib/python/xml_utils.py

@@ -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