path-optimization/optimize_test.py at main · ds12a/path-optimization · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
from stolen import Context, Environment, CostMap, Trajectory
import numpy as np
import re
import random
from relaxation import Relaxation
from splines import SplineInterpolation
import matplotlib.pyplot as plt
from matplotlib import colors

COSTMAP_FILE = "costmap.txt"
PATH_FILE = "path_history.txt"

context = Context()
env = Environment()
cost_map = CostMap()


with open(COSTMAP_FILE) as f:
   f.readline()
   g = [l.strip() for l in f.readlines()[:-1]]

grid = []
for l in g:
    arr = list(map(lambda v: 240 if v=='3' else 0, re.split(r'[\[\] ,]+', l)[1:-1]))
    grid.append(arr)
cost_map.data = np.asarray(grid, dtype=np.float64)

with open(PATH_FILE) as f:
    arr1 = list(map(int, re.split(r'[\[\] ,\(\)]+', f.readline().strip())[1:-1]))
    arr2 = []
    for i in range(len(arr1) // 2):
        arr2.append([arr1[2 * i], arr1[2 * i + 1]])
trajectory = Trajectory(np.asarray(arr2))

env.cost_map = cost_map
context.env = env


np.set_printoptions(precision=2, suppress=True, linewidth=np.inf)

new_trajectory = Relaxation.relax(context, trajectory, cost_map)
newer_trajectory = SplineInterpolation.interpolate(context, new_trajectory, cost_map)

print(Relaxation.cost_full(context, new_trajectory, cost_map))
# print(Relaxation.cost_full(context, newer_trajectory, cost_map))

fig, ax = plt.subplots()
ax.imshow(255-cost_map.data, cmap='gray', vmin=0, vmax=255)

ax.grid(which='major', axis='both', linestyle='-', color='k', linewidth=1)
ax.set_xticks(np.arange(-.5, len(cost_map.data[0]), 1))
ax.set_yticks(np.arange(-.5, len(cost_map.data), 1))
ax.tick_params(axis='both', which='both', labelsize=0)

plt.plot(trajectory.coordinates[:, 0], trajectory.coordinates[:, 1], 'o', color='r', )
plt.plot(new_trajectory.coordinates[:, 0], new_trajectory.coordinates[:, 1], 'o')
plt.plot(newer_trajectory.coordinates[:, 0], newer_trajectory.coordinates[:, 1], '-')

plt.show()