only_path_dynamic_preview.py

#!/usr/bin/python3

import sys
import math
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
from matplotlib.widgets import Slider
from matplotlib import animation

obs_lines = []
i = 0
pathLength = 0


def load_obstacles(filename):
    with open(filename, 'r') as myfile:
        data = myfile.read().replace('\n', ' ')
    s = data.split()
    obs_num = int(s[0])
    res = []
    j = 1
    for i in range(obs_num):
        p_num = int(s[j])
        j += 1
        pol = []
        for k in range(p_num):
            pol.append([float(s[j]), float(s[j + 1])])
            j += 2
        pol.append(pol[0])  # repeat the first point to create a 'closed loop'
        res.append(pol)
    return res


def draw_obstacles(obstacles, ax):
    for pol in obstacles:
        line1, = ax.plot([], [], 'red', lw=2)
        obs_lines.append(line1)


def parse_file(filename):
    with open(filename, 'r') as myfile:
        d = myfile.read().replace('\n', ' ')
        d = d.replace('\r', ' ')
        d = d.replace('\t', ' ')
        d = d.replace('  ', ' ')
        for ___ in range(10):
            d = d.replace('  ', ' ')
    return d.split()


def parse_polygon():
    global i, data
    ver_num = int(data[i])
    if ver_num == 0:
        return None
    i += 1
    poly = []
    for __ in range(ver_num):
        poly.append([float(data[i]), float(data[i + 1])])
        i += 2
    poly.append(poly[0])
    return poly


def parse_obstacles():
    global i, data
    obs = []
    poly = parse_polygon()
    if poly:
        obs.append(poly)
    obs_num = int(data[i])
    i += 1
    for _ in range(obs_num):
        poly = parse_polygon()
        if poly:
            obs.append(poly)
    return obs


def parse_path():
    global i, data, pathLength
    path = []
    pathLength = int(data[i])
    i += 1
    for _ in range(pathLength):
        path.append((float(data[i]), float(data[i + 1])))
        i += 2
    return path


if len(sys.argv) < 3:
    print("[USAGE1] obstacles.txt output.txt")
    exit()

cmds = sys.argv[1:]
data = parse_file(cmds[0])
i = 0
obs = load_obstacles(cmds[0])
data = parse_file(cmds[1])

i = 0
path = parse_path()

fig = plt.figure()

ax = fig.add_subplot(111, aspect='equal', autoscale_on=False,
                     xlim=(-100, 100), ylim=(-100, 100))

draw_obstacles(obs, ax)
path_num_text = ax.text(0.02, 0.95, '', transform=ax.transAxes)
robot, = ax.plot([], [], 'o-', lw=2, color='black')
frames_per_path_part = 3000


def init():
    """initialize animation"""
    global obs, obs_lines
    ret = [robot, path_num_text]
    robot.set_data([], [])
    i = 0
    for pol in obs:
        xs, ys = zip(*pol)  # create lists of x and y values
        obs_lines[i].set_data(xs, ys)
        ret.append(obs_lines[i])
        i += 1

    path_num_text.set_text('')
    return tuple(ret)


currentFrame = 0
currentSpeed = 20


def animate(i):
    """perform animation step"""
    global path, frames_per_path_part, obs_lines, currentFrame, currentSpeed, pathLength
    currentFrame = (currentFrame + currentSpeed) % (frames_per_path_part * (pathLength - 1))
    index = int(currentFrame / frames_per_path_part)
    part_of = currentFrame % frames_per_path_part
    part_of = part_of / frames_per_path_part
    p11 = path[index]
    p12 = path[index + 1]
    x1 = [p11[0] + (p12[0] - p11[0]) * part_of]
    y1 = [p11[1] + (p12[1] - p11[1]) * part_of]

    robot.set_data(x1, y1)
    ret = [robot, path_num_text]

    for pol in obs_lines:
        ret.append(pol)

    path_num_text.set_text('path part = %d -> %d (0-%d)' % (index, index + 1, len(path) - 1))
    return tuple(ret)


ani = animation.FuncAnimation(fig, animate, frames=frames_per_path_part * (pathLength - 1), interval=33, blit=True,
                              init_func=init)

axcolor = 'lightgoldenrodyellow'
plt.subplots_adjust(bottom=0.2)
axPlayer = plt.axes([0.1, 0.1, 0.8, 0.025], facecolor=axcolor)
axSpeed = plt.axes([0.1, 0.05, 0.8, 0.025], facecolor=axcolor)


def sliderChange(val):
    global currentFrame
    currentFrame = val


def sliderChangeSpeed(val):
    global currentSpeed
    currentSpeed = val


splayer = Slider(axPlayer, 'Player', 0, frames_per_path_part * (pathLength - 1), valinit=currentFrame)
splayerSpeed = Slider(axSpeed, 'Speed', 0, 1000, valinit=currentSpeed)
splayer.on_changed(sliderChange)
splayerSpeed.on_changed(sliderChangeSpeed)

plt.show()