Merge pull request #101 from jkk-research/99-crptg1-controllers-calib…

…ration-on-track

99 crptg1 controllers calibration on track

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
             }]
         )

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__':