import matplotlib.pyplot as plt
import numpy as np
from collections import deque
# Define the four possible movement directions: up, down, left, right
MOVES = [(-1, 0), (1, 0), (0, -1), (0, 1)]
class FSSP_BFS:
def __init__(self, grid, start, goal):
self.grid = grid
self.start = start
self.goal = goal
self.rows = len(grid)
self.cols = len(grid[0])
# Check if a position is within grid bounds and not a blocked cell
def is_valid(self, position):
r, c = position
return 0 <= r < self.rows and 0 <= c < self.cols and self.grid[r][c] == 0
# Breadth-First Search to find the shortest path from start to goal
def bfs(self):
queue = deque([(self.start, [self.start])]) # (current position, path)
visited = set([self.start])
while queue:
current, path = queue.popleft()
# If the current position is the goal, return the path
if current == self.goal:
return path
# Explore all possible moves (up, down, left, right)
for move in MOVES:
next_r, next_c = current[0] + move[0], current[1] + move[1]
next_position = (next_r, next_c)
if self.is_valid(next_position) and next_position not in visited:
visited.add(next_position)
queue.append((next_position, path + [next_position]))
return None # Return None if there is no path to the goal
# Function to visualize the grid and the path with enhanced styling
def visualize(self, path):
grid_np = np.array(self.grid)
# Create a figure and axis
fig, ax = plt.subplots(figsize=(8, 8))
ax.imshow(grid_np, cmap='Greys', alpha=0.8)
# Mark start and goal with distinct symbols
ax.text(self.start[1], self.start[0], 'S', color='green', fontsize=18, fontweight='bold', ha='center', va='center')
ax.text(self.goal[1], self.goal[0], 'G', color='red', fontsize=18, fontweight='bold', ha='center', va='center')
# Plot the path if found
if path:
path_np = np.array(path)
ax.plot(path_np[:, 1], path_np[:, 0], color='blue', linewidth=2.5, marker='o', markersize=10, markerfacecolor='yellow', label='Path')
# Style gridlines and labels
ax.set_xticks(np.arange(self.cols))
ax.set_yticks(np.arange(self.rows))
ax.set_xticklabels(np.arange(self.cols))
ax.set_yticklabels(np.arange(self.rows))
ax.grid(which='both', color='black', linewidth=1.5)
# Add title and make it visually appealing
plt.title("Enhanced Grid and Path Visualization", fontsize=16, fontweight='bold')
plt.tight_layout()
plt.show()
# Example usage:
# Define the grid (0 = free cell, 1 = obstacle)
grid = [
[0, 0, 0, 1, 0],
[0, 1, 0, 1, 0],
[0, 1, 0, 0, 0],
[0, 0, 0, 1, 0],
[1, 1, 0, 0, 0]
]
# Start position (row, col) and goal position
start = (0, 0)
goal = (4, 4)
# Create the FSSP_BFS object and run the search
planner = FSSP_BFS(grid, start, goal)
path = planner.bfs()
if path:
print(f"Path found: {path}")
# Visualize the path with enhanced styling
planner.visualize(path)
else:
print("No path found")