diff --git a/crp_APL/controllers/ctrl_vehicle_control_lqr/launch/lat_lon_control_lqr.launch.py b/crp_APL/controllers/ctrl_vehicle_control_lqr/launch/lat_lon_control_lqr.launch.py index a1e682ae..bd64408d 100644 --- a/crp_APL/controllers/ctrl_vehicle_control_lqr/launch/lat_lon_control_lqr.launch.py +++ b/crp_APL/controllers/ctrl_vehicle_control_lqr/launch/lat_lon_control_lqr.launch.py @@ -8,10 +8,10 @@ def generate_launch_description(): name='lqr_controller', output='screen', parameters=[{ - 'dt': 0.03, + 'dt': 0.0333, 'wheel_base': 2.79, 'max_steer_tire_angle': 0.436332, - 'Q': [0.005, 0.0, 0.005, 0, 3.5], # Q matrix diagonal elements + 'Q': [0.2, 0.0, 0.2, 0, 10.5], # Q matrix diagonal elements 'R': [10.0, 1.0], # R matrix diagonal elements }] ) diff --git a/crp_APL/controllers/ctrl_vehicle_control_lqr/scripts/lqr_controller.py b/crp_APL/controllers/ctrl_vehicle_control_lqr/scripts/lqr_controller.py index 4ea9dafc..9889ce15 100755 --- a/crp_APL/controllers/ctrl_vehicle_control_lqr/scripts/lqr_controller.py +++ b/crp_APL/controllers/ctrl_vehicle_control_lqr/scripts/lqr_controller.py @@ -1,11 +1,14 @@ #!/usr/bin/env python3 import numpy as np +import math import scipy.linalg as la +from scipy.interpolate import make_interp_spline import time import rclpy from rclpy.node import Node from autoware_control_msgs.msg import Lateral,Longitudinal +from std_msgs.msg import Float32,Float32MultiArray from autoware_planning_msgs.msg import Trajectory from crp_msgs.msg import Ego @@ -47,6 +50,54 @@ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0,steer=0.0): def pi_2_pi(angle): return angle_mod(angle) +def fit_spline(x,y): + + x = np.array(x) + y = np.array(y) + + # Generate a finer grid of x values for smooth interpolation + x_fine = np.linspace(x.min(), x.max(), 100) + + # Create the spline interpolation + spline = make_interp_spline(x, y) + + # Evaluate the first and second derivatives of the spline + dy_dx = spline.derivative(nu=1)(x_fine) # First derivative + d2y_dx2 = spline.derivative(nu=2)(x_fine) # Second derivative + + # Compute the curvature + curvature = np.abs(d2y_dx2) / (1 + dy_dx**2)**(3/2) + + return curvature + +def fit_polynomial_and_curvature(x, y, degree=4): + """ + Fits a polynomial of the specified degree to the given x, y points and calculates curvature. + + Args: + x (array-like): The x-coordinates of the data points. + y (array-like): The y-coordinates of the data points. + degree (int): The degree of the polynomial to fit. Default is 3. + plot (bool): Whether to plot the fit and curvature. Default is False. + + Returns: + tuple: + - numpy.ndarray: Coefficients of the polynomial in descending powers. + - numpy.ndarray: Curvature values at the given x points. + """ + # Fit the polynomial + coefficients = np.polyfit(x, y, degree) + polynomial = np.poly1d(coefficients) + + # First and second derivatives + first_derivative = np.polyder(polynomial, 1) + second_derivative = np.polyder(polynomial, 2) + + # Calculate curvature + curvature = np.abs(second_derivative(x)) / (1 + first_derivative(x)**2)**(3/2) + + return curvature + # create an ROS controller class class ROSController(Node): @@ -65,8 +116,8 @@ def __init__(self): self.state = State(x=0.0, y=0.0, yaw=0.0, v=0.0, steer=0.0) - self.dt = 0.05 - self.L = 2.9 + self.dt = 0.0333 + self.L = 2.7 self.max_steer = np.deg2rad(20.0) # LQR parameter @@ -89,14 +140,22 @@ def __init__(self): self.declare_parameter('Q', self.lqr_Q.diagonal().tolist()) self.declare_parameter('R', self.lqr_R.diagonal().tolist()) + self.use_spline_fit = False + self.curvature_lookahead = 0 + self.dt = self.get_parameter('dt').value self.L = self.get_parameter('wheel_base').value self.max_steer = self.get_parameter('max_steer_tire_angle').value - self.control_clock = self.create_timer(0.05, self.control_loop) + self.control_clock = self.create_timer(0.0333, self.control_loop) self.ctrl_lat_publisher = self.create_publisher(Lateral, '/control/command/control_cmdLat', 10) self.ctrl_long_publisher = self.create_publisher(Longitudinal, '/control/command/control_cmdLong', 10) + + self.debug_ff_publisher = self.create_publisher(Float32, '/control/lqr/debug/ff', 10) + self.debug_fb_publisher = self.create_publisher(Float32, '/control/lqr/debug/fb', 10) + self.debug_ck_publisher = self.create_publisher(Float32MultiArray, '/control/lqr/debug/ck', 10) + self.traj_subscriber = self.create_subscription(Trajectory, '/plan/trajectory', self.receive_trajectory, 10) self.ego_subscriber = self.create_subscription(Ego, '/ego', self.receive_ego, 10) @@ -112,7 +171,12 @@ def receive_trajectory(self, msg): self.cy = [point.pose.position.y for point in msg.points] self.sp = [point.longitudinal_velocity_mps for point in msg.points] - self.cyaw = np.arctan2(np.diff(self.cy), np.diff(self.cx)).tolist() + self.cyaw = np.arctan2(np.diff(self.cy), np.diff(self.cx)).tolist() + + if self.use_spline_fit: + self.ck = fit_spline(self.cx, self.cy) + else: + self.ck = fit_polynomial_and_curvature(self.cx, self.cy, degree=3) def receive_ego(self, msg): @@ -197,7 +261,12 @@ def control_loop(self): tv = self.sp[0] - # k = -self.ck[0] # for now skip the feedforward term #TODO + if self.curvature_lookahead < len(self.ck) - 1: + k = self.ck[self.curvature_lookahead] + else: + k = self.ck[0] + + v = self.state.v e = self.cy[0] th_e = self.cyaw[0] @@ -255,9 +324,25 @@ def control_loop(self): ustar = -K @ x # calc steering input - #ff = math.atan2(L * k, 1) # feedforward steering angle #TODO + ff = math.atan2(self.L * k, 1) # feedforward steering angle + ff = ff * -1 fb = pi_2_pi(ustar[0, 0]) # feedback steering angle - delta = fb + delta = fb + ff + + Float32_msg = Float32() + Float32_msg.data = ff + + self.debug_ff_publisher.publish(Float32_msg) + + Float32_msg.data = fb + self.debug_fb_publisher.publish(Float32_msg) + + ck_msg = Float32MultiArray() + + for ck in self.ck: + ck_msg.data.append(ck) + + self.debug_ck_publisher.publish(ck_msg) # calc accel input accel = ustar[1, 0] @@ -282,7 +367,7 @@ def main(args=None): # Clean up and shutdown node.destroy_node() - rclpy.shutdown() + rclpy.shutdown() if __name__ == '__main__':