Scribd
Upload
4 views3 pages

Class AStarPlanner

The AStarPlanner class implements an A* pathfinding algorithm using road points loaded from a CSV file. It includes methods for loading the map, finding neighbors, calculating heuristics, and planning a path from a start to a goal point. The class handles errors during loading and pathfinding, providing feedback on the process.

Uploaded by

Duddu Pragna Sai Teja me22b122
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
4 views3 pages

Class AStarPlanner

The AStarPlanner class implements an A* pathfinding algorithm using road points loaded from a CSV file. It includes methods for loading the map, finding neighbors, calculating heuristics, and planning a path from a start to a goal point. The class handles errors during loading and pathfinding, providing feedback on the process.

Uploaded by

Duddu Pragna Sai Teja me22b122
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 3

class AStarPlanner:

def __init__(self, csv_file):


self.road_points = set()
self.load_map(csv_file)
print(f"Map loaded with {len(self.road_points)} road points")

def load_map(self, csv_file):


try:
with open(csv_file, 'r') as file:
reader = csv.reader(file)
header = next(reader) # Skip header
for row in reader:
if len(row) >= 2: # Ensure we have y,x format
try:
y = int(row[0])
x = int(row[1])
self.road_points.add((x, y))
except ValueError:
continue
print("Map loading successful")
return True
except Exception as e:
print(f"Error loading CSV: {str(e)}")
return False

def get_neighbors(self, node):


# Check 8 surrounding grid cells
neighbors = []
for dx, dy in [(0,1), (1,0), (0,-1), (-1,0), (1,1), (1,-1), (-1,1), (-1,-
1)]:
nx, ny = node.x + dx, node.y + dy
if (nx, ny) in self.road_points:
neighbors.append(Node(nx, ny))
return neighbors

def heuristic(self, a, b):


# Manhattan distance
return abs(a.x - b.x) + abs(a.y - b.y)

def find_nearest_road_point(self, coords):


# Find the road point closest to given coordinates
if not self.road_points:
print("Error: No road points loaded")
return None

min_dist = float('inf')
nearest = None

for point in self.road_points:


dist = math.sqrt((coords[0]-point[0])**2 + (coords[1]-point[1])**2)
if dist < min_dist:
min_dist = dist
nearest = point

if nearest:
print(f"Found nearest road point {nearest} at distance {min_dist:.2f}")
return nearest
else:
print("Error: Could not find nearest road point")
return None

def plan(self, start_coords, goal_coords):


try:
# Find closest road points to start/goal coordinates
start_point = self.find_nearest_road_point(start_coords)
goal_point = self.find_nearest_road_point(goal_coords)

if not start_point or not goal_point:


print("Error: Start or goal not near any road point")
return None

start = Node(start_point[0], start_point[1])


goal = Node(goal_point[0], goal_point[1])

print(f"Planning path from {start_point} to {goal_point}")

open_set = []
open_set_hash = set()
closed_set = set()

heapq.heappush(open_set, start)
open_set_hash.add((start.x, start.y))

# Maximum iterations to prevent infinite loops


max_iterations = 100000
iterations = 0

while open_set and iterations < max_iterations:


iterations += 1

current = heapq.heappop(open_set)
open_set_hash.remove((current.x, current.y))

# Goal reached
if current.x == goal.x and current.y == goal.y:
path = []
while current:
path.append((current.x, current.y))
current = current.parent
print(f"Path found with {len(path)} points after {iterations}
iterations")
return path[::-1] # Return reversed path

closed_set.add((current.x, current.y))

for neighbor in self.get_neighbors(current):


if (neighbor.x, neighbor.y) in closed_set:
continue

# Calculate g score for this neighbor


tentative_g = current.g + math.sqrt((current.x-neighbor.x)**2 +
(current.y-neighbor.y)**2)

if (neighbor.x, neighbor.y) in open_set_hash:


# Check if this path to neighbor is better than previous
one
for open_node in open_set:
if open_node.x == neighbor.x and open_node.y ==
neighbor.y:
if tentative_g < open_node.g:
open_node.g = tentative_g
open_node.f = tentative_g + open_node.h
open_node.parent = current
break
else:
# Add new node to open set
neighbor.g = tentative_g
neighbor.h = self.heuristic(neighbor, goal)
neighbor.f = neighbor.g + neighbor.h
neighbor.parent = current
heapq.heappush(open_set, neighbor)
open_set_hash.add((neighbor.x, neighbor.y))

print(f"No path found after {iterations} iterations")


return None
except Exception as e:
print(f"Error during path planning: {str(e)}")
return None

You might also like