class Test_Car():
car_1 = Car(v=5)
car_2 = Car([10,0,0], 0, 0, 0)
def test_create_class(self):
assert self.car_1.pos == [0,0,0]
assert self.car_1.orientation == 0
assert self.car_1.v == 5
assert self.car_1.mode == 'constant acceleration'
assert self.car_1.b_static == False
def test_generate_outline(self):
outline = self.car_1.get_outline()
for line in outline:
print(line)
def test_viz(self):
plt.figure(1, figsize=(20,12))
self.car_1.viz()
plt.pause(1)
def test_get_object(self):
obj_1 = self.car_1.get_object([1,1,0])
assert obj_1.velocity == 5
assert obj_1.close_point == (2, 1.75/2)
obj_2 = self.car_2.get_object([0,0,0])
print(obj_2.close_point)
print(obj_2.other_points)
plt.figure(figsize=(20,12))
obj_2.viz()
plt.pause(1)
class Test_Lidar():
ego_pos = [0, 0, 0]
lidar = Lidar([0, 0, 0], 10, 0)
car_1 = Car([10, 5, 0], 0, 0, 0)
car_2 = Car([10, 0, 0], 0, 0, 0)
objs = [car_1.get_object(ego_pos), car_2.get_object(ego_pos)]
def test_create_lidar(self):
assert self.lidar.abs_pos == [0, 0, 0]
assert self.lidar.rel_pos == [0, 0, 0]
assert self.lidar.freq == 10
assert self.lidar.orientation == 0
def test_read(self):
sensor_data = self.lidar.read(self.objs, 0)
for data in sensor_data.data:
print(data.scan)
def test_find_visible_edges(self):
for obj in self.objs:
visible_edges = self.lidar.find_visible_edges(obj)
print(visible_edges)
def test_viz(self):
plt.figure(figsize=(20, 12))
_ = self.lidar.read(self.objs, 0)
self.lidar.viz()
plt.pause(1)
def test_read(self):
car = Car()
objs = [car.get_object([-10, 1, 0])]
sensor_data = self.radar.read(objs, 0)
print('testing reading radar')
print(sensor_data.data)
car.viz()
self.radar.viz()
plt.pause(0.5)
class Test_Line_Integration():
from src.simulator.car import Car
from src.sensor.lidar import Lidar, LidarData
ego_pos = [0, 0, 0]
car = Car([10, 5, 0], 0, 0, 0)
objs = [car.get_object(ego_pos)]
lidar = Lidar((0, 0, 0), 10, 0, abs_pos=ego_pos)
sensor_data = lidar.read(objs, 0)
scan = sensor_data.data[0].scan
def test_find_line(self):
plt.figure(1, figsize=(20, 12))
print(self.scan)
line_feature_1, is_line_1, line_error_1 = Line.find_line(self.scan)
print(line_feature_1)
print(is_line_1)
self.lidar.viz()
line_feature_1.viz()
_, points_far = line_feature_1.find_points_close_line(
self.scan, line_feature_1.close_threshold)
plt.figure(2)
self.lidar.viz()
plt.scatter([point[0] for point in points_far],
[point[1] for point in points_far],
marker='o',
c='r',
label='far')
plt.legend()
plt.show()
line_feature_2, is_line_2, line_error_2 = Line.find_line(points_far)
line_feature_2.viz()
class Test_LidarProc():
ego_pos = [0, 0, 0]
car = Car([10, 5, 0], 0, 0, 0)
objs = [car.get_object(ego_pos)]
lidar = Lidar((0, 0, 0), 10, 0, abs_pos=ego_pos)
sensor_data = lidar.read(objs, 0)
data = sensor_data.data[0]
def test_find_feature(self):
plt.figure(figsize=(20, 12))
feature = LidarProc.find_feature(self.data.scan)
print(feature)
# self.car.viz()
feature.viz()
self.lidar.viz()
plt.gca().axis('equal')
plt.pause(2)
plt.close()
def test_find_model_from_feature(self):
plt.figure(figsize=(20, 12))
feature = LidarProc.find_feature(self.data.scan)
feature.viz()
models = LidarProc.find_models_from_feature(feature,
self.data.sensor_pos)
print(models)
for model in models:
model.viz()
plt.pause(2)
def __init__(self,
gen_mode='constant',
fig_name=None,
object_num=1,
probability=0.1,
obj_mode='static'):
''' Road Simulator class
Args:
mode='constant': defines the mode of generating objects,
'constant' keeps the number of object specified by object_num,
'random' keeps the probability of generating object
object_num=1: defines the number of objects expected
probability=0.1: defines the probability of generating objects
obj_mode='static': defines the motion type of the generated objects.
'static' for not moving
'keeping' for random speed but keep it.
'accelerate' for random acceleration from zero velocity.
'''
self.gen_mode = gen_mode
self.object_num = object_num
self.gen_prob = probability
self.obj_mode = obj_mode
self.objects = []
self.boundary_offset = (-20, -10, 20, 10)
self.boundary = [-20, -10, 20, 10]
self.lane_width = 4
self.ego_object = Car(position=[0, -1 * self.lane_width / 2, 0],
orientation=0,
v=15,
a=0,
mode='constant acceleration')
self.road = Road(start=(0, 0, 0),
end=(100, 0, 0),
lane_num=1,
lane_width=self.lane_width)
self.fig_name = fig_name
self.time_acc = 0
self.velocity_max = 20
self.velocity_gen = self.velocity_max
def generate_an_object(self, pos, orientation):
mode = 'constant acceleration'
if self.obj_mode == 'static':
v = 0
a = 0
elif self.obj_mode == 'keeping':
v = random.randint(0, self.velocity_gen)
a = 0
elif self.obj_mode == 'accelerating':
v = 0
a = random.randint(0, 10)
else:
raise NotImplementedError
self.objects.append(Car(pos, orientation, v, a, mode))
class RoadSimulator(Simulator):
def __init__(self,
gen_mode='constant',
fig_name=None,
object_num=1,
probability=0.1,
obj_mode='static'):
''' Road Simulator class
Args:
mode='constant': defines the mode of generating objects,
'constant' keeps the number of object specified by object_num,
'random' keeps the probability of generating object
object_num=1: defines the number of objects expected
probability=0.1: defines the probability of generating objects
obj_mode='static': defines the motion type of the generated objects.
'static' for not moving
'keeping' for random speed but keep it.
'accelerate' for random acceleration from zero velocity.
'''
self.gen_mode = gen_mode
self.object_num = object_num
self.gen_prob = probability
self.obj_mode = obj_mode
self.objects = []
self.boundary_offset = (-20, -10, 20, 10)
self.boundary = [-20, -10, 20, 10]
self.lane_width = 4
self.ego_object = Car(position=[0, -1 * self.lane_width / 2, 0],
orientation=0,
v=15,
a=0,
mode='constant acceleration')
self.road = Road(start=(0, 0, 0),
end=(100, 0, 0),
lane_num=1,
lane_width=self.lane_width)
self.fig_name = fig_name
self.time_acc = 0
self.velocity_max = 20
self.velocity_gen = self.velocity_max
def simulate(self, dt):
''' simulate the system dynamics for dt
param:
dt: time length
return:
objs: objects information for sensors
time_acc: accumulated simulation time
'''
self.clean_objects()
self.generate_objects()
self.ego_object.simulate(dt)
for obj in self.objects:
if not obj.b_static:
obj.simulate(dt)
self.update_boundary()
self.viz(dt)
objs = self.get_objects(self.ego_object.pos)
self.time_acc += dt
return objs, self.time_acc
def get_objects(self, pos):
objs = [obj.get_object(pos) for obj in self.objects]
return objs
def clean_objects(self):
""" clean up the out of bound objects """
new_objects = []
for obj in self.objects:
if self.is_pos_in_boundary(obj.pos):
new_objects.append(obj)
self.objects = new_objects
def generate_objects(self):
""" generate objects at the boundary """
self.get_max_velocity()
pos, orientation = self.get_generating_point()
for obj in self.objects:
if obj.is_distance_safe(pos) == False:
return
if self.gen_mode == 'constant':
self.generate_objects_constant(pos, orientation)
elif self.gen_mode == 'random':
self.generate_objects_random(pos, orientation)
def get_max_velocity(self):
if len(self.objects) > 0:
self.velocity_gen = min(self.velocity_max,
min([obj.v for obj in self.objects]))
else:
self.velocity_gen = self.velocity_max
def generate_objects_constant(self, pos, orientation):
if len(self.objects) < self.object_num:
self.generate_an_object(pos, orientation)
def generate_objects_random(self, pos, orientation):
if random.random() < self.gen_prob:
self.generate_an_object(pos, orientation)
def generate_an_object(self, pos, orientation):
mode = 'constant acceleration'
if self.obj_mode == 'static':
v = 0
a = 0
elif self.obj_mode == 'keeping':
v = random.randint(0, self.velocity_gen)
a = 0
elif self.obj_mode == 'accelerating':
v = 0
a = random.randint(0, 10)
else:
raise NotImplementedError
self.objects.append(Car(pos, orientation, v, a, mode))
def get_generating_point(self):
''' return the position and orientation an object an be generated.'''
return self.road.get_generating_point(self.boundary)
def update_boundary(self):
''' get the new boundary around the ego_vehicle '''
self.boundary[0] = self.ego_object.pos[0] + self.boundary_offset[0]
self.boundary[1] = self.ego_object.pos[1] + self.boundary_offset[1]
self.boundary[2] = self.ego_object.pos[0] + self.boundary_offset[2]
self.boundary[3] = self.ego_object.pos[1] + self.boundary_offset[3]
def is_pos_in_boundary(self, pos):
if ( self.boundary[0] <= pos[0] <= self.boundary[2] and \
self.boundary[1] < pos[1] < self.boundary[3]):
return True
else:
return False
def viz(self, dt):
if self.fig_name is None:
pass
else:
plt.figure(self.fig_name)
plt.gca().set_xlim([self.boundary[0], self.boundary[2]])
plt.gca().set_ylim([self.boundary[1], self.boundary[3]])
self.ego_object.viz()
self.road.viz()
for obj in self.objects:
obj.viz()
plt.title('world {:.3f}, dt={:.3f}'.format(self.time_acc, dt))