formatting p1

Author: SchwartzCode

…imeBasedPathPlanning/moving_obstacles.py …PathPlanning/GridWithDynamicObstacles.pyPathPlanning/TimeBasedPathPlanning/moving_obstacles.py renamed to PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py PathPlanning/TimeBasedPathPlanning/moving_obstacles.py renamed to PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py

@@ -1,6 +1,8 @@
 import numpy as np
 import matplotlib.pyplot as plt
 from enum import Enum
+
+
 class Position:
     x: int
     y: int
@@ -15,12 +17,16 @@ def as_ndarray(self) -> np.ndarray[int, int]:
     def __add__(self, other):
         if isinstance(other, Position):
             return Position(self.x + other.x, self.y + other.y)
-        raise NotImplementedError(f"Addition not supported for Position and {type(other)}")
+        raise NotImplementedError(
+            f"Addition not supported for Position and {type(other)}"
+        )
 
     def __sub__(self, other):
         if isinstance(other, Position):
             return Position(self.x - other.x, self.y - other.y)
-        raise NotImplementedError(f"Subtraction not supported for Position and {type(other)}")
+        raise NotImplementedError(
+            f"Subtraction not supported for Position and {type(other)}"
+        )
 
     def __eq__(self, other):
         if isinstance(other, Position):
@@ -30,12 +36,14 @@ def __eq__(self, other):
     def __repr__(self):
         return f"Position({self.x}, {self.y})"
 
+
 class ObstacleArrangement(Enum):
     # Random obstacle positions and movements
     RANDOM = 0
     # Obstacles start in a line in y at center of grid and move side-to-side in x
     ARRANGEMENT1 = 1
 
+
 class Grid:
     # Set in constructor
     grid_size = None
@@ -50,7 +58,14 @@ class Grid:
     # Logging control
     verbose = False
 
-    def __init__(self, grid_size: np.ndarray[int, int], num_obstacles: int = 40, obstacle_avoid_points: list[Position] = [], obstacle_arrangement: ObstacleArrangement = ObstacleArrangement.RANDOM,  time_limit: int = 100):
+    def __init__(
+        self,
+        grid_size: np.ndarray[int, int],
+        num_obstacles: int = 40,
+        obstacle_avoid_points: list[Position] = [],
+        obstacle_arrangement: ObstacleArrangement = ObstacleArrangement.RANDOM,
+        time_limit: int = 100,
+    ):
         self.obstacle_avoid_points = obstacle_avoid_points
         self.time_limit = time_limit
         self.grid_size = grid_size
@@ -64,17 +79,18 @@ def __init__(self, grid_size: np.ndarray[int, int], num_obstacles: int = 40, obs
         elif obstacle_arrangement == ObstacleArrangement.ARRANGEMENT1:
             self.obstacle_paths = self.obstacle_arrangement_1(num_obstacles)
 
-        for (i, path) in enumerate(self.obstacle_paths):
-            obs_idx = i + 1 # avoid using 0 - that indicates free space in the grid
-            for (t, position) in enumerate(path):
+        for i, path in enumerate(self.obstacle_paths):
+            obs_idx = i + 1  # avoid using 0 - that indicates free space in the grid
+            for t, position in enumerate(path):
                 # Reserve old & new position at this time step
                 if t > 0:
-                    self.grid[path[t-1].x, path[t-1].y, t] = obs_idx
+                    self.grid[path[t - 1].x, path[t - 1].y, t] = obs_idx
                 self.grid[position.x, position.y, t] = obs_idx
 
     """
     Generate dynamic obstacles that move around the grid. Initial positions and movements are random
     """
+
     def generate_dynamic_obstacles(self, obs_count: int) -> list[list[Position]]:
         obstacle_paths = []
         for _obs_idx in (0, obs_count):
@@ -90,10 +106,18 @@ def generate_dynamic_obstacles(self, obs_count: int) -> list[list[Position]]:
             # Encourage obstacles to mostly stay in place - too much movement leads to chaotic planning scenarios
             # that are not fun to watch
             weights = [0.05, 0.05, 0.05, 0.05, 0.8]
-            diffs = [Position(0, 1), Position(0, -1), Position(1, 0), Position(-1, 0), Position(0, 0)]
-
-            for t in range(1, self.time_limit-1):
-                sampled_indices = np.random.choice(len(diffs), size=5, replace=False, p=weights)
+            diffs = [
+                Position(0, 1),
+                Position(0, -1),
+                Position(1, 0),
+                Position(-1, 0),
+                Position(0, 0),
+            ]
+
+            for t in range(1, self.time_limit - 1):
+                sampled_indices = np.random.choice(
+                    len(diffs), size=5, replace=False, p=weights
+                )
                 rand_diffs = [diffs[i] for i in sampled_indices]
 
                 valid_position = None
@@ -122,6 +146,7 @@ def generate_dynamic_obstacles(self, obs_count: int) -> list[list[Position]]:
     Bottom half start moving right, top half start moving left. If `obs_count` is less than the length of
     the grid, only the first `obs_count` obstacles will be generated.
     """
+
     def obstacle_arrangement_1(self, obs_count: int) -> list[list[Position]]:
         obstacle_paths = []
         half_grid_x = self.grid_size[0] // 2
@@ -132,17 +157,21 @@ def obstacle_arrangement_1(self, obs_count: int) -> list[list[Position]]:
             position = Position(half_grid_x, y_idx)
             path = [position]
 
-            for t in range(1, self.time_limit-1):
+            for t in range(1, self.time_limit - 1):
                 # sit in place every other time step
                 if t % 2 == 0:
                     path.append(position)
                     continue
 
                 # first check if we should switch direction (at edge of grid)
-                if (moving_right and position.x == self.grid_size[0] - 1) or (not moving_right and position.x == 0):
+                if (moving_right and position.x == self.grid_size[0] - 1) or (
+                    not moving_right and position.x == 0
+                ):
                     moving_right = not moving_right
                 # step in direction
-                position = Position(position.x + (1 if moving_right else -1), position.y)
+                position = Position(
+                    position.x + (1 if moving_right else -1), position.y
+                )
                 path.append(position)
 
             obstacle_paths.append(path)
@@ -159,8 +188,8 @@ def obstacle_arrangement_1(self, obs_count: int) -> list[list[Position]]:
     output:
         bool: True if position/time combination is valid, False otherwise
     """
-    def valid_position(self, position: Position, t: int) -> bool:
 
+    def valid_position(self, position: Position, t: int) -> bool:
         # Check if new position is in grid
         if not self.inside_grid_bounds(position):
             return False
@@ -171,62 +200,87 @@ def valid_position(self, position: Position, t: int) -> bool:
     """
     Returns True if the given position is valid at time t and is not in the set of obstacle_avoid_points
     """
+
     def valid_obstacle_position(self, position: Position, t: int) -> bool:
-        return self.valid_position(position, t) and position not in self.obstacle_avoid_points
+        return (
+            self.valid_position(position, t)
+            and position not in self.obstacle_avoid_points
+        )
 
     """
     Returns True if the given position is within the grid's boundaries
     """
+
     def inside_grid_bounds(self, position: Position) -> bool:
-        return position.x >= 0 and position.x < self.grid_size[0] and position.y >= 0 and position.y < self.grid_size[1]
+        return (
+            position.x >= 0
+            and position.x < self.grid_size[0]
+            and position.y >= 0
+            and position.y < self.grid_size[1]
+        )
 
     """
     Sample a random position that is within the grid's boundaries
 
     output:
         np.ndarray[int, int]: (x, y) position
     """
+
     def sample_random_position(self) -> Position:
-        return Position(np.random.randint(0, self.grid_size[0]), np.random.randint(0, self.grid_size[1]))
+        return Position(
+            np.random.randint(0, self.grid_size[0]),
+            np.random.randint(0, self.grid_size[1]),
+        )
 
     """
     Returns a tuple of (x_positions, y_positions) of the obstacles at time t
     """
-    def get_obstacle_positions_at_time(self, t: int) -> tuple[list[int], list[int]]:
 
+    def get_obstacle_positions_at_time(self, t: int) -> tuple[list[int], list[int]]:
         x_positions = []
         y_positions = []
         for obs_path in self.obstacle_paths:
             x_positions.append(obs_path[t].x)
             y_positions.append(obs_path[t].y)
         return (x_positions, y_positions)
 
+
 show_animation = True
 
+
 def main():
-    grid = Grid(np.array([11, 11]), num_obstacles=10, obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1)
+    grid = Grid(
+        np.array([11, 11]),
+        num_obstacles=10,
+        obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+    )
 
     if not show_animation:
         return
 
     fig = plt.figure(figsize=(8, 7))
-    ax = fig.add_subplot(autoscale_on=False, xlim=(0, grid.grid_size[0]-1), ylim=(0, grid.grid_size[1]-1))
-    ax.set_aspect('equal')
+    ax = fig.add_subplot(
+        autoscale_on=False,
+        xlim=(0, grid.grid_size[0] - 1),
+        ylim=(0, grid.grid_size[1] - 1),
+    )
+    ax.set_aspect("equal")
     ax.grid()
     ax.set_xticks(np.arange(0, 11, 1))
     ax.set_yticks(np.arange(0, 11, 1))
-    obs_points, = ax.plot([], [], 'ro', ms=15)
+    (obs_points,) = ax.plot([], [], "ro", ms=15)
 
     # for stopping simulation with the esc key.
-    plt.gcf().canvas.mpl_connect('key_release_event',
-        lambda event: [exit(
-            0) if event.key == 'escape' else None])
+    plt.gcf().canvas.mpl_connect(
+        "key_release_event", lambda event: [exit(0) if event.key == "escape" else None]
+    )
 
     for i in range(0, grid.time_limit - 1):
         obs_positions = grid.get_obstacle_positions_at_time(i)
         obs_points.set_data(obs_positions[0], obs_positions[1])
         plt.pause(0.2)
     plt.show()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()

PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py

@@ -8,7 +8,7 @@
 from __future__ import annotations # For typehints of a class within itself
 import numpy as np
 import matplotlib.pyplot as plt
-from PathPlanning.TimeBasedPathPlanning.moving_obstacles import Grid, ObstacleArrangement, Position
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import Grid, ObstacleArrangement, Position
 import heapq
 from collections.abc import Generator
 import random

tests/test_space_time_astar.py

@@ -1,13 +1,21 @@
-from PathPlanning.TimeBasedPathPlanning.moving_obstacles import Grid, ObstacleArrangement, Position
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+    Grid,
+    ObstacleArrangement,
+    Position,
+)
 from PathPlanning.TimeBasedPathPlanning import SpaceTimeAStar as m
 import numpy as np
 import conftest
 
+
 def test_1():
     start = Position(1, 11)
     goal = Position(19, 19)
     grid_side_length = 21
-    grid = Grid(np.array([grid_side_length, grid_side_length]), obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1)
+    grid = Grid(
+        np.array([grid_side_length, grid_side_length]),
+        obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+    )
 
     m.show_animation = False
     planner = m.TimeBasedAStar(grid, start, goal)
@@ -20,5 +28,6 @@ def test_1():
     # path should end at the goal
     assert path.path[-1].position == goal
 
-if __name__ == '__main__':
-    conftest.run_this_test(__file__)
+
+if __name__ == "__main__":
+    conftest.run_this_test(__file__)