Reformating of Strategix and Cetautomatix nodes (#39)

* Hello World!
Start of reformating of cetautomatix and strategix

* Blacked + continuation of robot.py reformating

* Strategix & Cetautomatix reformating finished! 🎉🎉🎉
Available actions: both Phares & Pavillon

* Update ros_ws.repos (#38)

* Build libceres-dev as N/A on Debian buster aarch64 (#41)

* Add ceres-solver
It's not available in debian buster aarch64

* [ros_ws] dependencies - override disable SSE for aarch64 cross build

* Fix foxy bondcpp version - 2.0.0

* Changed odom_callback to use kdl instead of tf2 ros

* Added tf2_kdl as dependency of cetautomatix

* [CI] Fail in case of process crash ⚠️ (#42)

* Added Gobelet support
Continued translating old action positions to new object classes
Few fixes & comments

* Reimplemented strategy modes

* Utils time 🎉 Plot map of all actions and plot of strategy modes

* Fix conflicts

* 🍺 Hi Ewen! 🍺

* 🍺 Blacked 🍺

Co-authored-by: Philéas LAMBERT <phileas.lambert@gmail.com>
Co-authored-by: Ewen BRUN <ewen.brun@icloud.com>

src/assurancetourix/strategix/strategix/action_objects/__init__.py

@@ -0,0 +1,5 @@
+from .action import Action
+from .gobelet import Gobelet
+from .manche_air import MancheAir
+from .phare import Phare
+from .ecueil import Ecueil

src/assurancetourix/strategix/strategix/action_objects/action.py

@@ -0,0 +1,32 @@
+"""Base Action class for all types of action"""
+
+
+class Action:
+    def __init__(self, position=(0, 0), **kwargs):
+        self.position = position
+        self.rotation = kwargs.get("rotation", 0)
+        self.tags = kwargs.get("tags", {})
+
+    def get_initial_position(self, robot=None):
+        """Function called when the robot needs to get the position of its objective"""
+        return self.position
+
+    def get_initial_orientation(self, robot=None):
+        """Function called when the robot needs to get the orientation of its objective"""
+        return self.rotation
+
+    def preempt_action(self, robot=None, action_list=None):
+        """Function called when the action is preempted"""
+        pass
+
+    def finish_action(self, robot=None):
+        """Function called when the action is finished"""
+        pass
+
+    def release_action(self, robot=None):
+        """Function called when the action is released"""
+        pass
+
+    def start_actuator(self, robot=None):
+        """Function called when the robot reaches its objective and starts the actuator"""
+        pass

src/assurancetourix/strategix/strategix/action_objects/ecueil.py

@@ -0,0 +1,25 @@
+from .action import Action
+
+
+class Ecueil(Action):
+    def __init__(self, position, **kwargs):
+        super().__init__(position, **kwargs)
+        self.gob_list = kwargs.get("gob_list", [])
+
+    def get_initial_position(self, robot):
+        return self.position
+
+    # def preempt_action(self):
+    #     for gobelet_id in self.gob_list:
+    #         gobelet = actions.get(gobelet_id)
+    #         gobelet.preempt_action()
+
+    # def release_action(self):
+    #     for gobelet_id in self.gob_list:
+    #         gobelet = actions.get(gobelet_id)
+    #         gobelet.release_action()
+
+    # def finish_action(self):
+    #     for gobelet_id in self.gob_list:
+    #         gobelet = actions.get(gobelet_id)
+    #         gobelet.finish_action()

src/assurancetourix/strategix/strategix/action_objects/gobelet.py

@@ -0,0 +1,70 @@
+from .action import Action
+import numpy as np
+from PyKDL import Vector, Rotation, Frame
+
+
+class Gobelet(Action):
+    def __init__(self, position, color, **kwargs):
+        super().__init__(position, **kwargs)
+        self.color = color
+        self.pump_id = None
+
+    def get_initial_orientation(self, robot):
+        theta = 180 - np.rad2deg(
+            np.arctan(
+                (self.position[1] - robot.position[1])
+                / (self.position[0] - robot.position[0])
+            )
+        )
+        return theta
+
+    def get_initial_position(self, robot):
+        if robot.simulation:
+            robot_to_gob = (0.04, 0.08)
+        else:
+            robot_to_gob = robot.actuators.PUMPS.get(self.pump_id).get("pos")
+        vector_robot_to_gob = Vector(robot_to_gob[0], robot_to_gob[1], 0)
+        # Find the angle between the robot's and the gobelet's position (theta)
+        theta = 180 - np.rad2deg(
+            np.arctan(
+                (self.position[1] - robot.position[1])
+                / (self.position[0] - robot.position[0])
+            )
+        )
+        # Frame between gobelet and center of robot
+        frame_robot_to_gob = Frame(Rotation.RotZ(theta), vector_robot_to_gob)
+        # Frame between gobelet and map
+        frame_map_to_gob = Frame(
+            Rotation.Identity(), Vector(self.position[0], self.position[1], 0)
+        )
+        # Frame between robot and map
+        frame_map_to_robot = frame_map_to_gob * frame_robot_to_gob.Inverse()
+        return (frame_map_to_robot.p.x(), frame_map_to_robot.p.y())
+
+    def preempt_action(self, robot, action_list):
+        if robot.simulation:
+            return
+        # Find the first available pump
+        for pump_id, pump_dict in robot.actuators.PUMPS.items():
+            if pump_dict.get("status") is None:
+                self.pump_id = pump_id
+                # Find the id of this Gobelet
+                for action_id, action_dict in action_list:
+                    if action_dict == self:
+                        pump_dict["status"] = action_id
+                        robot.get_logger().info(
+                            f"Pump {pump_id} preempted {action_id}."
+                        )
+                        return
+
+    def release_action(self, robot):
+        if robot.simulation:
+            return
+        robot.actuators.PUMPS.get(self.pump_id).pop("status")
+        self.pump_id = None
+
+    def finish_action(self, robot):
+        if robot.simulation:
+            return
+        robot.actuators.PUMPS.get(self.pump_id).pop("status")
+        self.pump_id = None

src/assurancetourix/strategix/strategix/action_objects/manche_air.py

@@ -0,0 +1,26 @@
+from .action import Action
+
+
+class MancheAir(Action):
+    def __init__(self, position, **kwargs):
+        super().__init__(position, **kwargs)
+        self.rotation = -90
+        self.step = 0
+
+    def get_initial_position(self, robot):
+        if self.step == 0:
+            offset = -0.1 if self.tags["ONLY_SIDE"] == "blue" else 0.1
+        else:
+            offset = 0.1 if self.tags["ONLY_SIDE"] == "blue" else -0.1
+        position = (self.position[0] + offset, robot.width.value / 2 + 0.05)
+        return position
+
+    def start_actuator(self, robot):
+        if self.step == 0:
+            # Open robot arm - robot.openArm()
+            # Force new objective to be next position
+            pass
+        else:
+            # Close robot arm - robot.closeArm()
+            pass
+        self.step += 1

src/assurancetourix/strategix/strategix/action_objects/phare.py

@@ -0,0 +1,17 @@
+from .action import Action
+
+
+class Phare(Action):
+    def __init__(self, position, **kwargs):
+        super().__init__(position, **kwargs)
+        self.rotation = 90
+
+    def get_initial_position(self, robot):
+        return (self.position[0], self.position[1] - robot.length.value / 2 - 0.1)
+
+    def start_actuator(self, robot):
+        response = robot.synchronous_call(
+            robot.trigger_deploy_pharaon_client,
+            robot.trigger_deploy_pharaon_request,
+        )
+        return response.success

src/assurancetourix/strategix/strategix/actions.py

@@ -1,92 +1,102 @@
 #!/usr/bin/env python3
+from strategix.action_objects import Action, Phare, Ecueil, MancheAir, Gobelet
 
 actions = {
-    "STUPID_1": {},
-    "STUPID_2": {},
-    "STUPID_3": {},
-    "MANCHE1": {"ONLY_SIDE": "blue"},
-    "MANCHE2": {"ONLY_SIDE": "blue"},
-    "MANCHE3": {"ONLY_SIDE": "yellow"},
-    "MANCHE4": {"ONLY_SIDE": "yellow"},
-    "PAVILLON": {"STATUS": "PREEMPTED"},
-    "ECUEIL_1": {
-        "ONLY_ROBOT": "obelix",
-        "GOBS": ["GOB35", "GOB36", "GOB37", "GOB38", "GOB39"],
-    },
-    "ECUEIL_2": {
-        "ONLY_ROBOT": "obelix",
-        "GOBS": ["GOB40", "GOB41", "GOB42", "GOB43", "GOB44"],
-    },
-    "ECUEIL_BLEU": {
-        "ONLY_ROBOT": "obelix",
-        "ONLY_SIDE": "blue",
-        "GOBS": ["GOB25", "GOB26", "GOB27", "GOB28", "GOB29"],
-    },
-    "ECUEIL_JAUNE": {
-        "ONLY_ROBOT": "obelix",
-        "ONLY_SIDE": "yellow",
-        "GOBS": ["GOB30", "GOB31", "GOB32", "GOB33", "GOB34"],
-    },
-    "PHARE_BLEU": {"ONLY_SIDE": "blue"},
-    "PHARE_JAUNE": {"ONLY_SIDE": "yellow"},
-    "CHENAL_BLEU_VERT_1": {"ONLY_SIDE": "blue", "STATUS": "PREEMPTED"},
-    # "CHENAL_BLEU_VERT_2": {"ONLY_SIDE": "blue"},
-    "CHENAL_BLEU_ROUGE_1": {"ONLY_SIDE": "blue", "STATUS": "PREEMPTED"},
-    # "CHENAL_BLEU_ROUGE_2": {"ONLY_SIDE": "blue"},
-    "CHENAL_JAUNE_VERT_1": {"ONLY_SIDE": "yellow", "STATUS": "PREEMPTED"},
-    # "CHENAL_JAUNE_VERT_2": {"ONLY_SIDE": "yellow"},
-    "CHENAL_JAUNE_ROUGE_1": {"ONLY_SIDE": "yellow", "STATUS": "PREEMPTED"},
-    # "CHENAL_JAUNE_ROUGE_2": {"ONLY_SIDE": "yellow"},
-    # Red Cups
-    "GOB2": {"COLOR": "RED"},
-    "GOB3": {"COLOR": "RED"},
-    "GOB5": {"COLOR": "RED"},
-    "GOB7": {"COLOR": "RED"},
-    "GOB9": {"COLOR": "RED"},
-    "GOB11": {"COLOR": "RED"},
-    "GOB13": {"COLOR": "RED"},
-    "GOB15": {"COLOR": "RED"},
-    "GOB17": {"COLOR": "RED"},
-    "GOB19": {"COLOR": "RED"},
-    "GOB22": {"COLOR": "RED"},
-    "GOB23": {"COLOR": "RED"},
-    # Cups in ECUEIL_BLEU:
-    "GOB25": {"COLOR": "RED", "IN_ECUEIL": True, "ONLY_SIDE": "blue"},
-    "GOB27": {"COLOR": "RED", "IN_ECUEIL": True, "ONLY_SIDE": "blue"},
-    "GOB29": {"COLOR": "RED", "IN_ECUEIL": True, "ONLY_SIDE": "blue"},
-    # Cups in ECUEIL_JAUNE:
-    "GOB31": {"COLOR": "RED", "IN_ECUEIL": True, "ONLY_SIDE": "yellow"},
-    "GOB33": {"COLOR": "RED", "IN_ECUEIL": True, "ONLY_SIDE": "yellow"},
-    # Cups in ECUEIL_1 & ECUEIL_2 following the Scenario 1:
-    "GOB36": {"COLOR": "RED", "IN_ECUEIL": True},
-    "GOB39": {"COLOR": "RED", "IN_ECUEIL": True},
-    "GOB41": {"COLOR": "RED", "IN_ECUEIL": True},
-    "GOB42": {"COLOR": "RED", "IN_ECUEIL": True},
-    "GOB44": {"COLOR": "RED", "IN_ECUEIL": True},
-    # Green Cups
-    "GOB1": {"COLOR": "GREEN"},
-    "GOB4": {"COLOR": "GREEN"},
-    "GOB6": {"COLOR": "GREEN"},
-    "GOB8": {"COLOR": "GREEN"},
-    "GOB10": {"COLOR": "GREEN"},
-    "GOB12": {"COLOR": "GREEN"},
-    "GOB14": {"COLOR": "GREEN"},
-    "GOB16": {"COLOR": "GREEN"},
-    "GOB18": {"COLOR": "GREEN"},
-    "GOB20": {"COLOR": "GREEN"},
-    "GOB21": {"COLOR": "GREEN"},
-    "GOB24": {"COLOR": "GREEN"},
-    # Cups in ECUEIL_BLEU:
-    "GOB26": {"COLOR": "GREEN", "IN_ECUEIL": True, "ONLY_SIDE": "blue"},
-    "GOB28": {"COLOR": "GREEN", "IN_ECUEIL": True, "ONLY_SIDE": "blue"},
-    # Cups in ECUEIL_JAUNE:
-    "GOB30": {"COLOR": "GREEN", "IN_ECUEIL": True, "ONLY_SIDE": "yellow"},
-    "GOB32": {"COLOR": "GREEN", "IN_ECUEIL": True, "ONLY_SIDE": "yellow"},
-    "GOB34": {"COLOR": "GREEN", "IN_ECUEIL": True, "ONLY_SIDE": "yellow"},
-    # Cups in ECUEIL_1 & ECUEIL_2 following the Scenario 1:
-    "GOB35": {"COLOR": "GREEN", "IN_ECUEIL": True},
-    "GOB37": {"COLOR": "GREEN", "IN_ECUEIL": True},
-    "GOB38": {"COLOR": "GREEN", "IN_ECUEIL": True},
-    "GOB40": {"COLOR": "GREEN", "IN_ECUEIL": True},
-    "GOB43": {"COLOR": "GREEN", "IN_ECUEIL": True},
+    "PAVILLON": Action(tags={"STATUS": "PREEMPT"}),
+    "PHARE_BLEU": Phare(position=(0.26, 2), tags={"ONLY_SIDE": "blue"}),
+    "PHARE_JAUNE": Phare(position=(2.775, 2), tags={"ONLY_SIDE": "yellow"}),
+    "MANCHE1": MancheAir(position=(0.23, 0), tags={"ONLY_SIDE": "blue"}),
+    "MANCHE2": MancheAir(position=(0.635, 0), tags={"ONLY_SIDE": "blue"}),
+    "MANCHE3": MancheAir(position=(2.365, 0), tags={"ONLY_SIDE": "yellow"}),
+    "MANCHE4": MancheAir(position=(2.77, 0), tags={"ONLY_SIDE": "yellow"}),
+    "ECUEIL_1": Ecueil(
+        position=(0.85, 2),
+        rotation=90,
+        gob_list=["GOB35", "GOB36", "GOB37", "GOB38", "GOB39"],
+        tags={"ONLY_ROBOT": "obelix"},
+    ),
+    "ECUEIL_2": Ecueil(
+        position=(2.15, 2),
+        rotation=90,
+        gob_list=["GOB40", "GOB41", "GOB42", "GOB43", "GOB44"],
+        tags={"ONLY_ROBOT": "obelix"},
+    ),
+    "ECUEIL_BLEU": Ecueil(
+        position=(0, 0.4),
+        rotation=180,
+        gob_list=["GOB25", "GOB26", "GOB27", "GOB28", "GOB29"],
+        tags={"ONLY_ROBOT": "obelix", "ONLY_SIDE": "blue"},
+    ),
+    "ECUEIL_JAUNE": Ecueil(
+        position=(3, 0.4),
+        rotation=0,
+        gob_list=["GOB30", "GOB31", "GOB32", "GOB33", "GOB34"],
+        tags={"ONLY_ROBOT": "obelix", "ONLY_SIDE": "yellow"},
+    ),
+    "GOB1": Gobelet(position=(0.3, 0.8), color="GREEN"),
+    "GOB2": Gobelet(position=(0.3, 1.6), color="RED"),
+    "GOB3": Gobelet(position=(0.45, 0.92), color="RED"),
+    "GOB4": Gobelet(position=(0.45, 1.49), color="GREEN"),
+    "GOB5": Gobelet(position=(0.67, 1.9), color="RED"),
+    "GOB6": Gobelet(position=(0.95, 1.6), color="GREEN"),
+    "GOB7": Gobelet(position=(1.005, 0.045), color="RED"),
+    "GOB8": Gobelet(position=(1.065, 0.35), color="GREEN"),
+    "GOB9": Gobelet(position=(1.1, 1.2), color="RED"),
+    "GOB10": Gobelet(position=(1.27, 0.8), color="GREEN"),
+    "GOB11": Gobelet(position=(1.335, 0.35), color="RED"),
+    "GOB12": Gobelet(position=(1.395, 0.045), color="GREEN"),
+    "GOB13": Gobelet(position=(1.605, 0.045), color="RED"),
+    "GOB14": Gobelet(position=(1.665, 0.35), color="GREEN"),
+    "GOB15": Gobelet(position=(1.73, 0.8), color="RED"),
+    "GOB16": Gobelet(position=(1.9, 1.2), color="GREEN"),
+    "GOB17": Gobelet(position=(1.935, 0.35), color="RED"),
+    "GOB18": Gobelet(position=(1.995, 0.045), color="GREEN"),
+    "GOB19": Gobelet(position=(2.05, 1.6), color="RED"),
+    "GOB20": Gobelet(position=(2.33, 1.9), color="GREEN"),
+    "GOB21": Gobelet(position=(2.55, 0.92), color="GREEN"),
+    "GOB22": Gobelet(position=(2.55, 1.49), color="RED"),
+    "GOB23": Gobelet(position=(2.7, 0.8), color="RED"),
+    "GOB24": Gobelet(position=(2.7, 1.6), color="GREEN"),
+    # "GOB25": Gobelet(
+    #     position=(0, 1), color="RED", tags={"IN_ECUEIL": True, "ONLY_SIDE": "blue"}
+    # ),
 }
+
+# actions = {
+#     "CHENAL_BLEU_VERT_1": {"ONLY_SIDE": "blue", "STATUS": "PREEMPTED"},
+#     # "CHENAL_BLEU_VERT_2": {"ONLY_SIDE": "blue"},
+#     "CHENAL_BLEU_ROUGE_1": {"ONLY_SIDE": "blue", "STATUS": "PREEMPTED"},
+#     # "CHENAL_BLEU_ROUGE_2": {"ONLY_SIDE": "blue"},
+#     "CHENAL_JAUNE_VERT_1": {"ONLY_SIDE": "yellow", "STATUS": "PREEMPTED"},
+#     # "CHENAL_JAUNE_VERT_2": {"ONLY_SIDE": "yellow"},
+#     "CHENAL_JAUNE_ROUGE_1": {"ONLY_SIDE": "yellow", "STATUS": "PREEMPTED"},
+#     # "CHENAL_JAUNE_ROUGE_2": {"ONLY_SIDE": "yellow"},
+#     # Red Cups
+#     # Cups in ECUEIL_BLEU:
+#     "GOB25": {"COLOR": "RED", "IN_ECUEIL": True, "ONLY_SIDE": "blue"},
+#     "GOB27": {"COLOR": "RED", "IN_ECUEIL": True, "ONLY_SIDE": "blue"},
+#     "GOB29": {"COLOR": "RED", "IN_ECUEIL": True, "ONLY_SIDE": "blue"},
+#     # Cups in ECUEIL_JAUNE:
+#     "GOB31": {"COLOR": "RED", "IN_ECUEIL": True, "ONLY_SIDE": "yellow"},
+#     "GOB33": {"COLOR": "RED", "IN_ECUEIL": True, "ONLY_SIDE": "yellow"},
+#     # Cups in ECUEIL_1 & ECUEIL_2 following the Scenario 1:
+#     "GOB36": {"COLOR": "RED", "IN_ECUEIL": True},
+#     "GOB39": {"COLOR": "RED", "IN_ECUEIL": True},
+#     "GOB41": {"COLOR": "RED", "IN_ECUEIL": True},
+#     "GOB42": {"COLOR": "RED", "IN_ECUEIL": True},
+#     "GOB44": {"COLOR": "RED", "IN_ECUEIL": True},
+#     # Green Cups
+#     # Cups in ECUEIL_BLEU:
+#     "GOB26": {"COLOR": "GREEN", "IN_ECUEIL": True, "ONLY_SIDE": "blue"},
+#     "GOB28": {"COLOR": "GREEN", "IN_ECUEIL": True, "ONLY_SIDE": "blue"},
+#     # Cups in ECUEIL_JAUNE:
+#     "GOB30": {"COLOR": "GREEN", "IN_ECUEIL": True, "ONLY_SIDE": "yellow"},
+#     "GOB32": {"COLOR": "GREEN", "IN_ECUEIL": True, "ONLY_SIDE": "yellow"},
+#     "GOB34": {"COLOR": "GREEN", "IN_ECUEIL": True, "ONLY_SIDE": "yellow"},
+#     # Cups in ECUEIL_1 & ECUEIL_2 following the Scenario 1:
+#     "GOB35": {"COLOR": "GREEN", "IN_ECUEIL": True},
+#     "GOB37": {"COLOR": "GREEN", "IN_ECUEIL": True},
+#     "GOB38": {"COLOR": "GREEN", "IN_ECUEIL": True},
+#     "GOB40": {"COLOR": "GREEN", "IN_ECUEIL": True},
+#     "GOB43": {"COLOR": "GREEN", "IN_ECUEIL": True},
+# }