def __init__(self, num_paths, path_offset, circle_offsets, circle_radii, path_select_weight, time_gap, a_max, slow_speed, stop_line_buffer): self._num_paths = num_paths self._path_offset = path_offset self._path_optimizer = path_optimizer.PathOptimizer() self._collision_checker = collision_checker.CollisionChecker(circle_offsets, circle_radii, path_select_weight) self._velocity_planner = velocity_planner.VelocityPlanner(time_gap, a_max, slow_speed, stop_line_buffer) self._prev_best_path = []
import numpy as np import path_optimizer import matplotlib.pyplot as plt po = path_optimizer.PathOptimizer() # path = po.optimize_spiral(9.185, -4.499, 4.261) path = po.optimize_spiral(16.49, -3.01, 0.0) # path = po.optimize_spiral(0.0, 0.0, 10.22874134679308) # NAN result for x, y, t in np.stack(arrays=path, axis=1): print("{:5.2f} {:5.2f} {:5.2f}".format(x, y, t)) fig = plt.figure() ax = fig.add_subplot(1, 2, 1) ax.plot(path[0], path[1]) ax.set_xlabel("x [m]") ax.set_ylabel("y [m]") ax = fig.add_subplot(1, 2, 2) ax.plot(path[0], path[2]) ax.set_xlabel("x [m]") ax.set_ylabel("theta [rad]") plt.show()