Fix/planner

config.toml

@@ -19,6 +19,7 @@ color = "yellow"
 coach = "SimpleCoach"
 
 [Control]
+planner = "VOPlanner"
 
 [OutputLayer]
 host_ip = "localhost"

neonfc_ssl/control_layer/control.py

@@ -1,10 +1,9 @@
 import logging
-import numpy as np
 from math import sqrt, cos, sin
 from neonfc_ssl.core import Layer
-from neonfc_ssl.commons.math import reduce_ang
-from neonfc_ssl.path_planning.drunk_walk import DrunkWalk
+from neonfc_ssl.commons.math import reduce_ang, point_in_rect
 from .control_data import ControlData, RobotCommand
+from .path_planning import PLANNERS
 
 from typing import TYPE_CHECKING
 if TYPE_CHECKING:
@@ -22,6 +21,8 @@ def __init__(self, config, log_q) -> None:
     def _start(self):
         self.logger.info("Starting control module starting ...")
 
+        self.__planner = PLANNERS[self.config["planner"]]
+
         self.logger.info("Control module started!")
 
     def _step(self, data: 'DecisionData'):
@@ -53,82 +54,44 @@ def _step(self, data: 'DecisionData'):
     def run_single_robot(self, data: 'MatchData', command: 'RobotRubric') -> RobotCommand:
         robot = data.robots[command.id]
         field = data.field
-
-        path_planning = DrunkWalk()
-        path_planning.start((robot.x, robot.y), command.target_pose[:2])
-
-        post_thickness = 0.02
-        goal_depht = 0.18
-        goal_height = 1
-        r = 0.09
-        L = 12
-        m = 0
-
-        # -- Friendly Goalkeeper Area -- #
-        if command.avoid_area:
-            path_planning.add_static_obstacle(
-                (0, field.field_width / 2 - field.penalty_width / 2),
-                field.penalty_depth,
-                field.penalty_width
+        pos = (robot.x, robot.y)
+
+        # Initialize Planner
+        path_planner = self.__planner()
+        path_planner.set_start((robot.x, robot.y))
+        path_planner.set_goal(command.target_pose[:2])
+        path_planner.set_velocity((robot.vx, robot.vy))
+        path_planner.set_map_area((field.field_length, field.field_width))
+
+        if point_in_rect(pos, (0.0, 0.0, field.field_length, field.field_width)):
+            path_planner.add_field_walls(
+                origin=0.0,
+                border=0.0
             )
-        # -- Friendly Goal Posts -- #
-        path_planning.add_static_obstacle(
-            (-r - goal_depht - post_thickness, field.field_width / 2 - r - goal_height / 2),
-            2 * r + post_thickness + goal_depht,
-            2 * r + goal_height
-        )
-        # -- Opponent Goalkeeper Area -- #
-        path_planning.add_static_obstacle(
-            (field.field_length - field.penalty_depth,
-             field.field_width / 2 - field.penalty_width / 2),
-            field.penalty_depth,
-            field.penalty_width
-        )
-        # # -- Opponent Goal Posts -- #
-        # path_planning.add_static_obstacle(
-        #     (-1, -1),
-        #     self._field.fieldLength + 2,
-        #     0.7
-        # )
-        # -- Lower Field Limit -- #
-        path_planning.add_static_obstacle(
-            (-L - m, -L - m),
-            field.field_length + 2 * (m + L),
-            L + r
-        )
-        # -- Right Field Limit -- #
-        path_planning.add_static_obstacle(
-            (field.field_length + m - r, -m),
-            L,
-            field.penalty_width + 2 * m
-        )
-        # -- Upper Field Limit -- #
-        path_planning.add_static_obstacle(
-            (-L - m, field.field_width + m - r),
-            field.field_length + 2 * (m + L),
-            L
-        )
-        # -- Left Field Limit -- #
-        path_planning.add_static_obstacle(
-            (-L - m, -m),
-            L + r,
-            field.field_width + 2 * m
-        )
 
-        # -- Opponent Robots -- #
+        if command.avoid_area and not point_in_rect(pos, path_planner.friendly_area):
+            path_planner.add_friendly_area_walls()
+
+        if not point_in_rect(pos, path_planner.opponent_area):
+            path_planner.add_opp_area_walls()
+
+        obstacles = []
+
+        # Add opponent robots as obstacles
         for opp in command.avoid_opponents:
-            opp = data.opposites[opp]
-            path_planning.add_dynamic_obstacle(opp, 0.2, np.array((opp.vx, opp.vy)))
+            opp_robot = data.opposites[opp]
+            obstacles.append((opp_robot.x, opp_robot.y))
 
-        # -- Friendly Robots -- #
+        # Add friendly robots as obstacles
         for rob in command.avoid_allies:
             if rob == command.id:
                 continue
+            friendly_robot = data.robots[rob]
+            obstacles.append((friendly_robot.x, friendly_robot.y))
 
-            rob = data.robots[rob]
-            path_planning.add_dynamic_obstacle(rob, 0.2, np.array((rob.vx, rob.vy)))
+        path_planner.set_obstacles(obstacles)
 
-        next_point = path_planning.find_path()
+        next_point = path_planner.plan()
 
         dx = next_point[0] - robot.x
         dy = next_point[1] - robot.y

neonfc_ssl/control_layer/path_planning/__init__.py

@@ -1,3 +1,15 @@
-from .base_planner import BasePathPlanner
 from .rrt.rrt_planner import RRTPlanner, RRTStarPlanner
 from .velocity_obstacle.vo_planner import VOPlanner
+
+from typing import TYPE_CHECKING, Type
+if TYPE_CHECKING:
+    from .base_planner import Planner
+
+_planner_list: list[Type['Planner']] = [
+    # Available planners
+    RRTPlanner,
+    RRTStarPlanner,
+    VOPlanner
+]
+
+PLANNERS: dict[str, Type['Planner']] = {p.__name__: p for p in _planner_list}

neonfc_ssl/control_layer/path_planning/base_planner.py

@@ -3,7 +3,7 @@
 import numpy as np
 
 
-class BasePathPlanner(ABC):
+class Planner(ABC):
     def __init__(self):
         self.start = None
         self.goal = None

neonfc_ssl/control_layer/path_planning/rrt/rrt_planner.py

@@ -1,9 +1,9 @@
-from ..base_planner import BasePathPlanner
+from ..base_planner import Planner
 from typing import List, Tuple
 from . import Node, RRT, RRTStar
 
 
-class RRTPlanner(BasePathPlanner):
+class RRTPlanner(Planner):
     def __init__(self, step_size=0.1, max_iter=5000, collision_margin=0.18):
         super().__init__()
         self.step_size = step_size
@@ -17,13 +17,16 @@ def set_start(self, start: Tuple[float, float]):
     def set_goal(self, goal: Tuple[float, float]):
         self.goal = goal
 
-    def set_speed(self, speed: Tuple[float, float]):
-        self.speed = speed
+    def set_velocity(self, velocity: Tuple[float, float]):
+        self.velocity = velocity
 
     def set_obstacles(self, obstacles: List):
         # Convert obstacles to Node objects
         self.obstacles = [Node(obs[0], obs[1]) for obs in obstacles]
 
+    def set_walls(self, walls: List):
+        pass
+
     def set_map_area(self, map_area: Tuple[float, float]):
         self.map_area = map_area
 
@@ -40,8 +43,8 @@ def plan(self) -> List[Tuple[float, float]]:
 
         path = rrt.plan()
 
-        self.path = path if path else [[self.start[0] + self.step_size * self.speed[0],
-                                       self.start[1] + self.step_size * self.speed[1]]]
+        self.path = path if path else [[self.start[0] + self.step_size * self.velocity[0],
+                                       self.start[1] + self.step_size * self.velocity[1]]]
 
         return self.path
 
@@ -88,7 +91,7 @@ def plan(self) -> List[Tuple[float, float]]:
         )
         path = rrt_star.plan()
 
-        self.path = path if len(path) > 0 else [[self.start[0] + self.step_size * self.speed[0],
-                                                 self.start[1] + self.step_size * self.speed[1]]]
+        self.path = path if len(path) > 0 else [[self.start[0] + self.step_size * self.velocity[0],
+                                                 self.start[1] + self.step_size * self.velocity[1]]]
 
         return self.path

neonfc_ssl/control_layer/path_planning/velocity_obstacle/vo_planner.py

@@ -1,10 +1,10 @@
-from ..base_planner import BasePathPlanner
+from ..base_planner import Planner
 from typing import List, Tuple
 from .vo import StarVO
 import numpy as np
 
 
-class VOPlanner(BasePathPlanner):
+class VOPlanner(Planner):
     def __init__(self):
         super().__init__()
         self.star_vo = StarVO(
@@ -13,6 +13,9 @@ def __init__(self):
             vel=(0,0)
         )
 
+        self.map_len = 0.0
+        self.map_wid = 0.0
+
     def set_start(self, start: Tuple[float, float]):
         self.start = start
         self.star_vo.pos = np.array(start, dtype=np.float64)
@@ -37,7 +40,7 @@ def set_walls(self, walls: List):
         self.star_vo.update_walls(walls)
 
     def set_map_area(self, map_area: Tuple[float, float]):
-        pass
+        self.map_len, self.map_wid = map_area
 
     def plan(self) -> List[float]:
         self.path = self.star_vo.pos + self.star_vo.update()
@@ -49,15 +52,59 @@ def update(self, current_state, *args, **kwargs):
     def get_path(self):
         return self.path
 
-    def add_field_walls(self, origin, length, width, border=0.3):
+    @property
+    def friendly_area(self):
+        width = self.map_wid
+
+        origin = (-0.3, width / 2 - 1.0)
+        area_wid = 2.0
+        area_len = 1.3
+
+        return origin[0], origin[1], area_len, area_wid
+
+    @property
+    def opponent_area(self):
+        length = self.map_len
+        width = self.map_wid
+
+        origin = (length - 1.0, width / 2 - 1.0)
+        area_wid = 2.0
+        area_len = 1.3
+
+        return origin[0], origin[1], area_len, area_wid
+
+    def add_friendly_area_walls(self):
+        o_x, o_y, length, width = self.friendly_area
+
+        self.set_walls([
+            [(o_x, o_y), (o_x + length, o_y)],
+            [(o_x, o_y + width), (o_x + length, o_y + width)],
+            [(o_x + length, o_y), (o_x + length, o_y + width)]
+        ])
+
+    def add_opp_area_walls(self):
+        o_x, o_y, length, width = self.opponent_area
+
+        self.set_walls([
+            [(o_x, o_y), (o_x + length, o_y)],
+            [(o_x, o_y + width), (o_x + length, o_y + width)],
+            [(o_x, o_y), (o_x, o_y + width)]
+        ])
+
+    def add_field_walls(self, origin, border=0.3):
+        length = self.map_len
+        width = self.map_wid
+
         x_min = origin - border
         y_min = origin - border
         x_max = origin + length + border
         y_max = origin + width + border
 
-        self.set_walls([
+        walls = [
             [(x_min, y_min), (x_min, y_max)],
             [(x_min, y_max), (x_max, y_max)],
             [(x_max, y_max), (x_max, y_min)],
             [(x_max, y_min), (x_min, y_min)]
-        ])
+        ]
+
+        self.set_walls(walls)

tests/integ/fixture/test_config.toml

@@ -19,6 +19,7 @@ color = "yellow"
 coach = "SimpleCoach"
 
 [Control]
+planner = "VOPlanner"
 
 [OutputLayer]
 host_ip = "127.0.0.1"