def test():
the_map = Map()
tic = time.clock()
the_map.discretize_map()
print time.clock() - tic
#obstacle = Polygon([(40,15), (45,15), (45,20), (40,20)], safety_region_length=4.0)
#the_map.add_obstacles([obstacle])
tend = 10
dT = 1
h = 0.05
N = int(tend/h) + 1
N2 = int(tend/dT) + 1
x0 = np.array([10, 10, 0.0, 3.0, 0, 0])
xg = np.array([50, 50, 0])
myDynWnd = DynamicWindow(dT, N2)
v = Vessel(x0, xg, h, dT, N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
v.current_goal = np.array([50, 50])
world = World([v], the_map)
myDynWnd.the_world = world
world.update_world(0,0)
fig = plt.figure()
ax = fig.add_subplot(111, aspect='equal', autoscale_on=False,
xlim=(-10, 160), ylim=(-10, 160))
world.visualize(ax, 0, 0)
plt.show()
def simple_scenario_with_plot():
the_map = Map('s1')
#obstacle = Polygon([(30,15), (35,15), (35,20), (30,20)], safety_region_length=4.0)
#the_map.add_obstacles([obstacle])
tend = 50
dT = 1
h = 0.05
N = int(tend / h) + 1
N2 = int(tend / dT) + 1
x0 = np.array([0, 0, 0.5, 3.0, 0, 0])
xg = np.array([120, 120, 0])
myDynWnd = DynamicWindow(dT, N2)
v = Vessel(x0,
xg,
h,
dT,
N, [myDynWnd],
is_main_vessel=True,
vesseltype='viknes')
v.current_goal = np.array([50, 50])
world = World([v], the_map)
myDynWnd.the_world = world
n = 0
for t in np.linspace(0, tend, N):
world.update_world(t, n, dT)
n += 1
fig = plt.figure()
ax = fig.add_subplot(111,
aspect='equal',
autoscale_on=False,
xlim=(-10, 10),
ylim=(-10, 10))
#fig, ax = plt.subplots()
ax.grid()
world.draw(ax, N)
print "N, N2, n: ", N, N2, n
myDynWnd.draw(ax, N2)
plt.show()
def test():
the_map = Map()
tic = time.clock()
the_map.discretize_map()
print time.clock() - tic
#obstacle = Polygon([(40,15), (45,15), (45,20), (40,20)], safety_region_length=4.0)
#the_map.add_obstacles([obstacle])
tend = 10
dT = 1
h = 0.05
N = int(tend / h) + 1
N2 = int(tend / dT) + 1
x0 = np.array([10, 10, 0.0, 3.0, 0, 0])
xg = np.array([50, 50, 0])
myDynWnd = DynamicWindow(dT, N2)
v = Vessel(x0,
xg,
h,
dT,
N, [myDynWnd],
is_main_vessel=True,
vesseltype='viknes')
v.current_goal = np.array([50, 50])
world = World([v], the_map)
myDynWnd.the_world = world
world.update_world(0, 0)
fig = plt.figure()
ax = fig.add_subplot(111,
aspect='equal',
autoscale_on=False,
xlim=(-10, 160),
ylim=(-10, 160))
world.visualize(ax, 0, 0)
plt.show()
def simple_scenario_with_plot():
the_map = Map('s1')
#obstacle = Polygon([(30,15), (35,15), (35,20), (30,20)], safety_region_length=4.0)
#the_map.add_obstacles([obstacle])
tend = 50
dT = 1
h = 0.05
N = int(tend/h) + 1
N2 = int(tend/dT) + 1
x0 = np.array([0, 0, 0.5, 3.0, 0, 0])
xg = np.array([120, 120, 0])
myDynWnd = DynamicWindow(dT, N2)
v = Vessel(x0, xg, h, dT, N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
v.current_goal = np.array([50, 50])
world = World([v], the_map)
myDynWnd.the_world = world
n = 0
for t in np.linspace(0, tend, N):
world.update_world(t, n, dT)
n += 1
fig = plt.figure()
ax = fig.add_subplot(111, aspect='equal', autoscale_on=False,
xlim=(-10, 10), ylim=(-10, 10))
#fig, ax = plt.subplots()
ax.grid()
world.draw(ax, N)
print "N, N2, n: ", N, N2, n
myDynWnd.draw(ax, N2)
plt.show()