Skip to content

Commit

Permalink
feat(control_data_collecting_tool): smoothing and bug fix (#79)
Browse files Browse the repository at this point in the history 
* commit smoothing np.min

* smoothing curvature

* bug fixed

* update smoothing curvature

* run `pre-commit run -a`

---------

Co-authored-by: masayukiaino <[email protected]>
  • Loading branch information
@asei-proxima @masayukiaino
asei-proxima and masayukiaino authored Jul 18, 2024
1 parent 031ec63 commit f7569f6
Show file tree
Hide file tree
Showing 3 changed files with 65 additions and 16 deletions.
2 changes: 1 addition & 1 deletion control_data_collecting_tool/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 |
Expand Down
2 changes: 1 addition & 1 deletion control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
)
Expand Down
77 changes: 63 additions & 14 deletions control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]

Expand Down Expand Up @@ -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):
Expand All @@ -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]"),
)

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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")
Expand All @@ -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
Expand Down

0 comments on commit f7569f6

Please sign in to comment.