import csv

import math
import heapq
import os
import numpy as np
from controller import Supervisor
import matplotlib.pyplot as plt

class Node:
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
self.g = 0
self.h = 0
self.f = 0

def __lt__(self, other):

return self.f < other.f

def __eq__(self, other):

return self.x == other.x and self.y == other.y

def __hash__(self):
return hash((self.x, self.y))

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, max_bias=0.5):

# Find the road point closest to given coordinates with max_bias limit
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 and dist <= max_bias:
min_dist = dist
nearest = point

# If no point within max_bias was found, find closest overall

if nearest is 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

class LPAStarPlanner:
def __init__(self, road_points):
# Store the valid road points from global planner
self.road_points = set(road_points) # Convert to set for faster lookup
 # g-values: cost from start to vertex
self.g = {}
# rhs-values: one-step lookahead values based on g-values of predecessors
self.rhs = {}
# Priority queue for vertices to be processed
self.U = []
# Start and goal vertices
self.start = None
self.goal = None
# Safety radius around obstacles (m)
self.safety_radius = 3.0
# Maximum allowable bias from road points
self.max_bias = 0.5

def initialize(self, start, goal):

"""Initialize the LPA* planner with start and goal coordinates"""
self.start = start
self.goal = goal

# Clear previous data

self.g = {}
self.rhs = {}
self.U = []

# Find nearest road points if start/goal aren't exactly on road points

start_point = self.find_nearest_road_point(start)
goal_point = self.find_nearest_road_point(goal)

if not start_point or not goal_point:

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

self.start = start_point
self.goal = goal_point

# Initialize all vertices with infinite g-values

for point in self.road_points:
self.g[point] = float('inf')
self.rhs[point] = float('inf')

# Initialize goal with rhs = 0

self.rhs[self.goal] = 0

# Insert goal into priority queue

heapq.heappush(self.U, (self.calculate_key(self.goal), self.goal))
return True

def calculate_key(self, s):

"""Calculate the priority key for vertex s"""
if s not in self.g:
self.g[s] = float('inf')
if s not in self.rhs:
self.rhs[s] = float('inf')

# Key is a tuple with lexicographical ordering

h_val = self.heuristic(s, self.start)
return (min(self.g[s], self.rhs[s]) + h_val, min(self.g[s], self.rhs[s]))

def heuristic(self, a, b):

 """Calculate heuristic distance between two points"""
return math.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)

def find_nearest_road_point(self, point):

"""Find nearest road point within allowed bias"""
if not self.road_points:
return None

min_dist = float('inf')
nearest = None

# First try to find a point within max_bias

for road_point in self.road_points:
dist = math.sqrt((point[0]-road_point[0])**2 + (point[1]-
road_point[1])**2)
if dist < min_dist and dist <= self.max_bias:
min_dist = dist
nearest = road_point

# If no point within max_bias was found, find the absolute closest

if nearest is None:
for road_point in self.road_points:
dist = math.sqrt((point[0]-road_point[0])**2 + (point[1]-
road_point[1])**2)
if dist < min_dist:
min_dist = dist
nearest = road_point

return nearest

def get_predecessors(self, vertex, obstacles=None):

"""Get vertices that can reach the given vertex"""
predecessors = []

# Maximum connection distance (slightly more than diagonal grid distance)

max_dist = self.max_bias * 4

# Check potential road points as predecessors

for road_point in self.road_points:
if road_point == vertex:
continue

dist = math.sqrt((vertex[0]-road_point[0])**2 + (vertex[1]-

road_point[1])**2)

# Only consider points within reasonable distance

if dist <= max_dist:
# Check collision with obstacles
if obstacles:
collision = False
for obs in obstacles:
if self.line_intersects_circle(road_point, vertex, obs,
self.safety_radius):
collision = True
break

if collision:
continue
 predecessors.append(road_point)

return predecessors

def get_successors(self, vertex, obstacles=None):

"""Get vertices that can be reached from the given vertex"""
# For this grid-based planner, successors and predecessors are symmetric
return self.get_predecessors(vertex, obstacles)

def line_intersects_circle(self, p1, p2, circle_center, radius):

"""Check if line segment p1-p2 intersects with circle"""
# Vector from p1 to p2
v = (p2[0] - p1[0], p2[1] - p1[1])
# Vector from p1 to circle center
w = (circle_center[0] - p1[0], circle_center[1] - p1[1])

# Length of line segment squared

len_v_squared = v[0]**2 + v[1]**2

# Avoid division by zero

if len_v_squared == 0:
return math.sqrt(w[0]**2 + w[1]**2) <= radius

# Project w onto v (normalized dot product)

proj = (w[0]*v[0] + w[1]*v[1]) / len_v_squared

# If projection is outside segment, check distance to endpoints

if proj < 0:
return math.sqrt(w[0]**2 + w[1]**2) <= radius
elif proj > 1:
return math.sqrt((circle_center[0]-p2[0])**2 + (circle_center[1]-
p2[1])**2) <= radius

# Calculate closest point on line segment to circle center

closest = (p1[0] + proj*v[0], p1[1] + proj*v[1])

# Check if closest point is within radius

return math.sqrt((circle_center[0]-closest[0])**2 + (circle_center[1]-
closest[1])**2) <= radius

def update_vertex(self, u, obstacles=None):

"""Update vertex u and maintain queue consistency"""
# Skip updating goal vertex
if u == self.goal:
return

# Compute rhs as minimum cost through any successor

min_cost = float('inf')
successors = self.get_successors(u, obstacles)

for succ in successors:

if succ not in self.g:
self.g[succ] = float('inf')

cost = self.heuristic(u, succ) + self.g[succ]

min_cost = min(min_cost, cost)

# Update rhs value

self.rhs[u] = min_cost
 # Remove u from queue if it exists
new_queue = []
for item in self.U:
if item[1] != u:
new_queue.append(item)
self.U = new_queue
heapq.heapify(self.U)

# Add u to queue if locally inconsistent

if self.g.get(u, float('inf')) != self.rhs.get(u, float('inf')):
heapq.heappush(self.U, (self.calculate_key(u), u))

def compute_shortest_path(self, obstacles=None):

"""Compute or update the shortest path"""
if not self.start or not self.goal:
print("Error: Start or goal not initialized")
return False

iterations = 0
max_iterations = 10000 # Prevent infinite loops

# Main LPA* loop

while (self.U and
(iterations < max_iterations) and
((self.U[0][0] < self.calculate_key(self.start)) or
(self.rhs.get(self.start, float('inf')) != self.g.get(self.start,
float('inf'))))):

iterations += 1

# Pop vertex with minimum key

k_old, u = heapq.heappop(self.U)

# Calculate new key

k_new = self.calculate_key(u)

if k_old < k_new:

# Reinsert with updated key
heapq.heappush(self.U, (k_new, u))
elif self.g.get(u, float('inf')) > self.rhs.get(u, float('inf')):
# Make g consistent with rhs
self.g[u] = self.rhs[u]

# Update all predecessors

for pred in self.get_predecessors(u, obstacles):
self.update_vertex(pred, obstacles)
else:
# Handle locally overconsistent vertices
self.g[u] = float('inf')

# Update vertex and its predecessors

self.update_vertex(u, obstacles)
for pred in self.get_predecessors(u, obstacles):
self.update_vertex(pred, obstacles)

print(f"LPA* completed in {iterations} iterations")

return True
 def extract_path(self):
"""Extract the path from start to goal"""
if not self.start or not self.goal:
print("Error: Start or goal not initialized")
return None

if self.g.get(self.start, float('inf')) == float('inf'):

print("No path exists from start to goal")
return None

path = [self.start]
current = self.start

# Maximum path length to prevent infinite loops

max_length = 1000

while current != self.goal and len(path) < max_length:

# Find best next vertex
best_cost = float('inf')
next_vertex = None

for succ in self.get_successors(current):

if succ in self.g:
cost = self.heuristic(current, succ) + self.g[succ]
if cost < best_cost:
best_cost = cost
next_vertex = succ

if next_vertex is None:
print("Path extraction failed - no valid successor found")
break

path.append(next_vertex)
current = next_vertex

print(f"Path extracted with {len(path)} waypoints")

return path

def plan_path(self, start, goal, obstacles=None):

"""Plan a path from start to goal avoiding obstacles"""
success = self.initialize(start, goal)
if not success:
return None

self.compute_shortest_path(obstacles)
return self.extract_path()

class StanleyController:
def __init__(self, k=0.2): # Increased gain parameter for more responsive
steering
self.k = k # Stanley gain parameter
self.prev_steering = 0.0 # For steering smoothing
self.prev_error = 0.0 # For derivative term
self.error_sum = 0.0 # For integral term
self.damping = 0.0 # Reduced damping to make steering more responsive
self.max_integral = 1.0 # Prevent integral windup

def compute_steering(self, car_x, car_y, car_heading, path_x, path_y,

path_heading, speed=1.0):
 try:
# Calculate the cross-track error
dx = path_x - car_x
dy = path_y - car_y

# Cross-track error (transformed to car's frame)

e = dx * math.sin(car_heading) - dy * math.cos(car_heading)

# Heading error
heading_error = path_heading - car_heading
# Normalize to [-pi, pi]
heading_error = (heading_error + math.pi) % (2 * math.pi) - math.pi

# Basic Stanley control law

base_steering = heading_error + math.atan2(self.k * e, max(0.3, speed))

# Add damping component based on error derivative

derivative = e - self.prev_error
self.prev_error = e

# Add minimal integral component for steady-state errors

self.error_sum += e * 0.01
self.error_sum = max(-self.max_integral, min(self.max_integral,
self.error_sum))

# Compute final steering with PID-like components

steering = base_steering - self.damping * derivative + 0.01 *
self.error_sum

# FIX: Reverse the steering direction to correct the turning problem

steering = -steering

# Smooth steering to prevent jerky movement

steering = 0.6 * steering + 0.4 * self.prev_steering
self.prev_steering = steering

# Limit steering angle

max_steer = 0.9 # Increased maximum steering angle for sharper turns
steering = max(-max_steer, min(max_steer, steering))

print(f"Cross-track error: {e:.3f}, Heading error: {heading_error:.3f},

Steering: {steering:.3f}")
return steering
except Exception as e:
print(f"Error computing steering: {str(e)}")
return 0.0 # Safe default on error

def get_yaw_from_supervisor_rotation(rotation_matrix):
"""
Calculate yaw angle from supervisor rotation matrix for X-Y plane movement

Args:
rotation_matrix: 3x3 rotation matrix in row-major order (list of 9
elements)

Returns:
yaw: rotation angle around Z-axis in radians
"""
# For rotation about z-axis, yaw = atan2(R[1,0], R[0,0])
 # In flattened form, R[1,0] is rotation_matrix[3] and R[0,0] is
rotation_matrix[0]
yaw = math.atan2(rotation_matrix[3], rotation_matrix[0])
return yaw

def generate_waypoints(path, spacing=4.0):

"""
Generate waypoints spaced approximately every 4 meters along the path

Args:
path: List of (x,y) tuples representing the original path
spacing: Desired spacing between waypoints in meters

Returns:
new_path: List of (x,y) tuples with approximately uniform spacing
"""
if not path or len(path) < 2:
return path

new_path = [path[0]] # Start with the first point

accumulated_distance = 0.0
last_point = path[0]

for i in range(1, len(path)):

current_point = path[i]
dx = current_point[0] - last_point[0] # X difference
dy = current_point[1] - last_point[1] # Y difference
segment_length = math.sqrt(dx*dx + dy*dy)

# If adding this segment would exceed the spacing

if accumulated_distance + segment_length >= spacing:
# Calculate how far along this segment the next point should be
remaining = spacing - accumulated_distance
ratio = remaining / segment_length

# Interpolate to get the new point

new_x = last_point[0] + ratio * dx
new_y = last_point[1] + ratio * dy
new_point = (new_x, new_y)
new_path.append(new_point)

# Continue adding points along this segment if it's long

remaining_segment = segment_length - remaining
while remaining_segment >= spacing:
remaining += spacing
ratio = remaining / segment_length
new_x = last_point[0] + ratio * dx
new_y = last_point[1] + ratio * dy
new_point = (new_x, new_y)
new_path.append(new_point)
remaining_segment -= spacing

accumulated_distance = remaining_segment
else:
accumulated_distance += segment_length

last_point = current_point

# Ensure last point is included

 if new_path[-1] != path[-1]:
new_path.append(path[-1])

return new_path

class CarController(Supervisor):
def __init__(self):
super().__init__()
self.timestep = int(self.getBasicTimeStep())

# Initialize variables
self.local_planner = None
self.initialization_success = False
self.car = None
self.path = []
self.current_waypoint = 0
self.other_vehicles = []

# Get vehicle node using supervisor

try:
self.car = self.getSelf()
print(f"Controlling vehicle: {self.car.getTypeName()}")

# List all devices for debugging

self.list_all_devices()

# Initialize motors
if not self.initialize_motors():
print("Error: Failed to initialize motors")
return

# Initialize path planner

if not self.initialize_path_planner():
print("Error: Failed to initialize path planner")
return

# Stanley controller with improved parameters

self.stanley = StanleyController(k=0.2)

# Find other vehicles in the scene

self.find_other_vehicles()

# Set initialization success

self.initialization_success = True
print("Car controller initialized successfully")

except Exception as e:
print(f"Error in controller initialization: {str(e)}")

def list_all_devices(self):
try:
n = self.getNumberOfDevices()
print(f"Vehicle has {n} devices:")
for i in range(min(n, 15)): # Print first 15 devices
device = self.getDeviceByIndex(i)
print(f" {i}: {device.getName()} (type: {device.getNodeType()})")
return True
except Exception as e:
print(f"Error listing devices: {str(e)}")
 return False

def initialize_motors(self):
try:
# BMW X5 has separate motors for left and right steering
self.left_steer = self.getDevice('left_steer')
self.right_steer = self.getDevice('right_steer')

if not self.left_steer or not self.right_steer:

print("Error: Steering motors not found")
return False

# Front wheel motors

self.left_front_wheel = self.getDevice('left_front_wheel')
self.right_front_wheel = self.getDevice('right_front_wheel')

if not self.left_front_wheel or not self.right_front_wheel:

print("Error: Front wheel motors not found")
return False

# Initialize steering motors

self.left_steer.setPosition(0.0)
self.right_steer.setPosition(0.0)

# Initialize drive motors (position infinity for velocity control)

self.left_front_wheel.setPosition(float('inf'))
self.right_front_wheel.setPosition(float('inf'))
self.left_front_wheel.setVelocity(0.0)
self.right_front_wheel.setVelocity(0.0)

# Try to get rear wheels if available

try:
self.left_rear_wheel = self.getDevice('left_rear_wheel')
self.right_rear_wheel = self.getDevice('right_rear_wheel')
self.left_rear_wheel.setPosition(float('inf'))
self.right_rear_wheel.setPosition(float('inf'))
self.left_rear_wheel.setVelocity(0.0)
self.right_rear_wheel.setVelocity(0.0)
self.has_rear_motors = True
print("All wheel motors initialized successfully")
except Exception as e:
print(f"Warning: Rear wheel motors not found - {str(e)}")
self.has_rear_motors = False

print("Motors initialized successfully")

return True
except Exception as e:
print(f"Error initializing motors: {str(e)}")
return False

def initialize_path_planner(self):
# Try to load the CSV file with the road grid
try:
# Use raw string to avoid Unicode issues
csv_path = r"road_grid_poits_inflatesobstacles_2m.csv"
print(f"Looking for CSV at: {csv_path}")
self.planner = AStarPlanner(csv_path)
print("Successfully initialized path planner")
return True
 except Exception as e:
print(f"Error in planner initialization: {str(e)}")
return False

def set_steering(self, angle):

"""Set steering angle for both front wheels with verification"""
try:
if not self.left_steer or not self.right_steer:
print("Error: Steering motors not initialized")
return False

# Apply steering commands

self.left_steer.setPosition(angle)
self.right_steer.setPosition(angle)

# Log the command

print(f"Steering command issued: {angle:.4f} radians")
return True
except Exception as e:
print(f"Error setting steering: {str(e)}")
return False

def set_velocity(self, speed):

"""Set velocity for all wheels with verification"""
try:
if not self.left_front_wheel or not self.right_front_wheel:
print("Error: Front wheel motors not initialized")
return False

# Set front wheel velocities

self.left_front_wheel.setVelocity(speed)
self.right_front_wheel.setVelocity(speed)

# Set rear wheel velocities if available

if hasattr(self, 'has_rear_motors') and self.has_rear_motors:
self.left_rear_wheel.setVelocity(speed)
self.right_rear_wheel.setVelocity(speed)

print(f"Velocity set to {speed:.2f}")

return True
except Exception as e:
print(f"Error setting velocity: {str(e)}")
return False

def get_position_and_orientation(self):
"""Get position and orientation from supervisor API"""
try:
# Get position
pos = self.car.getPosition()
if pos is None:
print("Warning: Failed to get position from supervisor")
return (0, 0), 0.0

# Get orientation matrix from supervisor

orientation_matrix = self.car.getOrientation()
if orientation_matrix is None:
print("Warning: Failed to get orientation from supervisor")
return (pos[0], pos[1]), 0.0
 # Extract yaw angle (rotation around Z axis)
yaw = get_yaw_from_supervisor_rotation(orientation_matrix)

return (pos[0], pos[1]), yaw

except Exception as e:
print(f"Error getting position and orientation: {str(e)}")
return (0, 0), 0.0

def get_path_heading(self, idx):

"""Calculate heading at a waypoint using the next waypoint"""
try:
if not self.path or len(self.path) <= 1:
return 0.0

if idx >= len(self.path):

idx = len(self.path) - 1

# If at last waypoint, use the same heading as before

if idx + 1 >= len(self.path):
if idx > 0:
prev = self.path[idx-1]
curr = self.path[idx]
return math.atan