started to make strategy

.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

soccer_strategy/src/soccer_strategy/__init__.py

@@ -0,0 +1 @@
+from soccer_strategy import *

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

soccer_strategy/src/soccer_strategy/behavior/__init__.py

@@ -0,0 +1 @@
+from soccer_strategy.behavior.behavior import Behavior

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

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

soccer_strategy/src/soccer_strategy/behavior/state/__init__.py

@@ -0,0 +1,2 @@
+# TODO setup proper imports
+from soccer_strategy.behavior.state import *

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