diff --git a/.idea/workspace.xml b/.idea/workspace.xml index 0ddf6b10e..e0f9c6389 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -5,23 +5,23 @@ - - - - - + + - - - - + + + @@ -44,25 +44,61 @@ - { + "keyToString": { + "Python.dgfs.executor": "Run", + "RunOnceActivity.ShowReadmeOnStart": "true", + "git-widget-placeholder": "master", + "last_opened_file_path": "/home/jonathan/catkin_ws/src/soccerbot/tools/setup", + "node.js.detected.package.eslint": "true", + "node.js.detected.package.tslint": "true", + "node.js.selected.package.eslint": "(autodetect)", + "node.js.selected.package.tslint": "(autodetect)", + "nodejs_package_manager_path": "npm", + "settings.editor.selected.configurable": "com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable", + "vue.rearranger.settings.migration": "true" } -}]]> +} + + + + + + + + + + + + + + + - @@ -77,10 +113,25 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 8944ebcf2..ca9c96843 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,7 +4,7 @@ x-exclude: &exclude repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.5.0 + rev: v5.0.0 hooks: - id: check-symlinks <<: *exclude @@ -18,7 +18,7 @@ repos: - id: prettier <<: *exclude - repo: https://github.com/psf/black - rev: 24.3.0 + rev: 22.3.0 hooks: - id: black <<: *exclude @@ -34,7 +34,7 @@ repos: - black <<: *exclude - repo: https://github.com/PyCQA/flake8 - rev: 7.0.0 + rev: 7.1.1 hooks: - id: flake8 <<: *exclude diff --git a/soccer_strategy/src/soccer_strategy/__init__.py b/soccer_strategy/src/soccer_strategy/__init__.py index e69de29bb..554a0076c 100644 --- a/soccer_strategy/src/soccer_strategy/__init__.py +++ b/soccer_strategy/src/soccer_strategy/__init__.py @@ -0,0 +1 @@ +from soccer_strategy import * diff --git a/soccer_strategy/src/soccer_strategy/autopilot_context_ros.py b/soccer_strategy/src/soccer_strategy/autopilot_context_ros.py new file mode 100644 index 000000000..32e901ab3 --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/autopilot_context_ros.py @@ -0,0 +1,98 @@ +import rospy +from std_srvs.srv import Empty, EmptyRequest, EmptyResponse + + +class AutoPilotContextRos(AutoPilotContext): + def __init__(self, behavior: BehaviorContextRos) -> None: + """ + Usually, the AutoPilotContext accepts an autopilot through the constructor, but + also provides a setter to change it at runtime. + """ + super(AutoPilotContextRos, self).__init__(AutoPilotActionsRos(behavior)) + + self._srv_hover = rospy.Service("evtol_behavior/test/hover", Empty, self.__callback_hover) + self._srv_hover_kill = rospy.Service("evtol_behavior/test/hover_kill", Empty, self.__callback_hover_kill) + self._srv_hover_uwb = rospy.Service("evtol_behavior/test/hover_uwb", Empty, self.__callback_hover_uwb) + self._srv_alt = rospy.Service("evtol_behavior/test/altitude", Empty, self.__callback_altitude) + self._srv_square_hover = rospy.Service("evtol_behavior/test/square_hover", Empty, self.__callback_square_hover) + self._srv_square_traj = rospy.Service("evtol_behavior/test/square_traj", Empty, self.__callback_square_traj) + self._srv_circle_traj = rospy.Service("evtol_behavior/test/circle_traj", Empty, self.__callback_circle_traj) + self._srv_lemniscate_traj = rospy.Service("evtol_behavior/test/lemniscate_traj", Empty, self.__callback_lemniscate_traj) + + # TODO can i add the same trick i used in behaviorcontextros + + def __callback_hover(self, request: EmptyRequest) -> EmptyResponse: + """ + This function handles the hover test service. + """ + rospy.loginfo("hover") + + self.autopilot = Hover(self.action) + self.autopilot.inprogress = True + + return EmptyResponse() + + def __callback_hover_kill(self, request: EmptyRequest) -> EmptyResponse: + """ + This function handles the hover test service. + """ + rospy.loginfo("hover kill") + + self.autopilot = HoverKill(self.action) + self.autopilot.inprogress = True + + return EmptyResponse() + + def __callback_hover_uwb(self, request: EmptyRequest) -> EmptyResponse: + """ + This function handles the hover test service. + """ + rospy.loginfo("hover uwb") + + self.autopilot = HoverUwb(self.action) + self.autopilot.inprogress = True + + return EmptyResponse() + + def __callback_altitude(self, request: EmptyRequest) -> EmptyResponse: + """ + This function handles the arm service, which triggers the arming action of the evtol. + """ + + self.autopilot = Altitude(self.action) + self.autopilot.inprogress = True + return EmptyResponse() + + def __callback_square_hover(self, request: EmptyRequest) -> EmptyResponse: + """ + This function handles the arm service, which triggers the arming action of the evtol. + """ + + self.autopilot = SquareHover(self.action) + self.autopilot.inprogress = True + return EmptyResponse() + + def __callback_square_traj(self, request: EmptyRequest) -> EmptyResponse: + """ + This function handles the arm service, which triggers the arming action of the evtol. + """ + + self.autopilot = Trajectory(self.action, "square") + self.autopilot.inprogress = True + return EmptyResponse() + + def __callback_circle_traj(self, request: EmptyRequest) -> EmptyResponse: + """ + This function handles the arm service, which triggers the arming action of the evtol. + """ + self.autopilot = Trajectory(self.action, "circle") + self.autopilot.inprogress = True + return EmptyResponse() + + def __callback_lemniscate_traj(self, request: EmptyRequest) -> EmptyResponse: + """ + This function handles the arm service, which triggers the arming action of the evtol. + """ + self.autopilot = Trajectory(self.action, "lemniscate") + self.autopilot.inprogress = True + return EmptyResponse() diff --git a/soccer_strategy/src/soccer_strategy/behavior/__init__.py b/soccer_strategy/src/soccer_strategy/behavior/__init__.py new file mode 100644 index 000000000..9caf2d9c9 --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/behavior/__init__.py @@ -0,0 +1 @@ +from soccer_strategy.behavior.behavior import Behavior diff --git a/soccer_strategy/src/soccer_strategy/behavior/behavior.py b/soccer_strategy/src/soccer_strategy/behavior/behavior.py new file mode 100644 index 000000000..f38ccd0d5 --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/behavior/behavior.py @@ -0,0 +1,43 @@ +from __future__ import annotations + +from abc import ABC, abstractmethod + + +class Behavior(ABC): + """ + The action a state should do like Arm, TakeOff. All state specific code + + It contains a reference to an instance of a BehaviorContext class + This reference allows for each behavior to switch to another behavior. + """ + + @property + def context(self): + # link to transition state + return self._context + + @context.setter + def context(self, context) -> None: + self._context = context + + @abstractmethod + def action(self) -> None: # TODO need to rethink this action + # Updating drone status and performing actions for that state + pass + + @abstractmethod + def run_algorithim(self) -> None: + # Condition to check based on drone state + pass + + @abstractmethod + def ready_to_switch_to(self) -> bool: + return True + + @property + def state(self) -> Behavior: + return self._context.state # type: ignore[no-any-return] + + @state.setter + def state(self, state) -> None: + self._context.state = state diff --git a/soccer_strategy/src/soccer_strategy/behavior/behavior_context.py b/soccer_strategy/src/soccer_strategy/behavior/behavior_context.py new file mode 100644 index 000000000..7949d518a --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/behavior/behavior_context.py @@ -0,0 +1,54 @@ +from __future__ import annotations + +from soccer_strategy.behavior import Behavior +from soccer_strategy.behavior.state.balance import Balance + + +class BehaviorContext: + """ + Interface to state and switching states. All code to access the state and how they switch + + It contains a reference to an instance of a Behavior subclass, which represents the current + state. + """ + + _state = None + """ + A reference to the current state of the BehaviorContext. + """ + + def __init__(self, sim: bool = True) -> None: + self.sim = sim # TODO clean up + self.transition_to(Balance()) # Has to be last. setting context for current state + + @property + def state(self) -> Behavior: + return self._state # type: ignore[return-value] + + @state.setter + def state(self, state) -> None: + self.transition_to(state) + + def transition_to(self, state) -> None: + """ + The BehaviorContext allows changing the State object at runtime. + """ + state.context = self # Why? + + if state.ready_to_switch_to(): # TODO needed? + print(f"BehaviorContext: Transition to {type(state).__name__}") + self._state = state + self._state.context = self + self.state_action() # TODO is this required + + """ + The BehaviorContext delegates part of its behavior to the current State object. + """ + + def state_action(self) -> None: + self._state.action() # type: ignore[union-attr] + + def run_state_algorithim(self) -> None: + self._state.run_algorithim() # type: ignore[union-attr] + + # TODO could have a fallen function diff --git a/soccer_strategy/src/soccer_strategy/behavior/state/__init__.py b/soccer_strategy/src/soccer_strategy/behavior/state/__init__.py new file mode 100644 index 000000000..5710490d2 --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/behavior/state/__init__.py @@ -0,0 +1,2 @@ +# TODO setup proper imports +from soccer_strategy.behavior.state import * diff --git a/soccer_strategy/src/soccer_strategy/behavior/state/balance.py b/soccer_strategy/src/soccer_strategy/behavior/state/balance.py new file mode 100644 index 000000000..fc0f142e2 --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/behavior/state/balance.py @@ -0,0 +1,12 @@ +from soccer_strategy.behavior import Behavior + + +class Balance(Behavior): + def action(self) -> None: + pass + + def run_algorithim(self) -> None: + pass + + def ready_to_switch_to(self) -> bool: + return True diff --git a/soccer_strategy/src/soccer_strategy/behavior_context_ros.py b/soccer_strategy/src/soccer_strategy/behavior_context_ros.py new file mode 100644 index 000000000..e605db133 --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/behavior_context_ros.py @@ -0,0 +1,53 @@ +from __future__ import annotations + +import rospy +from mavros_msgs.srv import SetMode, SetModeRequest +from std_srvs.srv import Empty, EmptyRequest, EmptyResponse + +from soccer_strategy.behavior.behavior_context import BehaviorContext + + +class BehaviorContextRos(BehaviorContext): + + _state = None + """ + A reference to the current state of the BehaviorContext. + """ + + def __init__(self, drone: DroneRos, sim: bool) -> None: + self.state_list = ["land", "rtl", "takeoff"] + # TODO decouple drone from behavior can pass from executive + self.drone = drone + self.sim = sim + self.path = PathStatusRos() + + self.transition_to(DisarmRos()) + # TODO add safe guards around manual safe switching + # Services + self._srv_arm = rospy.Service("evtol_behavior/arm", Empty, self.__callback_arm) + + """ + The BehaviorContext delegates part of its behavior to the current State object. + """ + + def state_action(self) -> None: + super(BehaviorContextRos, self).state_action() + + state_name = self._state.__class__.__name__.lower() + # TODO add a success condition + + if state_name in self.state_list: + rospy.wait_for_service("/evtol_nav/" + state_name) + rospy.ServiceProxy("evtol_nav/" + state_name, Empty).call(EmptyRequest()) + + @staticmethod + def check_disarm(next_state): + return type(next_state) == Disarm or type(next_state) == DisarmRos + + def __callback_arm(self, request: EmptyRequest) -> EmptyResponse: + """ + This function handles the arm service, which triggers the arming action of the evtol. + """ + self.transition_to_arm() + + return EmptyResponse() diff --git a/soccer_strategy/src/soccer_strategy/behavior_executive.py b/soccer_strategy/src/soccer_strategy/behavior_executive.py new file mode 100644 index 000000000..9d5bb6ea9 --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/behavior_executive.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 +import os + +import rospy +from evtol_behavior.autopilot_context_ros import AutoPilotContextRos +from evtol_behavior.behavior.state.land import Land +from evtol_behavior.behavior_context_ros import BehaviorContextRos +from evtol_behavior.health_system import HealthSystem +from evtol_common.drone_ros import DroneRos # type: ignore[attr-defined] +from evtol_msgs.msg import DroneStatus # type: ignore[attr-defined] +from std_msgs.msg import Header +from std_srvs.srv import SetBool, SetBoolRequest + + +class BehaviorExecutive: + """ + This class is responsible for the main decision-making of the evtol and uses all systems to control the evtol. + Integration and delegation for other modules. Code for any decision drone has to make. + """ + + def __init__(self): + # Initialize node + rospy.init_node("evtol_behavior") + + # Initialize attributes + # TODO tests fail if 30 hz, that shouldnt hapen + self.rate = rospy.Rate(rospy.get_param("/evtol_nav/rate", 20)) + self.sim = rospy.get_param("/simulation", os.environ.get("SIM", False)) + self.last_req = rospy.Time.now() + + self._drone = DroneRos() + # TODO limit drone nand path to pass from here during loop + self._behavior = BehaviorContextRos(self._drone, self.sim) # TODO clean up + self._autopilot = AutoPilotContextRos(self._behavior) + self._health_system = HealthSystem() + + # Publisher + self._drone_status_pub = rospy.Publisher("evtol_behavior/drone_status", DroneStatus, queue_size=10) + + # TODO add uwb to sim + if not self.sim: + rospy.wait_for_service("/evtol_sensors/uwb/enable") + self.srv = rospy.ServiceProxy("evtol_sensors/uwb/enable", SetBool) + + # Main communication node for ground control + def run(self): + """ + Main loop + + :return: None + """ + + # Main loop to follow waypoints + while not rospy.is_shutdown(): + # Behaviour Executive + # TODO pass drone & path harder then previously thought might be possible but not worth time rigth now + self._behavior.run_state_algorithim() + + # Health System TODO fix starting issue and maybe but this + if self._health_system.check_health(self._drone.z - self._drone.disarm_height): + if not self.sim: + self.srv.call(SetBoolRequest(data=True)) + rospy.loginfo_throttle(1, "switching to uwb") + self._behavior.state = Land() + + # AutoPilot + # TODO pass behavior + self._autopilot.check_autopilot() + + # TODO put in drone maybe? + msg = DroneStatus(header=Header(stamp=rospy.Time.now(), frame_id="map"), data=self._drone.status) + self._drone_status_pub.publish(msg) + + self.rate.sleep() diff --git a/soccer_strategy/src/soccer_strategy/strategy/__init__.py b/soccer_strategy/src/soccer_strategy/strategy/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/soccer_strategy/src/soccer_strategy/strategy/strategy.py b/soccer_strategy/src/soccer_strategy/strategy_old/strategy.py similarity index 100% rename from soccer_strategy/src/soccer_strategy/strategy/strategy.py rename to soccer_strategy/src/soccer_strategy/strategy_old/strategy.py diff --git a/soccer_strategy/src/soccer_strategy/strategy/strategy_determine_side.py b/soccer_strategy/src/soccer_strategy/strategy_old/strategy_determine_side.py similarity index 100% rename from soccer_strategy/src/soccer_strategy/strategy/strategy_determine_side.py rename to soccer_strategy/src/soccer_strategy/strategy_old/strategy_determine_side.py diff --git a/soccer_strategy/src/soccer_strategy/strategy/strategy_dummy.py b/soccer_strategy/src/soccer_strategy/strategy_old/strategy_dummy.py similarity index 100% rename from soccer_strategy/src/soccer_strategy/strategy/strategy_dummy.py rename to soccer_strategy/src/soccer_strategy/strategy_old/strategy_dummy.py diff --git a/soccer_strategy/src/soccer_strategy/strategy/strategy_finished.py b/soccer_strategy/src/soccer_strategy/strategy_old/strategy_finished.py similarity index 100% rename from soccer_strategy/src/soccer_strategy/strategy/strategy_finished.py rename to soccer_strategy/src/soccer_strategy/strategy_old/strategy_finished.py diff --git a/soccer_strategy/src/soccer_strategy/strategy/strategy_freekick.py b/soccer_strategy/src/soccer_strategy/strategy_old/strategy_freekick.py similarity index 100% rename from soccer_strategy/src/soccer_strategy/strategy/strategy_freekick.py rename to soccer_strategy/src/soccer_strategy/strategy_old/strategy_freekick.py diff --git a/soccer_strategy/src/soccer_strategy/strategy/strategy_penaltykick.py b/soccer_strategy/src/soccer_strategy/strategy_old/strategy_penaltykick.py similarity index 100% rename from soccer_strategy/src/soccer_strategy/strategy/strategy_penaltykick.py rename to soccer_strategy/src/soccer_strategy/strategy_old/strategy_penaltykick.py diff --git a/soccer_strategy/src/soccer_strategy/strategy/strategy_ready.py b/soccer_strategy/src/soccer_strategy/strategy_old/strategy_ready.py similarity index 100% rename from soccer_strategy/src/soccer_strategy/strategy/strategy_ready.py rename to soccer_strategy/src/soccer_strategy/strategy_old/strategy_ready.py diff --git a/soccer_strategy/src/soccer_strategy/strategy/strategy_set.py b/soccer_strategy/src/soccer_strategy/strategy_old/strategy_set.py similarity index 100% rename from soccer_strategy/src/soccer_strategy/strategy/strategy_set.py rename to soccer_strategy/src/soccer_strategy/strategy_old/strategy_set.py diff --git a/soccer_strategy/src/soccer_strategy/strategy/strategy_stationary.py b/soccer_strategy/src/soccer_strategy/strategy_old/strategy_stationary.py similarity index 100% rename from soccer_strategy/src/soccer_strategy/strategy/strategy_stationary.py rename to soccer_strategy/src/soccer_strategy/strategy_old/strategy_stationary.py diff --git a/soccer_strategy/src/soccer_strategy/strategy/utils.py b/soccer_strategy/src/soccer_strategy/strategy_old/utils.py similarity index 100% rename from soccer_strategy/src/soccer_strategy/strategy/utils.py rename to soccer_strategy/src/soccer_strategy/strategy_old/utils.py diff --git a/tools/setup/dgfs.py b/tools/setup/dgfs.py new file mode 100644 index 000000000..b4e599a22 --- /dev/null +++ b/tools/setup/dgfs.py @@ -0,0 +1,18 @@ +# with is like your try .. finally block in this case +with open("rosdep_2.txt", "r") as file: + # read a list of lines into data + data = file.readlines() + +for idx, line in enumerate(data): + print(line.split(" ")) + if len(line.split(" ")) > 1: + data[idx] = line.split(" ")[0] + "\n" +print(data) +# print "Your name: " + data[0] +# +# # now change the 2nd line, note that you have to add a newline +# data[1] = 'Mage\n' +# +# # and write everything back +with open("rosdep_2.txt", "w") as file: + file.writelines(data) diff --git a/tools/setup/rosdep_2.txt b/tools/setup/rosdep_2.txt new file mode 100644 index 000000000..7c4c01fbc --- /dev/null +++ b/tools/setup/rosdep_2.txt @@ -0,0 +1,130 @@ +screen +vim +git-lfs +python3-pip +python3-protobuf +protobuf-compiler +libprotobuf-dev +libjpeg8-dev +wget +ccache +dirmngr +gnupg2 +lsb-release +net-tools +iputils-ping +apt-utils +software-properties-common +sudo +unzip +curl +libxkbcommon-x11-0 +libxcb-icccm4 +libxcb-xkb1 +libxcb-icccm4 +libxcb-image0 +libxcb-render-util0 +libxcb-randr0 +libxcb-keysyms1 +libxcb-xinerama0 +qt5-default +qtbase5-dev +python3-pyqt5 +python-is-python3 +git +python3-setuptools +default-jdk +libprotobuf-dev +libprotoc-dev +protobuf-compiler +python3-protobuf +python3-scipy +xterm +acl +aria2 +autoconf +automake +binutils +bison +brotli +bzip2 +coreutils +curl +dbus +dnsutils +dpkg +dpkg-dev +fakeroot +file +findutils +flex +fonts-noto-color-emoji +ftp +g++ +gcc +gnupg2 +haveged +imagemagick +iproute2 +iputils-ping +jq +lib32z1 +libc++-dev +libc++abi-dev +libc6-dev +libcurl4 +libgbm-dev +libgconf-2-4 +libgsl-dev +libgtk-3-0 +libmagic-dev +libmagickcore-dev +libmagickwand-dev +libsecret-1-dev +libsqlite3-dev +libtool +libunwind8 +libxkbfile-dev +libxss1 +libyaml-dev +locales +m4 +make +mediainfo +mercurial +net-tools +netcat +openssh-client +p7zip-full +p7zip-rar +parallel +pass +patchelf +pigz +pkg-config +pollinate +python-is-python3 +rpm +rsync +shellcheck +sphinxsearch +sqlite3 +ssh +sshpass +subversion +sudo +swig +tar +telnet +texinfo +time +tk +tzdata +unzip +upx +wget +xorriso +xvfb +xz-utils +zip +zsync diff --git a/tools/setup/setup_fdsf.sh b/tools/setup/setup_fdsf.sh new file mode 100644 index 000000000..0bd984ebe --- /dev/null +++ b/tools/setup/setup_fdsf.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash + + +(type -p wget >/dev/null || (sudo apt update && sudo apt-get install wget -y)) \ + && sudo mkdir -p -m 755 /etc/apt/keyrings \ + && wget -qO- https://cli.github.com/packages/githubcli-archive-keyring.gpg | sudo tee /etc/apt/keyrings/githubcli-archive-keyring.gpg > /dev/null \ + && sudo chmod go+r /etc/apt/keyrings/githubcli-archive-keyring.gpg \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/githubcli-archive-keyring.gpg] https://cli.github.com/packages stable main" | sudo tee /etc/apt/sources.list.d/github-cli.list > /dev/null \ + && sudo apt update \ + && sudo apt install gh -y + +sudo apt-get install -y $(cat dep.txt)