Astar.py

import random
import time


class Astar:
    def __init__(self):
        self.map = None
        self.map_width = 7
        self.map_height = 7
        self.start = (0, 0)
        self.goal = (self.map_height - 1, self.map_width - 1)
        self.build_map()
        self.path = []
        self.direction = []

        self.robot_height = 0
        self.robot_width = 0

    def build_physical_map(self):
        self.map = [
            ['S', '.', '.', '#', '.', '.', '.'],
            ['#', '#', '.', '#', '.', '#', '.'],
            ['.', '#', '.', '#', '.', '#', '.'],
            ['.', '.', '.', '#', '.', '.', '.'],
            ['.', '#', '#', '#', '.', '#', '.'],
            ['.', '#', '.', '#', '.', '#', '.'],
            ['.', '.', '.', '.', '.', '#', 'G'],
        ]

    def build_map(self):
        self.map = []
        for i in range(self.map_height):
            inner_grid = []
            for j in range(self.map_width):
                r = random.random()
                if r < 0.5:
                    inner_grid.append('#')
                else:
                    inner_grid.append(".")
            self.map.append(inner_grid)

        self.map[self.start[0]][self.start[1]] = 'S'
        self.map[self.goal[0]][self.goal[1]] = 'G'


        if not self.is_solvable(self.start, self.goal):
            # self.build_map()
            self.build_physical_map()

    def print_map(self):
        print()
        for index, row in enumerate(self.map):
            display_row = list(row)
            if index == self.robot_height:
                display_row[self.robot_width] = 'R'
            print(index, display_row)

    def robot_traversal(self):
        self.print_map() # Initial draw

        for points in self.path:
            # print(points)
            self.robot_height = points[0]
            self.robot_width = points[1]
            self.print_map()
            time.sleep(0.4)

        print("Traversal Complete!")

    def is_solvable(self, start, end):
        visited = []
        to_visit = [start]
        while to_visit:
            current = to_visit.pop(0)
            visited.append(current)

            if current == end:
                return True

            UP = (current[0] - 1, current[1])
            RIGHT = (current[0], current[1] + 1)
            DOWN = (current[0] + 1, current[1])
            LEFT = (current[0], current[1] - 1)

            DIRECTIONS = [UP, RIGHT, DOWN, LEFT]

            for direction in DIRECTIONS:
                if 0 <= direction[0] <= self.map_height - 1 and 0 <= direction[1] <= self.map_width - 1 and self.map[direction[0]][direction[1]] != '#':
                    if direction not in visited:
                        to_visit.append(direction)
        return False

    def heuristic(self, current, goal):
        return abs(current[0] - goal[0]) + abs(current[1] - goal[1])

    def astar(self):
        # g_score tracks actual cost from start to each cell
        g_score = {}
        g_score[self.start] = 0

        # open list stores (f_score, node) pairs
        open_list = []
        open_list.append((self.heuristic(self.start, self.goal), self.start))

        came_from = {}

        while open_list:
            open_list.sort()
            f, current = open_list.pop(0)

            if current == self.goal:
                path = []
                while current != self.start:
                    path.append(current)
                    current = came_from[current]
                path.append(self.start)
                path.reverse()
                self.path = path
                print("Path found:", self.path)

                self.robot_traversal()

                start = path[0]
                for p1, p2 in path[1:]:
                    next_index = (p1, p2)
                    sub = (next_index[0] - start[0], next_index[1] - start[1])
                    print(sub)
                    start = next_index

                    if sub[0] == 1:
                        self.direction.append('D')
                    elif sub[1] == 1:
                        self.direction.append('R')
                    elif sub[0] == -1:
                        self.direction.append('U')
                    elif sub[1] == -1:
                        self.direction.append('L')

                print()
                print(self.direction)
                return path

            # Explore neighbors
            UP = (current[0] - 1, current[1])
            RIGHT = (current[0], current[1] + 1)
            DOWN = (current[0] + 1, current[1])
            LEFT = (current[0], current[1] - 1)


            for neighbor in [UP, RIGHT, DOWN, LEFT]:
                # boundary check
                if not (0 <= neighbor[0] <= self.map_height - 1 and 0 <= neighbor[1] <= self.map_width - 1):
                    continue
                # wall check
                if self.map[neighbor[0]][neighbor[1]] == '#':
                    continue

                # g score for this neighbor is current g + 1 step
                new_g = g_score[current] + 1

                # only add if we found a better path to this neighbor

                if neighbor not in g_score or new_g < g_score[neighbor]:
                    g_score[neighbor] = new_g
                    f_score = new_g + self.heuristic(neighbor, self.goal)
                    open_list.append((f_score, neighbor))
                    came_from[neighbor] = current

        return False

    def see_map(self):
        for index, num in enumerate(self.map):
            print(index, num)

if __name__ == "__main__":
    A = Astar()
    A.astar()