background image with know cords not scaling properly when same gri... (2024)

import matplotlib.pyplot as plt

import numpy as np

from PIL import Image

import os

from matplotlib.patches import Rectangle

from matplotlib.widgets import Button

# Directory path

dir_path = 'MY_DIR/'

# Load background image

print("Loading background image...")

background_image_path = os.path.join(dir_path, 'MY_IMAGE')

background_image = Image.open(background_image_path)

# Load grid size and offsets from file

print("Loading grid size, obstacle offset, and image offset...")

size_file_path = os.path.join(dir_path, 'size.txt')

with open(size_file_path, 'r') as file:

grid_size_x, grid_size_y = map(int, file.readline().strip().split(','))

obstacle_offset_x, obstacle_offset_y = map(float, file.readline().strip().split(','))

image_offset_x, image_offset_y = map(float, file.readline().strip().split(','))

print(f"Grid size: {grid_size_x}, {grid_size_y}")

print(f"Obstacle offset: {obstacle_offset_x}, {obstacle_offset_y}")

print(f"Image offset: {image_offset_x}, {image_offset_y}")

# Function to load obstacles from B&W image

def load_obstacles_from_image(image_path, grid_size_x, grid_size_y):

print("Loading obstacles from B&W image...")

bw_image = Image.open(image_path).convert('L')

bw_image = bw_image.resize((grid_size_x, grid_size_y))

bw_array = np.array(bw_image)

obstacles = []

for y in range(grid_size_y):

for x in range(grid_size_x):

if bw_array[y, x] < 128: # Assuming obstacles are black (value < 128)

obstacles.append((x + 1, y + 1)) # Adjust for grid origin (1,1)

return list(set((round(x), round(y)) for x, y in obstacles))

# Function to validate if obstacles file contains valid coordinates

def validate_obstacles_file(file_path):

with open(file_path, 'r') as file:

for line in file:

if line.strip():

try:

x, y = map(int, line.strip().split(','))

except ValueError:

return False

return True

# Load obstacles from file or image

obstacle_file_path = os.path.join(dir_path, 'obstacles.txt')

if os.path.exists(obstacle_file_path) and validate_obstacles_file(obstacle_file_path):

print("Loading obstacles from obstacles.txt...")

with open(obstacle_file_path, 'r') as file:

obstacles = [tuple(map(int, line.strip().split(','))) for line in file if line.strip()]

else:

obstacles = load_obstacles_from_image(os.path.join(dir_path, 'obstacles_bw.png'), grid_size_x, grid_size_y)

with open(obstacle_file_path, 'w') as file:

for x, y in obstacles:

file.write(f"{x},{y}\n")

print(f"Loaded {len(obstacles)} obstacles")

# Apply obstacle offset

obstacles = [(x + obstacle_offset_x, y + obstacle_offset_y) for x, y in obstacles]

# Load points of interest (POI) from file

print("Loading points of interest...")

poi_file_path = os.path.join(dir_path, 'poi.txt')

poi_points = []

with open(poi_file_path, 'r') as file:

for line in file:

x, y = map(float, line.strip().split(','))

poi_points.append((x, y))

print(f"Loaded {len(poi_points)} points of interest (POI)")

# Create maze for pathfinding

def create_maze(grid_size_x, grid_size_y, obstacles):

print("Creating maze...")

maze = np.zeros((grid_size_y + 1, grid_size_x + 1)) # Adjust for grid origin (1,1)

for x, y in obstacles:

maze[int(y)][int(x)] = 1

return maze

maze = create_maze(grid_size_x, grid_size_y, obstacles)

print("Maze created.")

print("Maze obstacles:")

print(np.argwhere(maze == 1))

# A* pathfinding algorithm

class Node:

def __init__(self, parent=None, position=None):

self.parent = parent

self.position = position

self.g = 0

self.h = 0

self.f = 0

def __eq__(self, other):

return self.position == other.position

def astar(maze, start, end):

print("Starting A* algorithm...")

start_node = Node(None, start)

end_node = Node(None, end)

open_list = []

closed_list = []

open_list.append(start_node)

iterations = 0

max_iterations = 10000 # Limit iterations for debugging

while open_list:

iterations += 1

if iterations > max_iterations:

print("Reached maximum iterations, stopping to avoid infinite loop.")

break

# Get the current node

current_node = open_list[0]

current_index = 0

for index, item in enumerate(open_list):

if item.f < current_node.f:

current_node = item

current_index = index

# Pop current off open list, add to closed list

open_list.pop(current_index)

closed_list.append(current_node)

# Found the goal

if current_node == end_node:

path = []

current = current_node

while current is not None:

path.append(current.position)

current = current.parent

print("Path found.")

return path[::-1] # Return reversed path

# Generate children

children = []

for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)]: # Adjacent squares

# Get node position

node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1])

# Make sure within range

if node_position[0] > (len(maze) - 1) or node_position[0] < 0 or node_position[1] > (len(maze[len(maze) - 1]) - 1) or node_position[1] < 0:

continue

# Make sure walkable terrain

if maze[node_position[1]][node_position[0]] != 0: # Note the inversion of indices here

continue

# Create new node

new_node = Node(current_node, node_position)

# Append

children.append(new_node)

# Loop through children

for child in children:

# Child is on the closed list

if child in closed_list:

continue

# Create the f, g, and h values

child.g = current_node.g + 1

child.h = ((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2)

child.f = child.g + child.h

# Child is already in the open list

if any(open_node for open_node in open_list if child == open_node and child.g > open_node.g):

continue

# Add the child to the open list

open_list.append(child)

if iterations % 1000 == 0:

print(f"Iteration {iterations}: Open list size: {len(open_list)}, Closed list size: {len(closed_list)}")

print("No path found.")

return []

def plot_path(ax, path):

if path:

path_x, path_y = zip(*path)

ax.plot(path_x, path_y, color='blue', linewidth=2, label='Path')

# Initial start and end points for testing

start_point = (255, 233)

end_point = (240, 240)

start_point_maze = (int(start_point[0]), int(start_point[1])) # Adjust for maze coordinates

end_point_maze = (int(end_point[0]), int(end_point[1])) # Adjust for maze coordinates

print(f"Running A* from {start_point_maze} to {end_point_maze}...")

path = astar(maze, start_point_maze, end_point_maze)

# Plot the results

print("Plotting results...")

fig, ax = plt.subplots(figsize=(10, 10))

ax.imshow(background_image, extent=[image_offset_x, grid_size_x + image_offset_x, grid_size_y + image_offset_y, image_offset_y], origin='upper')

obstacles_patches = [] # List to store obstacle patches

if obstacles:

for (x, y) in obstacles:

rect = Rectangle((x - 0.25, y - 0.25), 0.5, 0.5, linewidth=1, edgecolor='red', facecolor='red')

obstacles_patches.append(rect)

ax.add_patch(rect)

if poi_points:

px, py = zip(*poi_points)

ax.scatter(px, py, c='blue', s=10, label='POIs')

plot_path(ax, path)

ax.scatter(start_point[0], start_point[1], c='green', s=50, label='Start')

ax.scatter(end_point[0], end_point[1], c='purple', s=50, label='End')

ax.set_title('Background Image with Obstacles, POIs, and Path')

ax.legend()

# Toggle obstacles visibility

def toggle_obstacles(event):

for patch in obstacles_patches:

patch.set_visible(not patch.get_visible())

plt.draw()

# Add button to toggle obstacles

ax_button = plt.axes([0.8, 0.05, 0.1, 0.075])

button = Button(ax_button, 'Toggle Obstacles')

button.on_clicked(toggle_obstacles)

plt.show()

print("Finished plotting.")