diff --git a/sw/ground_segment/python/guided_mode_example.py b/sw/ground_segment/python/guided_mode_example.py index 1ba7abd34f..06e63ff931 100755 --- a/sw/ground_segment/python/guided_mode_example.py +++ b/sw/ground_segment/python/guided_mode_example.py @@ -12,224 +12,51 @@ PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abs sys.path.append(PPRZ_SRC + "/sw/lib/python") sys.path.append(PPRZ_HOME + "/var/lib/python") # pprzlink -from pprzlink.ivy import IvyMessagesInterface -from pprzlink.message import PprzMessage -from settings_xml_parse import PaparazziACSettings - -from math import radians -from time import sleep - - -class Guidance(object): - def __init__(self, ac_id, verbose=False, interface=None): - self.ac_id = ac_id - self.verbose = verbose - self._interface = interface - 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.") - if self._interface is None: - 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 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): - wait_step = 0.1 - timeout = 30 / wait_step # 30 seconds - new_answer = False - - def aircrafts_cb(ac_id, msg): - global new_answer - self.ac_list = [int(a) for a in msg['ac_list'].split(',') if a] - print("aircrafts: {}".format(self.ac_list)) - new_answer = True - - self._interface.send_request('ground', "AIRCRAFTS", aircrafts_cb) - # hack: sleep briefly to wait for answer - while not new_answer and timeout > 0: - sleep(wait_step) - timeout -= 1 - - if not new_answer: - print("WARNING: Getting the list of aircraft timed out. The results might be outdated.") - # Didn't raise an exception or return None in order to not break the API - - return self.ac_list - +from pprz_connect import PprzConnect, PprzConfig +from guided_mode import GuidedMode +from settings import PprzSettingsManager def main(): + from time import sleep + from math import radians import argparse parser = argparse.ArgumentParser(description="Guided mode example") - parser.add_argument("-i", "--ac_id", dest='ac_id', default=0, type=int, help="aircraft ID") + parser.add_argument("-i", "--ac_id", dest='ac_id', default=1, type=int, help="aircraft ID") args = parser.parse_args() - interface = None - if args.ac_id > 0: - ac_id = args.ac_id - else: - print("No aircraft ID specified, checking available aircrafts...") - interface = IvyMessagesInterface("guided mode example") - req = IvyRequester(interface) - # hack: sleep briefly so that connections can be established - sleep(0.1) - aircrafts = req.get_aircrafts() - if not aircrafts: - print("No active aircrafts found, aborting...") - sys.exit(1) - elif len(aircrafts) == 1: - ac_id = aircrafts[0] - else: - print("multiple aircrafts found: {}".format(aircrafts)) - print("please specify one on the commandline...") - sys.exit(1) - try: - g = Guidance(ac_id, interface=interface) + connect = PprzConnect() + sleep(2) + print(connect._conf_list_by_id) + conf = connect.conf_by_id(str(args.ac_id)) + settings = PprzSettingsManager(conf.settings, conf.id, connect.ivy) + guided = GuidedMode(connect.ivy) + + settings['auto2'] = 'Guided' + sleep(0.1) - g.set_guided_mode() - sleep(0.2) - g.goto_ned(north=2.0, east=2.0, down=-3.0, heading=radians(90)) - sleep(10) - g.goto_ned_relative(north=-2.0, east=-2.0, down=1.0, yaw=-radians(45)) - sleep(10) - g.goto_body_relative(forward=0.0, right=1.0, down=0.0) - sleep(10) - g.move_at_ned_vel(north=0.5) - sleep(3) - g.move_at_body_vel(forward=-0.5) - sleep(3) - g.set_nav_mode() - sleep(0.2) + print("Goto NED 2, 2, -3, 90°") + guided.goto_ned(conf.id, north=2.0, east=2.0, down=-3.0, heading=radians(90)) + sleep(5) + print("Goto NED relative -2, -2, 1, -45°") + guided.goto_ned_relative(conf.id, north=-2.0, east=-2.0, down=1.0, yaw=-radians(45)) + sleep(5) + print("Goto body 0, 1, 0") + guided.goto_body_relative(conf.id, forward=0.0, right=1.0, down=0.0) + sleep(5) + print("Move NED 0.5m/s north") + guided.move_at_ned_vel(conf.id, north=0.5) + sleep(5) + print("Move body -0.5m/s forward") + guided.move_at_body_vel(conf.id, forward=-0.5) + sleep(2) + + settings['auto2'] = 'Nav' + except KeyboardInterrupt: print("Stopping on request") - g.shutdown() + finally: + connect.shutdown() if __name__ == '__main__': diff --git a/sw/lib/python/guided_mode.py b/sw/lib/python/guided_mode.py new file mode 100755 index 0000000000..8be08f7b0f --- /dev/null +++ b/sw/lib/python/guided_mode.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python3 +# +# Copyright (C) 2025 Gautier Hattenberger +# +# 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 +# . +# + +import sys +from os import path, getenv + +# if PAPARAZZI_SRC or PAPARAZZI_HOME not set, then assume the tree containing this +# file is a reasonable substitute +PPRZ_HOME = getenv("PAPARAZZI_HOME", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../'))) +sys.path.append(PPRZ_HOME + "/var/lib/python") # pprzlink + +from pprzlink.ivy import IvyMessagesInterface +from pprzlink.message import PprzMessage + + +class GuidedMode(object): + ''' + a collection of commonly used functions in guided mode + ''' + FLAG_ABS_POS = 0x00 + FLAG_XY_OFFSET = 0x01 + FLAG_XY_BODY = 0x02 + FLAG_Z_OFFSET = 0x04 + FLAG_YAW_OFFSET = 0x08 + FLAG_XY_VEL = 0x20 + FLAG_Z_VEL = 0x40 + FLAG_YAW_VEL = 0x80 + + def __init__(self, interface=None): + self._interface = interface + if self._interface is None: + self._interface = IvyMessagesInterface("guided mode") + + def shutdown(self): + if self._interface is not None: + self._interface.shutdown() + self._interface = None + + def __del__(self): + self.shutdown() + + def guided_cmd(self, ac_id: int, flag: int, x: float, y: float, z: float, yaw: float): + ''' + generic guided mode function + messages are sent in RAW_DATALINK so that the server can log it + ''' + msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED") + msg['ac_id'] = ac_id + msg['flags'] = flag + msg['x'] = x + msg['y'] = y + msg['z'] = z + msg['yaw'] = yaw + self._interface.send_raw_datalink(msg) + + def goto_ned(self, ac_id, north, east, down, heading=0.0): + """ + goto a local NorthEastDown position in meters (if already in GUIDED mode) + """ + self.guided_cmd(ac_id, self.FLAG_ABS_POS, north, east, down, heading) + + def goto_enu(self, ac_id, east, north, up, heading=0.0): + """ + goto a local EastNorthUp position in meters (if already in GUIDED mode) + """ + self.guided_cmd(ac_id, 0x0, north, east, -up, heading) + + def goto_ned_relative(self, ac_id, north, east, down, yaw=0.0): + """ + goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode) + """ + self.guided_cmd(ac_id, self.FLAG_XY_OFFSET | self.FLAG_Z_OFFSET | self.FLAG_YAW_OFFSET, north, east, down, yaw) + + def goto_enu_relative(self, ac_id, east, north, up, yaw=0.0): + """ + goto a local EastNorthUp position relative to current position in meters (if already in GUIDED mode) + """ + self.guided_cmd(ac_id, self.FLAG_XY_OFFSET | self.FLAG_Z_OFFSET | self.FLAG_YAW_OFFSET, north, east, -up, yaw) + + def goto_body_relative(self, ac_id, forward, right, down, yaw=0.0): + """ + goto to a position relative to current position and heading in meters (if already in GUIDED mode) + """ + self.guided_cmd(ac_id, self.FLAG_XY_BODY | self.FLAG_Z_OFFSET | self.FLAG_YAW_OFFSET, forward, right, down, yaw) + + def move_at_ned_vel(self, ac_id, north=0.0, east=0.0, down=0.0, yaw=0.0): + """ + move at specified NorthEastDown velocity in meters/sec with absolute heading (if already in GUIDED mode) + """ + self.guided_cmd(ac_id, self.FLAG_XY_VEL | self.FLAG_Z_VEL, north, east, down, yaw) + + def move_at_enu_vel(self, ac_id, east=0.0, north=0.0, up=0.0, yaw=0.0): + """ + move at specified EastNorthUp velocity in meters/sec with absolute heading (if already in GUIDED mode) + """ + self.guided_cmd(ac_id, self.FLAG_XY_VEL | self.FLAG_Z_VEL, north, east, -up, yaw) + + def move_at_body_vel(self, ac_id, 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) + """ + self.guided_cmd(ac_id, self.FLAG_XY_BODY | self.FLAG_XY_VEL | self.FLAG_Z_VEL, forward, right, down, yaw) + + def move_at_body_vel_yaw_rate(self, ac_id, forward=0.0, right=0.0, down=0.0, yaw_rate=0.0): + """ + move at specified velocity in meters/sec with yaw rate (if already in GUIDED mode) + """ + self.guided_cmd(ac_id, self.FLAG_XY_BODY | self.FLAG_XY_VEL | self.FLAG_Z_VEL | self.FLAG_YAW_VEL, forward, right, down, yaw_rate) + + + +def main(): + from math import radians + from time import sleep + import argparse + + parser = argparse.ArgumentParser(description="Guided mode example") + parser.add_argument("-i", "--ac_id", dest='ac_id', default=1, type=int, help="aircraft ID") + args = parser.parse_args() + + print(f"Starting guided mode example for A/C id '{args.ac_id}'") + g = None + try: + g = GuidedMode() + sleep(0.1) + print("goto ned") + g.goto_ned(args.ac_id, north=2.0, east=2.0, down=-3.0, heading=radians(90)) + sleep(5) + print("goto ned relative") + g.goto_ned_relative(args.ac_id, north=-2.0, east=-2.0, down=1.0, yaw=-radians(45)) + sleep(5) + print("goto body relative") + g.goto_body_relative(args.ac_id, forward=0.0, right=1.0, down=0.0) + sleep(5) + print("move at ned velocity") + g.move_at_ned_vel(args.ac_id, north=0.5) + sleep(5) + print("move at body frame velocity") + g.move_at_body_vel(args.ac_id, forward=-0.5) + sleep(5) + print("stop moving") + g.move_at_ned_vel(args.ac_id) + sleep(1) + except KeyboardInterrupt: + print("Stopping on request") + except Exception as e: + print("Failing with error:", e) + finally: + if g is not None: + print("[guided] Shutting down ivy interface...") + g.shutdown() + + +if __name__ == '__main__': + main() diff --git a/sw/lib/python/settings.py b/sw/lib/python/settings.py index df57520b90..751e1213c1 100755 --- a/sw/lib/python/settings.py +++ b/sw/lib/python/settings.py @@ -50,12 +50,16 @@ class PprzSettingsManager: def __getitem__(self, item): return self.settings_grp.__getitem__(item) - def __setitem__(self, key, value): - setting = self.settings_grp[key] + def __setitem__(self, key, value: int|float|str): + setting: PprzSetting = self.settings_grp[key] msg = PprzMessage("ground", "DL_SETTING") msg['ac_id'] = self.ac_id msg['index'] = setting.index - msg['value'] = value + if isinstance(value, str): + msg['value'] = setting.value_from_name(value) + elif isinstance(value, float) or isinstance(value, int): + msg['value'] = value + self.ivy.send(msg) def __len__(self): @@ -141,7 +145,8 @@ class PprzSetting: self.xml = xml def __str__(self): - return "{{var: {}, shortname: {}, index: {}}}".format(self.var, self.shortname, self.index) + values = f", values:{'|'.join(self.values)}" if self.values is not None else "" + return f"{{var: {self.var}, shortname: {self.shortname}, index: {self.index}{values}}}" def value_from_name(self, name): """Return the index in 'values' table matching a given name. Raise ValueError if the name is unknown."""