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