def __init__(self,
position: Vector2 = Vector2(0, 0),
orientation: float = 90.0,
color: Tuple[float, float, float] = (0, 0, 0),
mesh: List[Vector2] = (Vector2(1.25, 2), Vector2(-1.25, 2),
Vector2(-1.25, -2), Vector2(1.25, -2)),
velocity: float = 0.0,
max_velocity: float = 10.0,
acceleration: float = 0.0,
max_acceleration: float = 1.0):
super(Vehicle, self).__init__(position, orientation, color, velocity, max_velocity,
acceleration, max_acceleration, mesh)
self.__target = None # type: Vector2
self.trajectory = [] # type: [Vector2]
self.active = False
self.length = self.mesh[0][1] - self.mesh[2][1] # TODO: length and width specifically for rectangular vehicles!
self.width = self.mesh[0][0] - self.mesh[1][0]
self.driver = IntelligentDriver()
self.turn_signal = Vehicle.TURN_SIGNAL_NONE
self.events = []
self.lane = None # type: Lane
self.change_permit = False
self.respawn = True
self.blocker = None
self.logger = logging.getLogger('simulation.agent.Vehicle')
def test_dot(self):
v = Vector2(1, 0)
u = Vector2(0, 1)
w = Vector2(2, 1)
self.assertTrue(v.dot(u) == 0)
self.assertTrue(u.dot(v) == 0)
self.assertTrue(v.dot(w) == 2)
def test_length_squared(self):
v = Vector2(1, 0)
u = Vector2(0, -1)
w = Vector2(1, 1)
self.assertTrue(v.length_squared() == 1)
self.assertTrue(u.length_squared() == 1)
self.assertTrue(w.length_squared() == 2)
def handle_input(self) -> None:
if not self.interactive:
return
k = cv2.waitKey(1)
if k == 27 or k == 1048603: # wait for ESC key to exit
self.quit = True
cv2.destroyAllWindows()
return
if k == 119 or k == 1048695: # W
self.camera.move(Vector2(0, 10.0) / self.camera.zoom)
if k == 115 or k == 1048691: # S
self.camera.move(Vector2(0.0, -10.0) / self.camera.zoom)
if k == 100 or k == 1048676: # D
self.camera.move(Vector2(10.0, 0.0) / self.camera.zoom)
if k == 97 or k == 1048673: # A
self.camera.move(Vector2(-10.0, 0) / self.camera.zoom)
if k == 113 or k == 1048689: # Q
self.camera.orientation -= 5
if k == 101 or k == 1048677: # E
self.camera.orientation += 5
if k == 114 or k == 1048690: # R
self.camera.zoom *= 1.1
if k == 102 or k == 1048678: # F
self.camera.zoom /= 1.1
if k == 109 or k == 1048685: # M
self.mode = Renderer.Mode(
(self.mode.value + 1) % len(Renderer.Mode))
if k == 112 or k == 1048688: # P
self.pause = not self.pause
def test_truediv(self):
v = Vector2(3, 2)
u = Vector2(6, 4)
w = Vector2(-3, -2)
self.assertTrue(v / 0.5 == u)
self.assertTrue(v / -1 == w)
u /= 2
self.assertTrue(v == u)
def test_distance_squared(self):
v = Vector2(1, 0)
u = Vector2(0, 1)
w = Vector2(-1, 0)
eps = 0.001
self.assertTrue(abs(v.distance_squared(u) - 2) < eps)
self.assertTrue(abs(u.distance_squared(v) - 2) < eps)
self.assertTrue(abs(v.distance_squared(w) - 4) < eps)
def test_abs(self):
v = Vector2(1, 0)
u = Vector2(0, -1)
w = Vector2(1, 1)
eps = 0.001
self.assertTrue(abs(v) == 1)
self.assertTrue(abs(u) == 1)
self.assertTrue(abs(abs(w) - math.sqrt(2)) < eps)
def test_distance(self):
v = Vector2(1, 0)
u = Vector2(0, 1)
w = Vector2(-1, 0)
eps = 0.001
self.assertTrue(abs(v.distance(u) - math.sqrt(2)) < eps)
self.assertTrue(abs(u.distance(v) - math.sqrt(2)) < eps)
self.assertTrue(abs(v.distance(w) - 2) < eps)
def test_eq(self):
v = Vector2(3, 2)
u = Vector2(3, 2)
w = Vector2(7, 6)
self.assertTrue(v == u)
self.assertFalse(u == v)
self.assertFalse(v == w)
self.assertFalse(w == u)
def test_mul(self):
v = Vector2(3, 2)
u = Vector2(6, 4)
w = Vector2(-3, -2)
self.assertTrue((v * 2 == 2 * v) and (v * 2 == u))
self.assertTrue((v * -1 == -1 * v) and (v * -1 == w))
v *= 2
self.assertTrue(v == u)
def test_length(self):
v = Vector2(1, 0)
u = Vector2(0, -1)
w = Vector2(1, 1)
eps = 0.001
self.assertTrue(v.length() == 1)
self.assertTrue(u.length() == 1)
self.assertTrue(abs(w.length() - math.sqrt(2)) < eps)
def test_rotate(self):
v = Vector2(1, 0)
u = Vector2(0, 1)
w = Vector2(1 / math.sqrt(2), 1 / math.sqrt(2))
eps = 0.001
v.rotate(-90)
self.assertTrue(abs(v.x - u.x) < eps and abs(v.y - u.y) < eps)
u.rotate(45)
self.assertTrue(abs(w.x - u.x) < eps and abs(w.y - u.y) < eps)
def __init__(self, position: Vector2=Vector2(0, 0)):
super(Obstacle, self).__init__(position,
orientation=90,
color=(0, 0, 0),
mesh=[Vector2(0.5, 0.5), Vector2(-0.5, 0.5),
Vector2(-0.5, -0.5), Vector2(0.5, -0.5)],
velocity=0,
max_velocity=0,
acceleration=0,
max_acceleration=0)
self.driver = DummyDriver()
def test_normalize(self):
v = Vector2(0.5, 0)
u = Vector2(0, -0.5)
w = Vector2(1, 1)
v.normalize()
u.normalize()
w.normalize()
eps = 0.001
self.assertTrue(v == Vector2(1, 0))
self.assertTrue(u == Vector2(0, -1))
self.assertTrue(abs(w.x - 1 / math.sqrt(2)) < eps and w.x == w.y)
def update_vehicle(self, delta_time: float) -> None:
self.velocity += delta_time * self.acceleration
heading = Vector2(0, 1)
heading.rotate(self.orientation)
self.position += delta_time * self.velocity * heading
direction = self.position - self.target
direction.normalize()
angle = Vector2(0, 1).distance_angular_signed(direction)
# check if agent passed its target
if abs(angle - self.orientation) < Lane.EPSILON:
self.lane.fix_vehicle_position(self)
def __init__(self,
width: int = 512,
height: int = 256,
zoom: float = 2.0,
target: SimulationObject = None):
super(Camera2D, self).__init__()
self.__viewport = Vector2(width, height) # type: Vector2
self.__offset = Vector2(self.__viewport.x / 2,
self.__viewport.y / 2) # type: Vector2
self.zoom = zoom # type: float
self.target = target # type: SimulationObject
self.distance_threshold = 0.05 # type: float
def move(self, distance: Vector2) -> None:
"""Move the camera a given distance in the camera's perspective"""
heading = Vector2(0, 1)
heading.rotate(self.orientation)
normal = heading.copy()
normal.rotate(90)
self.position += heading * distance.y + normal * distance.x
def projected_velocity(self, vehicle):
"""Project the velocity ot the given agent on the lane.
:param vehicle: agent of which to project the velocity
:return: projected velocity
"""
if vehicle is None:
return None
first, second, lane = self.find_closest_segment(vehicle.position)
if second is None:
if first == 0:
first, second = 0, 1
else:
first, second = first - 1, first
if first > second:
first, second = second, first
direction = (lane.center[second] - lane.center[first])
direction.normalize()
heading = Vector2(0, 1)
heading.rotate(vehicle.orientation)
directed_velocity = vehicle.velocity * heading
projected_velocity = directed_velocity.dot(direction)
if projected_velocity < 0.0:
projected_velocity *= -1
return projected_velocity
def apply_inverse_view_transform(self, vector: Vector2) -> Vector2:
"""Transform a vector from the camera coordinate system of this camera to the world coordinate system"""
result = vector.copy()
result.y = self.viewport.y - result.y
result -= self.__offset
result /= self.zoom
result.rotate(self.orientation)
result += self.position
return result
def mouse_callback(self, event: int, x: int, y: int, flags: int,
param: int) -> None:
window_position = Vector2(x, y)
world_position = self.camera.apply_inverse_view_transform(
window_position)
if event == cv2.EVENT_LBUTTONDBLCLK:
print('(' + str(world_position.x) + ', ' + str(world_position.y) +
')')
if event == cv2.EVENT_RBUTTONDOWN:
self.mouse_drag = True
self.ix, self.iy = x, y
self.last_position = self.camera.position.copy()
elif event == cv2.EVENT_RBUTTONUP:
self.mouse_drag = False
elif event == cv2.EVENT_MOUSEMOVE and self.mouse_drag:
movement = Vector2(x, y) - Vector2(self.ix, self.iy)
self.camera.position = self.last_position.copy()
self.camera.position += Vector2(-movement.x,
movement.y) / self.camera.zoom
def __init__(self, camera: Camera2D):
self.camera = camera # type: Camera2D
self.camera.viewport = Vector2(cr.CONFIG.getint('renderer', 'width'),
cr.CONFIG.getint('renderer', 'height'))
self.camera.zoom = cr.CONFIG.getfloat('renderer',
'zoom') # type: float
self.interactive = cr.CONFIG.getboolean('renderer',
'interactive') # type: bool
self.mode = Renderer.Mode.SURFACE # type Enum
self.background_brightness = 1.0 # type: int
def __init__(self, path, num_lanes):
super(Road, self).__init__(position=Vector2(),
orientation=0,
color=(0.5, 0.5, 0.5),
mesh=[])
self.lanes = [] # type: [Lane]
self.create_lanes(path, num_lanes)
for node in self.lanes[0].right:
self.mesh.append(node)
for node in reversed(self.lanes[-1].left):
self.mesh.append(node)
def move_camera(self) -> None:
if self.step % self.writer.write_sample_size == 0:
location = self.rng.randint(0, len(self.locations) - 1)
self.camera.position = self.locations[location][0].copy()
self.camera.orientation = self.locations[location][1]
if self.jitter:
r = self.rng.uniform(-self.jitter_size[2], self.jitter_size[2])
f = self.rng.randint(0, 1)
s = self.rng.uniform(-self.jitter_size[1], self.jitter_size[1])
t = self.rng.uniform(-self.jitter_size[0], self.jitter_size[0])
self.camera.position += Vector2(t, 1 + s)
self.camera.orientation += r + f * 180.0
def __init__(self,
position: Vector2 = Vector2(0, 0),
orientation: float = 0.0,
color: Tuple[float, float, float] = (0.0, 0.0, 0.0),
velocity: float = 0.0,
max_velocity: float = 0.0,
acceleration: float = 0.0,
max_acceleration: float = 0.0,
mesh: List[Vector2] = None):
if mesh is None:
mesh = [
Vector2(0.5, 0.5),
Vector2(-0.5, 0.5),
Vector2(-0.5, -0.5),
Vector2(0.5, -0.5)
]
self.position = Vector2(position.x, position.y) # type: Vector2
self.orientation = orientation # type: float
self.color = color # type: Tuple[float, float, float]
self.mesh = mesh # type: List[Vector2]
self.active = True # type: bool
self.__acceleration = acceleration # type: float
self.max_acceleration = max_acceleration # type: float
self.__velocity = velocity # type: float
self.max_velocity = max_velocity # type: float
self.id = SimulationObject.ID # type: int
SimulationObject.ID += 1
def test_distance_angular_unsigned(self):
v = Vector2(1, 0)
u = Vector2(0, 2)
w = Vector2(-3, 0)
t = Vector2(-3, -0.1)
s = Vector2(-3, 0.1)
eps = 0.001
self.assertTrue(
abs(v.distance_angular_unsigned(u)) - 90 < eps
and v.distance_angular_unsigned(u) > 0)
self.assertTrue(
abs(u.distance_angular_unsigned(v)) - 90 < eps
and u.distance_angular_unsigned(v) > 0)
self.assertTrue(
abs(v.distance_angular_unsigned(w)) - 180 < eps
and v.distance_angular_unsigned(w) > 0)
self.assertTrue(
abs(v.distance_angular_unsigned(t)) - 178.0908 < eps
and v.distance_angular_unsigned(t) > 0)
self.assertTrue(
abs(v.distance_angular_unsigned(s)) - 178.0908 < eps
and v.distance_angular_unsigned(s) > 0)
def __init__(self, road):
super(Lane, self).__init__(position=Vector2(0, 0),
orientation=0,
color=(0.5, 0.5, 0.5),
mesh=[])
self.road = road # type: Road
self.left = [] # type: [Vector2]
self.center = [] # type: [Vector2]
self.right = [] # type: [Vector2]
self.accumulated_distance = [] # type: [Vector2]
self.vehicles = [] # type: [Vehicle]
self.neighboring_lanes = [] # type: [Lane]
self.left_neighbor = None # type: Lane
self.right_neighbor = None # type: Lane
self.back_connection = None # type: Lane
self.front_connection = None # type: Lane
self.lane_change_threshold = 0.0
self.closest_node_hash = {}
self.closest_segment_hash = {}
def test_str(self):
v = Vector2(1, 1)
self.assertTrue(str(v) == "(1, 1)")
def test_setitem(self):
v = Vector2(1, 2)
v[0] = 4
v[1] = 5
self.assertTrue(v.x == 4)
self.assertTrue(v.y == 5)
def test_getitem(self):
v = Vector2(1, 2)
self.assertTrue(v[0] == v.x)
self.assertTrue(v[1] == v.y)
def test_sub(self):
v = Vector2(3, 2)
u = Vector2(4, 4)
w = Vector2(7, 6)
self.assertTrue(w - u == v)
self.assertFalse(u - w == v)
