feat(control_data_collecting_tool): smoothing and bug fix

control_data_collecting_tool/README.md

@@ -85,7 +85,7 @@ ROS 2 params in `/data_collecting_trajectory_publisher` node:
 
 | Name                                     | Type     | Description                                                           | Default value |
 | :--------------------------------------- | :------- | :-------------------------------------------------------------------- | :------------ |
-| `max_lateral_accel`                      | `double` | Max lateral acceleration limit [m/ss]                                 | 2.94          |
+| `max_lateral_accel`                      | `double` | Max lateral acceleration limit [m/ss]                                 | 0.5           |
 | `lateral_error_threshold`                | `double` | Lateral error threshold where applying velocity limit [m/s]           | 5.0           |
 | `yaw_error_threshold`                    | `double` | Yaw error threshold where applying velocity limit [rad]               | 0.5           |
 | `velocity_limit_by_tracking_error`       | `double` | Velocity limit applied when tracking error exceeds threshold [m/s]    | 2.0           |

control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py

@@ -317,7 +317,7 @@ def linearized_pure_pursuit_control(
             if np.abs(yaw_err) < np.pi:
                 break
 
-        lookahead = lookahead_coef + lookahead_intercept * np.abs(longitudinal_vel_obs)
+        lookahead = lookahead_intercept + lookahead_coef * np.abs(longitudinal_vel_obs)
         linearized_pure_pursuit_steer_kp = (
             linearized_pure_pursuit_steer_kp_param * wheel_base / (lookahead * lookahead)
         )

control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py

@@ -32,10 +32,23 @@
 from visualization_msgs.msg import MarkerArray
 
 debug_matplotlib_plot_flag = False
+Differential_Smoothing_Flag = True
+USE_CURVATURE_RADIUS_FLAG = False
 if debug_matplotlib_plot_flag:
     import matplotlib.pyplot as plt
 
 
+def smooth_bounding(upper: np.ndarray, threshold: np.ndarray, x: np.ndarray):
+    result = np.zeros(x.shape)
+    for i in range(x.shape[0]):
+        if x[i] <= threshold[i]:
+            result[i] = x[i]
+        else:
+            z = np.exp(-(x[i] - threshold[i]) / (upper[i] - threshold[i]))
+            result[i] = upper[i] * (1 - z) + threshold[i] * z
+    return result
+
+
 def getYaw(orientation_xyzw):
     return R.from_quat(orientation_xyzw.reshape(-1, 4)).as_euler("xyz")[:, 2]
 
@@ -79,38 +92,41 @@ def get_trajectory_points(
             x[i] = (b - a) * t / (2 * OB)
             y[i] = a * t / (2 * OB)
             yaw[i] = θB
-            curve[i] = 1e10
+            curve[i] = 1e-10
         if OB <= t and t <= OB + BD:
             t1 = t - OB
             t1_rad = t1 / R
             x[i] = OR[0] + R * cos(pi / 2 - t1_rad)
             y[i] = OR[1] + R * sin(pi / 2 - t1_rad)
             yaw[i] = -t1_rad
-            curve[i] = R
+            curve[i] = 1 / R
         if OB + BD <= t and t <= OB + BD + AD:
             t2 = t - (OB + BD)
             x[i] = D[0] - (b - a) * t2 / (2 * OB)
             y[i] = D[1] + a * t2 / (2 * OB)
             yaw[i] = pi - θB
-            curve[i] = 1e10
+            curve[i] = 1e-10
         if OB + BD + AD <= t and t <= OB + BD + AD + AC:
             t3 = t - (OB + BD + AD)
             t3_rad = t3 / R
             x[i] = OL[0] + R * cos(pi / 2 + t3_rad)
             y[i] = OL[1] + R * sin(pi / 2 + t3_rad)
             yaw[i] = pi + t3_rad
-            curve[i] = R
+            curve[i] = 1 / R
         if OB + BD + AD + AC <= t and t <= OB + BD + AD + AC + CO:
             t4 = t - (OB + BD + AD + AC)
             x[i] = C[0] + (b - a) * t4 / (2 * OB)
             y[i] = C[1] + a * t4 / (2 * OB)
             yaw[i] = θB
-            curve[i] = 1e10
+            curve[i] = 1e-10
     # drop rest
     x = x[:i_end]
     y = y[:i_end]
     yaw = yaw[:i_end]
-    return np.array([x, y]).T, yaw, curve[:i_end]
+    if USE_CURVATURE_RADIUS_FLAG:
+        return np.array([x, y]).T, yaw, 1 / curve[:i_end]
+    else:
+        return np.array([x, y]).T, yaw, curve[:i_end]
 
 
 class DataCollectingTrajectoryPublisher(Node):
@@ -119,7 +135,7 @@ def __init__(self):
 
         self.declare_parameter(
             "max_lateral_accel",
-            2.94,  # 0.3G
+            0.5,  # 0.3G
             ParameterDescriptor(description="Max lateral acceleration limit [m/ss]"),
         )
 
@@ -380,6 +396,18 @@ def updateNominalTargetTrajectory(self):
 
             trajectory_yaw_data = smoothed_trajectory_yaw_data.copy()
 
+            if not USE_CURVATURE_RADIUS_FLAG:
+                augmented_curvature_data = np.hstack(
+                    [
+                        trajectory_curvature_data[-window:],
+                        trajectory_curvature_data,
+                        trajectory_curvature_data[:window],
+                    ]
+                )
+                trajectory_curvature_data = (
+                    1 * np.convolve(augmented_curvature_data, w, mode="same")[window:-window]
+                )
+
         # [2-4] nominal velocity
         target_longitudinal_velocity = (
             self.get_parameter("target_longitudinal_velocity").get_parameter_value().double_value
@@ -531,16 +559,26 @@ def timer_callback(self):
             max_lateral_accel = (
                 self.get_parameter("max_lateral_accel").get_parameter_value().double_value
             )
-            lateral_acc_limit = np.sqrt(max_lateral_accel * trajectory_curvature_data)
+            if USE_CURVATURE_RADIUS_FLAG:
+                lateral_acc_limit = np.sqrt(max_lateral_accel * trajectory_curvature_data)
+            else:
+                lateral_acc_limit = np.sqrt(max_lateral_accel / trajectory_curvature_data)
             lateral_acc_limit = np.hstack(
                 [
                     lateral_acc_limit,
                     lateral_acc_limit[:aug_data_length],
                 ]
             )
-            trajectory_longitudinal_velocity_data = np.minimum(
-                trajectory_longitudinal_velocity_data, lateral_acc_limit
-            )
+            if Differential_Smoothing_Flag:
+                trajectory_longitudinal_velocity_data = smooth_bounding(
+                    lateral_acc_limit,
+                    0.9 * lateral_acc_limit,
+                    trajectory_longitudinal_velocity_data,
+                )
+            else:
+                trajectory_longitudinal_velocity_data = np.minimum(
+                    trajectory_longitudinal_velocity_data, lateral_acc_limit
+                )
             # [5-3] apply limit by lateral error
             velocity_limit_by_tracking_error = (
                 self.get_parameter("velocity_limit_by_tracking_error")
@@ -563,9 +601,20 @@ def timer_callback(self):
             tmp_yaw_error = np.abs(present_yaw - trajectory_yaw_data[nearestIndex])
 
             if lateral_error_threshold < tmp_lateral_error or yaw_error_threshold < tmp_yaw_error:
-                trajectory_longitudinal_velocity_data = np.minimum(
-                    trajectory_longitudinal_velocity_data, velocity_limit_by_tracking_error
-                )
+                if Differential_Smoothing_Flag:
+                    velocity_limit_by_tracking_error_array = (
+                        velocity_limit_by_tracking_error
+                        * np.ones(trajectory_longitudinal_velocity_data.shape)
+                    )
+                    trajectory_longitudinal_velocity_data = smooth_bounding(
+                        velocity_limit_by_tracking_error_array,
+                        0.9 * velocity_limit_by_tracking_error_array,
+                        trajectory_longitudinal_velocity_data,
+                    )
+                else:
+                    trajectory_longitudinal_velocity_data = np.minimum(
+                        trajectory_longitudinal_velocity_data, velocity_limit_by_tracking_error
+                    )
 
             # [6] publish
             # [6-1] publish trajectory