I’m working with an EUFS simulator on ROS2, using an RViz formula car simulator. My goal is to control the car using PID and Stanley controllers for accurate path following and stability. However, I’m encountering an issue where the car starts swaying randomly. This happens despite implementing standard control algorithms for handling heading and cross-track errors.
Details:

Simulator: RViz Formula Car Simulator
Control Algorithms:
    PID Controller: Used for adjusting acceleration based on speed error.
    Stanley Controller: Used for steering based on cross-track and heading errors.
Problem: The car exhibits random swaying behavior while navigating, which is not expected. The car should follow the path smoothly without such instability.
Current Implementation:
    Heading Error Calculation: Computes the angular difference between the car's heading and the desired heading.
    Cross-Track Error Calculation: Computes the distance between the car and the path line.
    PID Controller: Adjusts acceleration based on the difference between the reference speed and current speed.
    Stanley Controller: Computes the steering angle based on cross-track and heading errors.

here is the code:

import rclpy
from rclpy.node import Node
from ackermann_msgs.msg import AckermannDriveStamped as control
from nav_msgs.msg import Odometry as pos
import csv
import math 
import time
import numpy as np

class Car(Node):
    def __init__(self):
        super().__init__("PID_control")
        
        self.waypoints = list()
        self.pid = PID()
        self.stanley = Stanley()
        with open("/home/psb/EUFS_FM/src/sim/sim/waypoint.csv", "r") as f:
            r = csv.reader(f)
            for i in r:
                self.waypoints.append([float(i[0]), float(i[1])])
        
        self.yaw = 0
        self.origin = (self.waypoints[0][0], self.waypoints[0][1])
        self.current_waypoint_index = 0
        self.m = self.m_perp = 0
        self.lap_start_time = None  
        self.lap_times = []         
        self.total_laps = 0 
        self.prev_waypoint = 0
        self.prev_op_sign = self.prev_timer_sign = 0
        self.publisher = self.create_publisher(control, '/cmd', 10)
        
        c = control()
        c.drive.acceleration = 1.0
        self.publisher.publish(c)
        self.pos = self.create_subscription(pos, 'ground_truth/odom', self.position, 10)
        
    def position(self, msg):
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        q = msg.pose.pose.orientation
        t3 = +2.0 * (q.w * q.z + q.x * q.y)
        t4 = +1.0 - 2.0 * (q.y * q.y + q.z * q.z)
        Z = np.arctan2(t3, t4)
        vehicle_yaw = Z
        self.yaw = vehicle_yaw
        twist = msg.twist.twist
        current_x_dot = twist.linear.x
        self.update_waypoint(x, y)
        t1 = self.straight_line()
        t2 = self.straight_line(2)
        t3 = self.straight_line(3)
        t_avg = 0.85 * t1 + 0.15 * t2 + 0.05 * t3
        if self.current_waypoint_index == 0 or current_x_dot == 0 or current_x_dot > 8:
            ref_x_dot = 5
        else:
            ref_x_dot = 5 * np.float_power(np.cos(t_avg), 1)
        error_x = ref_x_dot - current_x_dot
        acc = self.pid.control_op(error_x)
        cross_track_error = self.cross_track_error(x, y)
        heading_error = self.heading_error(x, y)
        steering1 = self.stanley.control_op(cross_track_error, heading_error, current_x_dot)
        nx1, ny1 = self.predict(x, y, acc, twist)
        cross_track_error = self.cross_track_error(nx1, ny1)
        heading_error = self.heading_error(nx1, ny1)
        steering2 = self.stanley.control_op(cross_track_error, heading_error, current_x_dot)
        nx2, ny2 = self.predict(nx1, ny1, acc, twist)
        cross_track_error = self.cross_track_error(nx2, ny2)
        heading_error = self.heading_error(nx2, ny2)
        steering3 = self.stanley.control_op(cross_track_error, heading_error, current_x_dot)
        steering = 0.8 * steering1 + 0.15 * steering2 + 0.05 * steering3
        car_control = control()
        car_control.drive.acceleration = acc
        car_control.drive.steering_angle = steering
        self.publisher.publish(car_control)
        self.update_waypoint(x, y)
        print(acc)
    
    def timer(self, x, y):
        x1 = 2.9938483238220215
        y1 = 2.6967356204986572
        x2 = 3.0065183639526367
        y2 = -1.8987843990325928
        m = (y2 - y1) / (x2 - x1)
        m_perp = -1 / m
        c = y1 - (m_perp * x1)
        op = m_perp * x + c - y
        if (x2 - x) ** 2 + (y2 - y) ** 2 <= 10:
            if (self.prev_op_sign > 0 and op < 0) or (self.prev_op_sign < 0 and op > 0) or op == 0:
                if self.lap_start_time is None:
                    self.lap_start_time = time.time()
                    print("Lap started!")
                else:
                    lap_time = time.time() - self.lap_start_time
                    self.lap_times.append(lap_time)
                    self.lap_start_time = None
                    self.total_laps += 1
                    print(f"Lap {self.total_laps} completed! Time: {lap_time:.2f} seconds")
        self.prev_timer_sign = op
    
    def lap_timer(self):
        waypoint = self.current_waypoint_index
        if waypoint % 37 == 1 and self.prev_waypoint != waypoint:
            if self.lap_start_time is not None:
                lap_time = time.time() - self.lap_start_time
                self.lap_times.append(lap_time)
                self.lap_start_time = None
                self.total_laps += 1
                print(f"Lap {self.total_laps} completed! Time: {lap_time:.2f} seconds")
        self.prev_waypoint = waypoint
    
    def predict(self, x, y, a, twist):
        t = 0.05
        nx = x + (twist.linear.x * t + 0.5 * (a * np.sin(self.yaw) * t * t))
        ny = y + (twist.linear.y * t + 0.5 * (a * np.cos(self.yaw) * t * t))
        return nx, ny
    
    def heading_error(self, x1, y1):
        x, y = self.global_to_car_frame(self.yaw, x1, y1)
        heading_error = np.arctan2(y, x)
        return heading_error
    
    def global_to_car_frame(self, yaw, x, y):
        rotation_matrix = np.array([
            [np.cos(-yaw), -np.sin(-yaw)],
            [np.sin(-yaw), np.cos(-yaw)]
        ])
        points = np.array([
            self.waypoints[self.current_waypoint_index % 37][0] - x,
            self.waypoints[self.current_waypoint_index % 37][1] - y
        ])
        rotated = rotation_matrix @ points
        return rotated[0], rotated[1]
    
    def straight_line(self, x=1):
        x2 = self.waypoints[(self.current_waypoint_index + x) % 37][0]
        y2 = self.waypoints[(self.current_waypoint_index + x) % 37][1]
        x1 = self.waypoints[self.current_waypoint_index % 37][0]
        y1 = self.waypoints[self.current_waypoint_index % 37][1]
        m = (y2 - y1) / (x2 - x1)
        if x == 1:
            self.m = m
            self.m_perp = -1 / self.m
        return np.arctan(m)
    
    def update_waypoint(self, x, y):
        x1 = self.waypoints[self.current_waypoint_index % 37][0]
        y1 = self.waypoints[self.current_waypoint_index % 37][1]
        c = y1 - (self.m_perp * x1)
        op = self.m_perp * x + c - y
        if (self.prev_op_sign > 0 and op < 0) or (self.prev_op_sign < 0 and op > 0) or op == 0:
            self.current_waypoint_index += 1
            self.straight_line()
            self.lap_timer()
        self.prev_op_sign = op

    def cross_track_error(self, x, y, k=0):
        x1 = self.waypoints[(self.current_waypoint_index - 1) % 37][0]
        y1 = self.waypoints[(self.current_waypoint_index - 1) % 37][1]
        x2 = self.waypoints[(self.current_waypoint_index + k) % 37][0]
        y2 = self.waypoints[(self.current_waypoint_index + k) % 37][1]
        A = -(y2 - y1)
        B = (x2 - x1)
        C = -B * y1 - A * x1
        distance = abs(A * x + B * y + C) / np.sqrt(A * A + B * B)
        return distance

class PID:
    def __init__(self):
        self.prev_error = 0
        self.integral = 0
        self.Kp = 1.0
        self.Ki = 0.1
        self.Kd = 0.01
    
    def control_op(self, error):
        self.integral += error
        derivative = error - self.prev_error
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.prev_error = error
        return output

class Stanley:
    def __init__(self):
        self.K = 1.0
    
    def control_op(self, cross_track_error, heading_error, velocity):
        return np.arctan2(self.K * cross_track_error, velocity) + heading_error

def main(args=None):
    rclpy.init(args=args)
    car = Car()
    rclpy.spin(car)
    car.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

I tried changing PID and Stanley constants, but it did not work.
What I Expect:

The car should follow the path smoothly with minimal swaying.
The PID and Stanley controllers should work together to keep the car stable and accurately on the path.

Code Snippets:

PID Controller Class
Stanley Controller Class
Heading Error Calculation
Cross-Track Error Calculation

I would appreciate any guidance or suggestions on what could be causing the random swaying and how to resolve it.