Plot an example bezier curve.
()
| 123 | |
| 124 | |
| 125 | def main(): |
| 126 | """Plot an example bezier curve.""" |
| 127 | start_x = 10.0 # [m] |
| 128 | start_y = 1.0 # [m] |
| 129 | start_yaw = np.radians(180.0) # [rad] |
| 130 | |
| 131 | end_x = -0.0 # [m] |
| 132 | end_y = -3.0 # [m] |
| 133 | end_yaw = np.radians(-45.0) # [rad] |
| 134 | offset = 3.0 |
| 135 | |
| 136 | path, control_points = calc_4points_bezier_path( |
| 137 | start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset) |
| 138 | |
| 139 | # Note: alternatively, instead of specifying start and end position |
| 140 | # you can directly define n control points and compute the path: |
| 141 | # control_points = np.array([[5., 1.], [-2.78, 1.], [-11.5, -4.5], [-6., -8.]]) |
| 142 | # path = calc_bezier_path(control_points, n_points=100) |
| 143 | |
| 144 | # Display the tangent, normal and radius of cruvature at a given point |
| 145 | t = 0.86 # Number in [0, 1] |
| 146 | x_target, y_target = bezier(t, control_points) |
| 147 | derivatives_cp = bezier_derivatives_control_points(control_points, 2) |
| 148 | point = bezier(t, control_points) |
| 149 | dt = bezier(t, derivatives_cp[1]) |
| 150 | ddt = bezier(t, derivatives_cp[2]) |
| 151 | # Radius of curvature |
| 152 | radius = 1 / curvature(dt[0], dt[1], ddt[0], ddt[1]) |
| 153 | # Normalize derivative |
| 154 | dt /= np.linalg.norm(dt, 2) |
| 155 | tangent = np.array([point, point + dt]) |
| 156 | normal = np.array([point, point + [- dt[1], dt[0]]]) |
| 157 | curvature_center = point + np.array([- dt[1], dt[0]]) * radius |
| 158 | circle = plt.Circle(tuple(curvature_center), radius, |
| 159 | color=(0, 0.8, 0.8), fill=False, linewidth=1) |
| 160 | |
| 161 | assert path.T[0][0] == start_x, "path is invalid" |
| 162 | assert path.T[1][0] == start_y, "path is invalid" |
| 163 | assert path.T[0][-1] == end_x, "path is invalid" |
| 164 | assert path.T[1][-1] == end_y, "path is invalid" |
| 165 | |
| 166 | if show_animation: # pragma: no cover |
| 167 | fig, ax = plt.subplots() |
| 168 | ax.plot(path.T[0], path.T[1], label="Bezier Path") |
| 169 | ax.plot(control_points.T[0], control_points.T[1], |
| 170 | '--o', label="Control Points") |
| 171 | ax.plot(x_target, y_target) |
| 172 | ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent") |
| 173 | ax.plot(normal[:, 0], normal[:, 1], label="Normal") |
| 174 | ax.add_artist(circle) |
| 175 | plot_arrow(start_x, start_y, start_yaw) |
| 176 | plot_arrow(end_x, end_y, end_yaw) |
| 177 | ax.legend() |
| 178 | ax.axis("equal") |
| 179 | ax.grid(True) |
| 180 | plt.show() |
| 181 | |
| 182 |
no test coverage detected