def visualize_planning_result(filename: str, obstacle_idx: int, states: np.ndarray, goal: np.ndarray): obs_center = load_obstacles(obstacle_idx) obstacles = [Rectangle((xy[0] - 4, xy[1] - 4), 8, 8) for xy in obs_center] obstacles = PatchCollection(obstacles, facecolor="gray", edgecolor="black") car = PatchCollection( [Rectangle((-1, -0.5), 2, 1), Arrow(0, 0, 2, 0, width=1.0)], facecolor="blue", edgecolor="blue") fig = Figure() canvas = FigureCanvas(fig) ax = fig.subplots() ax.set_aspect('equal', 'box') ax.set_xlim([-25, 25]) ax.set_ylim([-35, 35]) canvas.draw() image = np.array(canvas.buffer_rgba()) H, W, _ = image.shape writer = cv2.VideoWriter(filename, cv2.VideoWriter_fourcc(*'mp4v'), 30., (W, H), True) T = states.shape[0] for t in range(T): # obstacles ax.add_collection(obstacles) # target ax.scatter(goal[0], goal[1], marker='*', c="red") # current position transform = Affine2D().rotate(states[t, 2]).translate( states[t, 0], states[t, 1]) car.set_transform(transform + ax.transData) ax.add_collection(car) # plot canvas.draw() image = np.array(canvas.buffer_rgba())[:, :, :3] writer.write(image) ax.clear() writer.release() h264_converter(filename)
def visualize_global_map(filename: str, obstacle_idx: int, observations: torch.Tensor, predictions: torch.Tensor): observations = observations[:, 3 * 64 * 64:].cpu().numpy() observations = observations * np.array( [25.0, 35.0, np.pi, 25.0, 35.0, np.pi], dtype=np.float32) * 2.0 predictions = predictions[:, 3 * 64 * 64:].cpu().numpy() predictions = predictions * np.array( [25.0, 35.0, np.pi, 25.0, 35.0, np.pi], dtype=np.float32) * 2.0 obs_center = load_obstacles(obstacle_idx) obstacles = [Rectangle((xy[0] - 4, xy[1] - 4), 8, 8) for xy in obs_center] obstacles = PatchCollection(obstacles, facecolor="gray", edgecolor="black") car_observation = PatchCollection( [Rectangle((-1, -0.5), 2, 1), Arrow(0, 0, 2, 0, width=1.0)], facecolor="blue", edgecolor="blue") car_prediction = PatchCollection( [Rectangle((-1, -0.5), 2, 1), Arrow(0, 0, 2, 0, width=1.0)], facecolor="yellow", edgecolor="yellow") fig = Figure() canvas = FigureCanvas(fig) ax = fig.subplots() ax.set_aspect('equal', 'box') ax.set_xlim([-25, 25]) ax.set_ylim([-35, 35]) canvas.draw() image = np.array(canvas.buffer_rgba()) H, W, _ = image.shape writer = cv2.VideoWriter(filename, cv2.VideoWriter_fourcc(*'mp4v'), 30., (W, H), True) T = observations.shape[0] for t in range(T): # obstacles ax.add_collection(obstacles) # target ax.scatter(observations[-1, 3], observations[-1, 4], marker='*', c="red") # current position transform = Affine2D().rotate(observations[t, 2]).translate( observations[t, 0], observations[t, 1]) car_observation.set_transform(transform + ax.transData) ax.add_collection(car_observation) # predicted position transform = Affine2D().rotate(predictions[t, 2]).translate( predictions[t, 0], predictions[t, 1]) car_prediction.set_transform(transform + ax.transData) ax.add_collection(car_prediction) # plot ax.set_xlim([-25, 25]) ax.set_ylim([-35, 35]) canvas.draw() image = np.array(canvas.buffer_rgba())[:, :, :3] writer.write(image) ax.clear() writer.release() h264_converter(filename)