test.py

import carla
import math
import numpy as np
import time

# 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

# Stanley Controller
class StanleyController:
    def __init__(self, k, k_soft):
        self.k = k
        self.k_soft = k_soft

    def control(self, current_pose, target_path, current_speed):
        # Calculate cross-track error
        min_dist = float('inf')
        closest_point = None
        for point in target_path:
            dist = math.sqrt((point[0] - current_pose[0])**2 + (point[1] - current_pose[1])**2)
            if dist < min_dist:
                min_dist = dist
                closest_point = point

        if closest_point is None:
            return 0

        # Heading error
        path_heading = math.atan2(closest_point[1] - current_pose[1], closest_point[0] - current_pose[0])
        heading_error = path_heading - current_pose[2]
        heading_error = math.atan2(math.sin(heading_error), math.cos(heading_error))

        # Control law
        cross_track_error = min_dist
        control_angle = heading_error + math.atan2(self.k * cross_track_error, self.k_soft + current_speed)
        return control_angle

# Simulation loop
class CarlaController:
    def __init__(self, client, vehicle):
        self.client = client
        self.world = client.get_world()
        self.vehicle = vehicle
        self.pid = PIDController(kp=1.0, ki=0.1, kd=0.05)
        self.stanley = StanleyController(k=1.0, k_soft=1.0)

    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, transform.rotation.yaw, speed)

    def run(self, target_path, target_speeds, dt=0.05):
        while True:
            # Get current state
            current_state = self.get_current_state()

            # Longitudinal control
            target_speed = target_speeds[0]  # Use first target speed for simplicity
            throttle = self.pid.control(target_speed, current_state[3], dt)
            throttle = np.clip(throttle, 0, 1)

            # Lateral control
            steering = self.stanley.control(current_state, target_path, current_state[3])
            steering = np.clip(steering, -1, 1)

            # Apply control
            control = carla.VehicleControl()
            control.throttle = throttle
            control.steer = steering
            self.vehicle.apply_control(control)

            # Wait for next iteration
            time.sleep(dt)

# Main function
if __name__ == "__main__":
    client = carla.Client('localhost', 2000)
    client.set_timeout(10.0)

    world = client.get_world()
    blueprint_library = world.get_blueprint_library()
    vehicle_bp = blueprint_library.filter('vehicle.*')[0]
    spawn_point = world.get_map().get_spawn_points()[0]

    vehicle = world.spawn_actor(vehicle_bp, spawn_point)

    try:
        # Define target path (list of waypoints) and target speeds (constant for simplicity)
        
        target_path = [(x, 0) for x in range(0, 100)]  # Straight line
        target_speeds = [10] * len(target_path)  # 10 m/s

        controller = CarlaController(client, vehicle)
        controller.run(target_path, target_speeds)

    finally:
        vehicle.destroy()