Merge pull request #179 from cogip/168-planner-rework-planner-using-goap-approach

Planner: Rework Planner using GOAP approach

Author: ecourtois

cogip/cpp/libraries/shared_memory/binding.cpp

@@ -85,6 +85,7 @@ NB_MODULE(shared_memory, m) {
         .def_rw("strategy", &shared_properties_t::strategy, "Strategy ID")
         .def_rw("start_position", &shared_properties_t::start_position, "Start position ID")
         .def_rw("avoidance_strategy", &shared_properties_t::avoidance_strategy, "Avoidance strategy ID")
+        .def_rw("goap_depth", &shared_properties_t::goap_depth, "Depth of the GOAP search tree, 0 to disable GOAP")
         .def("__repr__", [](const shared_properties_t& properties) {
            std::ostringstream oss;
            oss << properties;

cogip/cpp/libraries/shared_memory/include/shared_memory/shared_properties.hpp

@@ -35,6 +35,7 @@ typedef struct shared_properties_t {
     std::uint8_t strategy;  ///< Strategy ID
     std::uint8_t start_position;  ///< Start position ID
     std::uint8_t avoidance_strategy;  ///< Avoidance strategy ID
+    std::uint8_t goap_depth;  ///< Depth of the GOAP search tree, 0 to disable GOAP
 } shared_properties_t;
 
 /// Overloads the stream insertion operator for `shared_properties_t`.
@@ -58,6 +59,7 @@ inline std::ostream& operator<<(std::ostream& os, const shared_properties_t& dat
        << "strategy=" << static_cast<int>(data.strategy) << ", "
        << "start_position=" << static_cast<int>(data.start_position) << ", "
        << "avoidance_strategy=" << static_cast<int>(data.avoidance_strategy) << ", "
+       << "goap_depth=" << static_cast<int>(data.goap_depth) << ", "
        << ")";
     return os;
 }

cogip/tools/planner/__main__.py

@@ -1,19 +1,18 @@
 #!/usr/bin/env python3
 import asyncio
 import logging
-import os
 from pathlib import Path
 from typing import Annotated, Optional
 
 import typer
 from watchfiles import PythonFilter, run_process
 
 from . import logger
-from .actions import Strategy
+from .actions import StrategyEnum
 from .avoidance.avoidance import AvoidanceStrategy
 from .planner import Planner
-from .positions import StartPosition
 from .properties import properties_schema
+from .start_positions import StartPositionEnum
 from .table import TableEnum
 
 
@@ -219,23 +218,23 @@ def main_opt(
         ),
     ] = TableEnum.Game.name,
     strategy: Annotated[
-        Strategy,
+        StrategyEnum,
         typer.Option(
             "-s",
             "--strategy",
             help="Default strategy on startup",
             envvar="PLANNER_STRATEGY",
         ),
-    ] = Strategy.TestVisitStartingAreas.name,
+    ] = StrategyEnum.TestVisitStartingAreas.name,
     start_position: Annotated[
-        StartPosition,
+        StartPositionEnum,
         typer.Option(
             "-p",
             "--start-position",
             help="Default start position on startup",
             envvar="PLANNER_START_POSITION",
         ),
-    ] = StartPosition.Bottom.name,
+    ] = StartPositionEnum.Bottom.name,
     avoidance_strategy: Annotated[
         AvoidanceStrategy,
         typer.Option(
@@ -245,6 +244,17 @@ def main_opt(
             envvar="PLANNER_AVOIDANCE_STRATEGY",
         ),
     ] = AvoidanceStrategy.AvoidanceCpp.name,
+    goap_depth: Annotated[
+        int,
+        typer.Option(
+            "-gd",
+            "--goap-depth",
+            min=properties["goap_depth"]["minimum"],
+            max=properties["goap_depth"]["maximum"],
+            help=properties["goap_depth"]["description"],
+            envvar="PLANNER_GOAP_DEPTH",
+        ),
+    ] = properties["goap_depth"]["default"],
     reload: Annotated[
         bool,
         typer.Option(
@@ -267,9 +277,6 @@ def main_opt(
     if debug:
         logger.setLevel(logging.DEBUG)
 
-    # Make sure robot ID is also available as environment variable for context creation
-    os.environ["ROBOT_ID"] = str(id)
-
     if not server_url:
         server_url = f"http://localhost:809{id}"
 
@@ -298,6 +305,7 @@ def main_opt(
         strategy,
         start_position,
         avoidance_strategy,
+        goap_depth,
         debug,
     )
 

cogip/tools/planner/actions/__init__.py

@@ -5,16 +5,16 @@
 
 from cogip.utils.argenum import ArgEnum
 from .. import logger
-from .actions import Actions
+from .strategy import Strategy
 
 
 def strip_action_name(name: str) -> str:
-    if name.endswith("Actions"):
-        return name[:-7]
+    if name.endswith("Strategy"):
+        return name[:-8]
     return name
 
 
-actions_found = []
+strategies_found = []
 
 for path in Path(__file__).parent.glob("*.py"):
     if path.name == "__init__.py":
@@ -30,19 +30,19 @@ def strip_action_name(name: str) -> str:
         logger.error(
             f"Import error in 'cogip/tools/planner/actions/{module_path.name}': "
             "Modules from the 'cogip.planner.actions' package cannot use relative import "
-            "to allow dynamic discovery of Actions classes."
+            "to allow dynamic discovery of Strategy classes."
         )
         sys.exit(1)
 
     for name, obj in inspect.getmembers(module, inspect.isclass):
-        if issubclass(obj, Actions) and obj is not Actions:
-            actions_found.append(obj)
+        if issubclass(obj, Strategy) and obj is not Strategy:
+            strategies_found.append(obj)
 
-sorted_actions = sorted(actions_found, key=lambda cls: cls.__name__)
-actions_map = {strip_action_name(strategy.__name__): i + 1 for i, strategy in enumerate(sorted_actions)}
+sorted_strategies = sorted(strategies_found, key=lambda cls: cls.__name__)
+strategies_map = {strip_action_name(strategy.__name__): i + 1 for i, strategy in enumerate(sorted_strategies)}
 
-Strategy = ArgEnum("Strategy", actions_map)
+StrategyEnum = ArgEnum("StrategyEnum", strategies_map)
 
-action_classes: dict[Strategy, Actions] = {
-    strategy: actions_class for strategy, actions_class in zip(Strategy, sorted_actions)
+strategy_classes: dict[StrategyEnum, Strategy] = {
+    strategy: strategies_class for strategy, strategies_class in zip(StrategyEnum, sorted_strategies)
 }

cogip/tools/planner/actions/action.py

@@ -0,0 +1,114 @@
+import asyncio
+import math
+from collections.abc import Awaitable, Callable
+from typing import TYPE_CHECKING, final
+
+from cogip import models
+from cogip.tools.planner import logger
+from cogip.tools.planner.pose import Pose
+
+if TYPE_CHECKING:
+    from ..planner import Planner
+    from .strategy import Strategy
+
+
+class Action:
+    """
+    This class represents an action of the game.
+    It contains a list of Pose to reach in order.
+    A function can be executed before the action starts and after it ends.
+    """
+
+    logger = logger
+
+    def __init__(self, name: str, planner: "Planner", strategy: "Strategy", interruptable: bool = True):
+        self.name = name
+        self.planner = planner
+        self.strategy = strategy
+        self.interruptable = interruptable
+        self.poses: list[Pose] = []
+        self.before_action_func: Callable[[], Awaitable[None]] | None = None
+        self.after_action_func: Callable[[], Awaitable[None]] | None = None
+        self.recycled: bool = False
+
+    def weight(self) -> float:
+        """
+        Weight of the action.
+        It can be used to choose the next action to select.
+        This is the generic implementation.
+        """
+        raise NotImplementedError
+
+    @final
+    async def act_before_action(self):
+        """
+        Function executed before the action starts.
+        """
+        if self.before_action_func:
+            await self.before_action_func()
+
+    @final
+    async def act_after_action(self):
+        """
+        Function executed after the action ends.
+        """
+        if self.after_action_func:
+            await self.after_action_func()
+
+        # Re-enable all actions after a successful action
+        for action in self.strategy:
+            action.recycled = False
+
+    async def recycle(self):
+        """
+        Function called if the action is blocked and put back in the actions list
+        """
+        self.recycled = True
+
+    @property
+    def pose_current(self) -> models.Pose:
+        return self.planner.pose_current
+
+    async def evaluate(self):
+        # Average robot speed in mm/s.
+        # This is just an empirical value found by testing that gives a good enough
+        # estimation of the time needed to perform the action.
+        # This could be improved later by using target speed and acceleration.
+        average_speed = 100
+
+        await self.act_before_action()
+
+        # Update countdown
+        self.planner.game_context.countdown -= asyncio.sleep.total_sleep
+        asyncio.sleep.reset()
+
+        while len(self.poses) and self.planner.game_context.countdown > 0:
+            pose = self.poses.pop(0)
+
+            await pose.act_before_pose()
+            await pose.act_after_pose()
+
+            # Update countdown
+            distance = math.dist(
+                (self.planner.pose_current.x, self.planner.pose_current.y),
+                (pose.x, pose.y),
+            )
+            self.planner.game_context.countdown -= asyncio.sleep.total_sleep + distance / average_speed
+            asyncio.sleep.reset()
+
+            # Update pose_current
+            self.planner.pose_current.x = pose.x
+            self.planner.pose_current.y = pose.y
+            self.planner.pose_current.O = pose.O
+
+        await self.act_after_action()
+
+        # Update countdown
+        self.planner.game_context.countdown -= asyncio.sleep.total_sleep
+        asyncio.sleep.reset()
+
+    def __str__(self) -> str:
+        return f"Action[0x{id(self):x}]({self.name})"
+
+    def __repr__(self) -> str:
+        return str(self)

cogip/tools/planner/actions/action_align.py

@@ -3,8 +3,9 @@
 from typing import TYPE_CHECKING
 
 from cogip import models
-from cogip.tools.planner import actuators, logger
-from cogip.tools.planner.actions.actions import Action, Actions
+from cogip.tools.planner import actuators
+from cogip.tools.planner.actions.action import Action
+from cogip.tools.planner.actions.strategy import Strategy
 from cogip.tools.planner.avoidance.avoidance import AvoidanceStrategy
 from cogip.tools.planner.pose import AdaptedPose, Pose
 
@@ -21,7 +22,7 @@ class AlignBottomAction(Action):
     def __init__(
         self,
         planner: "Planner",
-        actions: Actions,
+        strategy: Strategy,
         *,
         final_pose: models.Pose = Pose(x=-750, y=-250, O=0),
         reset_countdown=False,
@@ -30,11 +31,11 @@ def __init__(
         self.final_pose = final_pose
         self.reset_countdown = reset_countdown
         self.custom_weight = weight
-        super().__init__("Align Bottom action", planner, actions)
+        super().__init__("Align Bottom action", planner, strategy)
         self.before_action_func = self.init_poses
 
     def set_avoidance(self, new_strategy: AvoidanceStrategy):
-        logger.info(f"{self.name}: set avoidance to {new_strategy.name}")
+        self.logger.info(f"{self.name}: set avoidance to {new_strategy.name}")
         self.planner.shared_properties.avoidance_strategy = new_strategy.val
 
     async def init_poses(self):
@@ -92,11 +93,11 @@ async def init_poses(self):
         self.poses.append(pose)
 
     async def before_align_back(self):
-        logger.info(f"{self.name}: before_align_back")
+        self.logger.info(f"{self.name}: before_align_back")
         self.set_avoidance(AvoidanceStrategy.Disabled)
 
     async def after_align_back(self):
-        logger.info(f"{self.name}: after_align_back")
+        self.logger.info(f"{self.name}: after_align_back")
         current_pose = models.Pose(
             x=-1000 + self.planner.shared_properties.robot_length / 2,
             y=self.start_pose.y,
@@ -106,23 +107,23 @@ async def after_align_back(self):
         await asyncio.sleep(1)
 
     async def before_step_forward(self):
-        logger.info(f"{self.name}: before_step_forward")
+        self.logger.info(f"{self.name}: before_step_forward")
 
     async def after_step_forward(self):
-        logger.info(f"{self.name}: after_step_forward")
+        self.logger.info(f"{self.name}: after_step_forward")
 
     async def before_final_pose(self):
-        logger.info(f"{self.name}: before_final_pose")
+        self.logger.info(f"{self.name}: before_final_pose")
 
     async def after_final_pose(self):
-        logger.info(f"{self.name}: after_final_pose")
+        self.logger.info(f"{self.name}: after_final_pose")
         self.set_avoidance(self.avoidance_backup)
         if self.reset_countdown:
             now = datetime.now(UTC)
             self.planner.countdown_start_timestamp = now
             await self.planner.sio_ns.emit(
                 "start_countdown",
-                (self.planner.robot_id, self.game_context.game_duration, now.isoformat(), "deepskyblue"),
+                (self.planner.robot_id, self.planner.game_context.game_duration, now.isoformat(), "deepskyblue"),
             )
 
     def weight(self) -> float:
@@ -133,26 +134,24 @@ class AlignBottomForBannerAction(AlignBottomAction):
     def __init__(
         self,
         planner: "Planner",
-        actions: Actions,
+        strategy: Strategy,
         weight: float = 2000000.0,
     ):
         super().__init__(
             planner,
-            actions,
+            strategy,
             final_pose=models.Pose(x=-1000 + 220, y=-50 - 450 / 2, O=180),
             reset_countdown=False,
             weight=weight,
         )
         self.after_action_func = self.after_action
 
     async def after_action(self):
-        logger.info("AlignBottomForBannerAction: after_action.")
-        await asyncio.gather(
-            actuators.arm_left_side(self.planner),
-            actuators.arm_right_side(self.planner),
-            actuators.magnet_center_right_in(self.planner),
-            actuators.magnet_center_left_in(self.planner),
-            actuators.magnet_side_right_in(self.planner),
-            actuators.magnet_side_left_in(self.planner),
-            actuators.lift_125(self.planner),
-        )
+        self.logger.info("AlignBottomForBannerAction: after_action.")
+        await actuators.arm_left_side(self.planner)
+        await actuators.arm_right_side(self.planner)
+        await actuators.magnet_center_right_in(self.planner)
+        await actuators.magnet_center_left_in(self.planner)
+        await actuators.magnet_side_right_in(self.planner)
+        await actuators.magnet_side_left_in(self.planner)
+        await actuators.lift_125(self.planner)