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.
New contributor
Prithvi Bhende is a new contributor to this site. Take care in asking for clarification, commenting, and answering.
Check out our Code of Conduct.