def generate_path(s):
"""
design path using reeds-shepp path generator.
divide paths into sections, in each section the direction is the same.
:param s: objective positions and directions.
:return: paths
"""
wheelbase_ = l_f + l_r
max_c = math.tan(0.5 * max_steer_angle) / wheelbase_
path_x, path_y, yaw, direct, rc = [], [], [], [], []
x_rec, y_rec, yaw_rec, direct_rec, rc_rec = [], [], [], [], []
direct_flag = 1.0
for i in range(len(s) - 1):
s_x, s_y, s_yaw = s[i][0], s[i][1], np.deg2rad(s[i][2])
g_x, g_y, g_yaw = s[i + 1][0], s[i + 1][1], np.deg2rad(s[i + 1][2])
path_i = rs.calc_optimal_path(s_x, s_y, s_yaw,
g_x, g_y, g_yaw, max_c)
irc, rds = rs.calc_curvature(path_i.x, path_i.y, path_i.yaw, path_i.directions)
ix = path_i.x
iy = path_i.y
iyaw = path_i.yaw
idirect = path_i.directions
for j in range(len(ix)):
if idirect[j] == direct_flag:
x_rec.append(ix[j])
y_rec.append(iy[j])
yaw_rec.append(iyaw[j])
direct_rec.append(idirect[j])
rc_rec.append(irc[j])
else:
if len(x_rec) == 0 or direct_rec[0] != direct_flag:
direct_flag = idirect[j]
continue
path_x.append(x_rec)
path_y.append(y_rec)
yaw.append(yaw_rec)
direct.append(direct_rec)
rc.append(rc_rec)
x_rec, y_rec, yaw_rec, direct_rec, rc_rec = \
[x_rec[-1]], [y_rec[-1]], [yaw_rec[-1]], [-direct_rec[-1]], [rc_rec[-1]]
path_x.append(x_rec)
path_y.append(y_rec)
yaw.append(yaw_rec)
direct.append(direct_rec)
rc.append(rc_rec)
x_all, y_all = [], []
for ix, iy in zip(path_x, path_y):
x_all += ix
y_all += iy
return path_x, path_y, yaw, direct, rc, x_all, y_all
def generate_path(s):
# max_c = math.tan(C.MAX_STEER) / C.WB
max_c = math.tan(0.25) / C.WB
path_x, path_y, yaw, direct, rc = [], [], [], [], []
x_rec, y_rec, yaw_rec, direct_rec, rc_rec = [], [], [], [], []
direc_flag = 1.0
for i in range(len(s) - 1):
s_x, s_y, s_yaw = s[i][0], s[i][1], np.deg2rad(s[i][2])
g_x, g_y, g_yaw = s[i + 1][0], s[i + 1][1], np.deg2rad(s[i + 1][2])
path_i = rs.calc_optimal_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, max_c)
irc, rds = rs.calc_curvature(path_i.x, path_i.y, path_i.yaw,
path_i.directions)
ix = path_i.x
iy = path_i.y
iyaw = path_i.yaw
idirect = path_i.directions
for j in range(len(ix)):
if idirect[j] == direc_flag:
x_rec.append(ix[j])
y_rec.append(iy[j])
yaw_rec.append(iyaw[j])
direct_rec.append(idirect[j])
rc_rec.append(irc[j])
else:
if len(x_rec) == 0 or direct_rec[0] != direc_flag:
direc_flag = idirect[j]
continue
path_x.append(x_rec)
path_y.append(y_rec)
yaw.append(yaw_rec)
direct.append(direct_rec)
rc.append(rc_rec)
x_rec, y_rec, yaw_rec, direct_rec, rc_rec = \
[x_rec[-1]], [y_rec[-1]], [yaw_rec[-1]], [-direct_rec[-1]], [rc_rec[-1]]
path_x.append(x_rec)
path_y.append(y_rec)
yaw.append(yaw_rec)
direct.append(direct_rec)
rc.append(rc_rec)
x_all, y_all = [], []
for ix, iy in zip(path_x, path_y):
x_all += ix
y_all += iy
return path_x, path_y, yaw, direct, rc, x_all, y_all