def reeds_shepp_path_planning(start_x, start_y, start_yaw,
end_x, end_y, end_yaw, curvature):
q0 = [start_x, start_y, start_yaw]
q1 = [end_x, end_y, end_yaw]
step_size = 0.1
qs = reeds_shepp.path_sample(q0, q1, curvature, step_size)
xs = [q[0] for q in qs]
ys = [q[1] for q in qs]
yaw = [q[2] for q in qs]
xs.append(end_x)
ys.append(end_y)
yaw.append(end_yaw)
clen = reeds_shepp.path_length(q0, q1, curvature)
pathtypeTuple = reeds_shepp.path_type(q0, q1, curvature)
ptype = ""
for t in pathtypeTuple:
if t == 1:
ptype += "L"
elif t == 2:
ptype += "S"
elif t == 3:
ptype += "R"
return xs, ys, yaw, ptype, clen
def plot_table(cols):
rows = ((len(qs)) / cols)
for i, (q0, q1) in enumerate(qs):
plt.subplot(rows, cols, i + 1)
plot_path(q0, q1)
dist = reeds_shepp.path_length(q0, q1, rho)
print(reeds_shepp.path_type(q0, q1, rho))
plt.title('length: {:.2f}'.format(dist))
plt.savefig('fig/demo.png')
plt.show()
def extract_segments(q_from, q_to):
segments.extend(reeds_shepp.path_type(q_from, q_to, 1. / self.maximum_curvature))
return q_to