path_plan_carla.py

import carla
import math
import heapq
import time
import random
import itertools
import matplotlib.pyplot as plt

# --- Utility Functions ---

def distance(loc1, loc2):
    """Euclidean distance in the XY plane."""
    return math.hypot(loc1.x - loc2.x, loc1.y - loc2.y)


def heuristic(a, b):
    """Straight-line distance heuristic for A*."""
    return distance(a, b)

# --- Global Planner (Waypoint-based A*) ---

def astar(world, start_wp, goal_wp, resolution=3.0):
    """
    A* search over CARLA waypoints, respecting lane direction.
    :param world: carla.World
    :param start_wp: carla.Waypoint
    :param goal_wp: carla.Waypoint
    :param resolution: distance step for neighbor waypoints
    :return: List[carla.Waypoint] representing the path
    """
    counter = itertools.count()
    open_set = []  # heap of (f_score, count, key, waypoint)
    came_from = {}
    g_score = {}
    f_score = {}

    def wp_key(wp):
        loc = wp.transform.location
        return (round(loc.x,1), round(loc.y,1), wp.road_id, wp.lane_id)

    start_key = wp_key(start_wp)
    goal_loc = goal_wp.transform.location
    g_score[start_key] = 0.0
    f_score[start_key] = heuristic(start_wp.transform.location, goal_loc)
    heapq.heappush(open_set, (f_score[start_key], next(counter), start_key, start_wp))
    visited = set()

    while open_set:
        _, _, current_key, current_wp = heapq.heappop(open_set)
        if current_key in visited:
            continue
        visited.add(current_key)

        # goal test
        if distance(current_wp.transform.location, goal_loc) < resolution:
            path = [current_wp]
            key = current_key
            while key in came_from:
                wp = came_from[key]
                path.append(wp)
                key = wp_key(wp)
            path.reverse()
            return path

        # neighbors along lane direction
        for nbr in current_wp.next(resolution):
            nbr_key = wp_key(nbr)
            if nbr_key in visited:
                continue
            tentative_g = g_score[current_key] + distance(current_wp.transform.location,
                                                            nbr.transform.location)
            if tentative_g < g_score.get(nbr_key, float('inf')):
                came_from[nbr_key] = current_wp
                g_score[nbr_key] = tentative_g
                f_score[nbr_key] = tentative_g + heuristic(nbr.transform.location, goal_loc)
                heapq.heappush(open_set, (f_score[nbr_key], next(counter), nbr_key, nbr))

    return []  # no path found

# --- Local Controller (Pure Pursuit) ---

def compute_control(vehicle, target, throttle_gain=0.5, steering_gain=1.0):
    """
    Compute throttle and steering to drive towards a target point.
    :param vehicle: carla.Vehicle
    :param target: carla.Location
    :return: (carla.VehicleControl, reached_boolean)
    """
    transform = vehicle.get_transform()
    loc = transform.location
    yaw = math.radians(transform.rotation.yaw)

    dx = target.x - loc.x
    dy = target.y - loc.y
    desired_yaw = math.atan2(dy, dx)
    yaw_error = (desired_yaw - yaw + math.pi) % (2 * math.pi) - math.pi
    steer = max(-1.0, min(1.0, steering_gain * yaw_error / (math.pi/4)))

    dist = distance(loc, target)
    throttle = throttle_gain if dist > 1.0 else 0.0

    control = carla.VehicleControl(throttle=throttle, steer=steer, brake=0.0)
    done = dist < 0.5
    return control, done

# --- Main Execution ---

def main():
    client = carla.Client('localhost', 2000)
    client.set_timeout(10.0)
    world = client.get_world()

    # Define start transform
    start_transform = carla.Transform(
        carla.Location(x=10.0, y=20.0, z=0.0),
        carla.Rotation(pitch=0.0, yaw=0.0, roll=0.0)
    )
    start_loc = start_transform.location

    # Random goal from spawn points
    spawn_points = world.get_map().get_spawn_points()
    goal_transform = random.choice(spawn_points)
    goal_loc = goal_transform.location
    print(f"Randomly selected goal at ({goal_loc.x:.1f}, {goal_loc.y:.1f}, {goal_loc.z:.1f})")

    # Spawn vehicle safely
    blueprint = world.get_blueprint_library().filter('vehicle.*')[0]
    vehicle = world.try_spawn_actor(blueprint, start_transform)
    if vehicle is None:
        for sp in spawn_points:
            vehicle = world.try_spawn_actor(blueprint, sp)
            if vehicle:
                start_transform = sp
                start_loc = sp.location
                print(f"Spawned at alternative start {start_loc}")
                break
        else:
            print("Failed to spawn vehicle.")
            return

    # Convert start/goal to waypoints
    start_wp = world.get_map().get_waypoint(start_loc,
                                           project_to_road=True,
                                           lane_type=carla.LaneType.Driving)
    goal_wp = world.get_map().get_waypoint(goal_loc,
                                          project_to_road=True,
                                          lane_type=carla.LaneType.Driving)

    # Plan global path
    wp_path = astar(world, start_wp, goal_wp, resolution=3.0)
    if not wp_path:
        print("No path found!")
        return

    # Extract locations
    path = [wp.transform.location for wp in wp_path]

    # Plot path
    xs = [loc.x for loc in path]
    ys = [loc.y for loc in path]
    plt.figure()
    plt.plot(xs, ys, marker='o', linestyle='-')
    plt.title('Global Lane-Aware Path')
    plt.xlabel('X (m)')
    plt.ylabel('Y (m)')
    plt.grid(True)
    plt.show()

    # Visualize in CARLA
    for loc in path:
        world.debug.draw_point(loc + carla.Location(z=1.0),
                               size=0.2,
                               color=carla.Color(0, 255, 0),
                               life_time=120.0)
    print(f"Planned {len(path)} lane-aware waypoints.")

    # Follow path
    idx = 0
    while idx < len(path):
        target = path[idx]
        control, reached = compute_control(vehicle, target)
        vehicle.apply_control(control)
        if reached:
            idx += 1
        world.wait_for_tick()

    print("Arrived at goal!")

if __name__ == '__main__':
    main()