Skip to content
Snippets Groups Projects
Commit 4ad877a7 authored by Maximilian Gruber's avatar Maximilian Gruber
Browse files

Minor restructuring

parent 0bfcc24c
No related branches found
No related tags found
No related merge requests found
...@@ -3,6 +3,7 @@ import matplotlib as mpl ...@@ -3,6 +3,7 @@ import matplotlib as mpl
import itertools import itertools
import time import time
class Vehicle: class Vehicle:
def __init__(self, mass=1.0, floor=None): def __init__(self, mass=1.0, floor=None):
...@@ -18,7 +19,6 @@ class Vehicle: ...@@ -18,7 +19,6 @@ class Vehicle:
self.floor = floor self.floor = floor
def get_wheel_positions(self): def get_wheel_positions(self):
# build rotation matrix R # build rotation matrix R
c = np.cos(self.rotation) c = np.cos(self.rotation)
...@@ -28,26 +28,6 @@ class Vehicle: ...@@ -28,26 +28,6 @@ class Vehicle:
# return absolute wheel positions # return absolute wheel positions
return self.position + np.matmul(R, self.wheels_position_delta.T).T return self.position + np.matmul(R, self.wheels_position_delta.T).T
def representation(self):
# represent vehicle by a rectangle from wheel positions
shape = mpl.patches.Polygon(self.wheels_position, fill=False)
# arrow direction
x = self.position[0]
y = self.position[1]
dx = 1.0
dy = np.tan(self.rotation + np.pi/2)
l = np.sqrt(dx**2 + dy**2)
arrow = mpl.patches.Arrow(x, y, dx/l, dy/l, width=0.1, fill=False)
return shape, arrow
def update_representation(self, shape, arrow):
pass
def update_floor(self): def update_floor(self):
# could be optimized by binary search tree and fixed length of sensor time-series # could be optimized by binary search tree and fixed length of sensor time-series
f = self.floor f = self.floor
...@@ -68,7 +48,6 @@ class Vehicle: ...@@ -68,7 +48,6 @@ class Vehicle:
sensor.values[-1] += self.wheels_weight[iw] * self.mass sensor.values[-1] += self.wheels_weight[iw] * self.mass
break break
def is_inside(self, wheel_position, tile): def is_inside(self, wheel_position, tile):
b = np.append(wheel_position,1) b = np.append(wheel_position,1)
c = tile["corners"] c = tile["corners"]
...@@ -92,6 +71,29 @@ class Vehicle: ...@@ -92,6 +71,29 @@ class Vehicle:
return False return False
class VehicleInteractor:
def __init__(self, vehicle, ax):
self.vehicle = vehicle
self.ax = ax
def get_shape_and_arrow(self):
# represent vehicle by a rectangle from wheel positions
shape = mpl.patches.Polygon(self.vehicle.wheels_position, fill=False)
# arrow direction
x = self.vehicle.position[0]
y = self.vehicle.position[1]
dx = 1.0
dy = np.tan(self.vehicle.rotation + np.pi/2)
l = np.sqrt(dx**2 + dy**2)
arrow = mpl.patches.Arrow(x, y, dx/l, dy/l, width=0.1, fill=False)
return shape, arrow
def update_representation(self, shape, arrow):
pass
def connect(self): def connect(self):
pass pass
...@@ -108,15 +110,14 @@ class Vehicle: ...@@ -108,15 +110,14 @@ class Vehicle:
class Floor(): class Floor():
def __init__(self): def __init__(self, shape=(10,12), tile_length=0.5):
self.shape = (10,12) self.shape = shape
self.tile_length = 0.5 # meter self.tile_length = tile_length # meter
# #
self.tiles = self.init_floor_tiles() self.tiles = self.init_floor_tiles()
self.draw_limits = np.array([[0, self.shape[0]+1], [0, self.shape[1]+1]]) * self.tile_length self.draw_limits = np.array([[0, self.shape[0]+1], [0, self.shape[1]+1]]) * self.tile_length
def init_floor_tiles(self): def init_floor_tiles(self):
tiles = {} tiles = {}
...@@ -131,7 +132,6 @@ class Floor(): ...@@ -131,7 +132,6 @@ class Floor():
return tiles return tiles
def get_grid(self): def get_grid(self):
x = np.linspace(0, self.shape[0], self.shape[0]+1) * self.tile_length x = np.linspace(0, self.shape[0], self.shape[0]+1) * self.tile_length
y = np.linspace(0, self.shape[1], self.shape[1]+1) * self.tile_length y = np.linspace(0, self.shape[1], self.shape[1]+1) * self.tile_length
......
from base import Vehicle, Floor from base import Vehicle, Floor, VehicleInteractor
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
...@@ -10,10 +10,10 @@ v.update_floor() ...@@ -10,10 +10,10 @@ v.update_floor()
fig = plt.figure() fig = plt.figure()
ax = fig.add_subplot(111) ax = fig.add_subplot(111)
# vehicle representation # vehicle representation
vr = v.representation() vi = VehicleInteractor(v, ax)
v_shape = vr[0] v_shape, v_arrow = vi.get_shape_and_arrow()
v_arrow = vr[1]
ax.add_patch(v_shape) ax.add_patch(v_shape)
ax.add_patch(v_arrow) ax.add_patch(v_arrow)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment