control.py

import carla
import numpy as np
import  time
import math
import pandas as pd
import scipy
import matplotlib.pyplot as plt

# PID Controller
class PIDController:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.prev_error = 0
        self.integral = 0

    def control(self, target, current, dt):
        error = target - current
        self.integral += error * dt
        derivative = (error - self.prev_error) / dt
        self.prev_error = error
        return self.kp * error + self.ki * self.integral + self.kd * derivative

class pure_pursuit:
    def __init__(self, wheelbase, min_lookahead, max_lookahead, k_v):
        self.wheelbase = wheelbase
        self.min_lookahead = min_lookahead
        self.max_lookahead = max_lookahead
        self.k_v = k_v  

    def get_lookahead_distance(self, speed):
        return np.clip(self.k_v * speed, self.min_lookahead, self.max_lookahead)

    def control(self, current_pose, target_waypoint, current_speed):
        x, y, yaw = current_pose[:3]
        target_x, target_y, target_yaw = target_waypoint

        lookahead_distance = self.get_lookahead_distance(current_speed)
        dist = math.sqrt((target_x - x) ** 2 + (target_y - y) ** 2)

        if dist < lookahead_distance:
            return 0  

        alpha = math.atan2(target_y - y, target_x - x) - yaw
        alpha = math.atan2(math.sin(alpha), math.cos(alpha))  

        steering_angle = math.atan2(2 * self.wheelbase * math.sin(alpha), lookahead_distance)
        return steering_angle
    

class LQRController:
    def __init__(self, wheelbase, Q=None, R=None):
        self.wheelbase = wheelbase  # Vehicle's wheelbase
        
        # Weight matrices (tunable)
        self.Q = Q if Q is not None else np.diag([1, 1, 1])  # State cost
        self.R = R if R is not None else np.array([[1]])  # Control cost

    def compute_control(self, current_pose, target_path, current_speed, dt=0.066):
        #Compute the steering angle using LQR
        x, y, yaw = current_pose
        x_ref, y_ref, yaw_ref = target_path[-1]  # Use last point as reference

        # Compute state error
        ex = x_ref - x
        ey = y_ref - y
        e_yaw = math.atan2(math.sin(yaw_ref - yaw), math.cos(yaw_ref - yaw))

        # Define state matrix
        A = np.array([[1, dt, 0],
                      [0, 1,  dt],
                      [0, 0,  1]])

        B = np.array([[0],
                      [0],
                      [dt / self.wheelbase]])

        # Compute optimal control gain K using Riccati equation
        P = scipy.linalg.solve_discrete_are(A, B, self.Q, self.R)
        K = np.linalg.inv(B.T @ P @ B + self.R) @ (B.T @ P @ A)

        # Compute optimal control (steering angle)
        state_error = np.array([[ex], [ey], [e_yaw]])
        steering_angle = -K @ state_error
        return float(steering_angle)
    
class StanleyController:
    def __init__(self, k, k_soft):
        self.k = k
        self.k_soft = k_soft

    def control(self, current_pose, target_waypoint, current_speed):
        target_x, target_y, target_yaw = target_waypoint
        path_heading = math.atan2(target_y - current_pose[1], target_x - current_pose[0])

        heading_error = path_heading - current_pose[2]
        heading_error = math.atan2(math.sin(heading_error), math.cos(heading_error))

        cross_track_error = math.sqrt((target_x - current_pose[0])**2 + (target_y - current_pose[1])**2)
        control_angle = heading_error + math.atan2(self.k * cross_track_error, self.k_soft + current_speed)
        return control_angle
    
class Carla_Controls:
    def __init__(self, client, vehicle):
        self.client = client
        self.world = client.get_world()
        self.vehicle = vehicle
        #Tuning Parameters
        self.controller_type="Stanley"
        self.pid = PIDController(kp=1.0, ki=0.1, kd=0.05)
        self.stanley = StanleyController(k=1.0, k_soft=1.0)
        self.pure_pursuit=pure_pursuit(wheelbase=0,min_lookahead=3,max_lookahead=10,k_v=0.5)
        self.lqr=LQRController(wheelbase=2.5, Q=None, R=None)
        self.actual_trajectory=[]

    '''def find_closest_waypoint(self, target_path, lookahead_distance, current_state):
        x, y, _ = current_state
        closest_point = None
        
        for target_x, target_y, target_yaw in target_path:
            dist = math.sqrt((target_x - x) ** 2 + (target_y - y) ** 2)
            if dist >= lookahead_distance:
                closest_point = (target_x, target_y, target_yaw)
                break
        
        return closest_point'''


    def get_current_state(self):
        transform = self.vehicle.get_transform()
        velocity = self.vehicle.get_velocity()
        speed = math.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2)
        return (transform.location.x, transform.location.y, math.radians(transform.rotation.yaw), speed)
    
    
    def run(self, target_path, target_speeds, dt=0.066):
        waypoint_index = 0
        while waypoint_index < len(target_path):
            current_state = self.get_current_state()
            self.actual_trajectory.append([current_state[0], current_state[1]])

            #lookahead_distance = self.pure_pursuit.get_lookahead_distance(current_state[3])
            # closest_point = self.find_closest_waypoint(target_path, lookahead_distance, current_state)
            
            # if closest_point is None:
            #     break  # Stop if no valid waypoint found
            
            target_waypoint = target_path[waypoint_index]
            target_speed = target_speeds[waypoint_index]

            distance_to_waypoint = math.sqrt((target_waypoint[0] - current_state[0])**2 + 
                                             (target_waypoint[1] - current_state[1])**2)

            transition_threshold = max(0.5, 0.1 * current_state[3]) #Thresholding distance

            if distance_to_waypoint < transition_threshold:
                waypoint_index += 1

            if waypoint_index >= len(target_path):
                break

            throttle = np.clip(self.pid.control(target_speed, current_state[3], dt), 0, 1)
            if self.controller_type == "Stanley":
                steering = self.stanley.control(current_state, target_waypoint, current_state[3])
            elif self.controller_type == "PurePursuit":
                steering = self.pure_pursuit.control(current_state, target_waypoint, current_state[3])  #  No list needed
            elif self.controller_type == "LQR":
                steering = self.lqr.compute_control(current_state, target_waypoint, current_state[3], dt)  #  No list needed
            else:
                steering = 0  # Default
            
            steering = np.clip(steering, -1, 1)
            #Apply Controls
            control = carla.VehicleControl()
            control.throttle = throttle
            control.steer = steering
            self.vehicle.apply_control(control)

            time.sleep(dt)


#Path following plot of the vehicle, path following by different controllers plot

    def plot_trajectory(self, target_path):
            target_x = [point[0] for point in target_path]
            target_y = [point[1] for point in target_path]

            actual_x = [point[0] for point in self.actual_trajectory]
            actual_y = [point[1] for point in self.actual_trajectory]

            plt.figure(figsize=(8, 6))
            plt.plot(target_x, target_y, 'g--', label="Target Path (Waypoints)")
            plt.plot(actual_x, actual_y, 'r-', label="Actual Vehicle Trajectory")
            plt.xlabel("X Position")
            plt.ylabel("Y Position")
            plt.title("Vehicle Trajectory vs Waypoints")
            plt.legend()
            plt.grid()
            plt.show()



# Main function
if __name__ == "__main__":
    client = carla.Client('localhost', 2000)
    client.set_timeout(10.0)
    vehicle=None
    world = client.get_world()
    blueprint_library = world.get_blueprint_library()
    vehicle_bp = blueprint_library.find("vehicle.mercedes.coupe_2020")
    carla_map=world.get_map()
    spawn_point = carla_map.get_spawn_points()[5]
    while vehicle is None:
        vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
        time.sleep(0.5)

    spectator = world.get_spectator()
    #x = carla.Transform(vehicle.get_transform().transform(carla.Location(x=-0.4, z=1.8)), vehicle.get_transform().rotation)
    offset = carla.Location(x=-4.0, z=2.0)  # Move back (-x) and up (+z)

# Transform spectator position to world coordinates
    vehicle_transform = vehicle.get_transform()
    spectator_location = vehicle_transform.transform(offset)
    spectator_rotation = vehicle_transform.rotation
    x=carla.Transform(spectator_location, spectator_rotation)
    
    spectator.set_transform(x)
    
    time.sleep(0.2)

    target_path=[]
    
    try:
        # Load the dataset consisting of latitude,longitdue,yaw and vehicle speed
        data=pd.read_csv("output.csv")
        for i in range(len(data)):
            
            waypoint=carla_map.get_waypoint(carla_map.transform_to_geolocation(carla.GeoLocation(latitude=data.loc[i,"Latitude"],longitude=data.loc[i,"Longitude"],altitude=0)))
            target_path.append([waypoint.transform.location.x,waypoint.transform.location.y,data.loc[i,"heading"]])

        vehicle_target_speed=data["velocity"].tolist()
        controller = Carla_Controls(client, vehicle)
        controller.run(target_path, vehicle_target_speed)

    finally:
        controller.plot_trajectory(target_path)
        vehicle.destroy()