def __init__(self,threadlock):
self.x,self.y,self.img = load_circuit('circuit1.png')
self.car = Car(128,400)
self.camera = Camera()
self.line = []
self.distance = []
self.distance.append(self.car.lineposition(self.x,self.y))
self.pilot = CarControl()
self.car.setspeed(10)
self.steps = 30000
self.step = 1.0/1000
self.elapsed = 0
self.camera_refresh_rate = 0.01
self.last_camera_refresh = 0
self.early_stop = False
self.threadlock = threadlock
self.t = threading.Thread(target=self.simulate)
self.t.start()
class CameraPlacement(object):
def __init__(self,
id,
position,
pitch_deg,
yaw_deg,
model,
resolution,
fov,
positional_error_stddev=0.03 / 2.0,
orientation_error_stddev=0.5 / 2.0):
self.id = id
# this will encompass two Cameras
# one true positioned
# one with error
offset_xyz = np.random.normal(loc=0.0,
scale=positional_error_stddev,
size=3)
offset_rp = np.random.normal(loc=0.0,
scale=orientation_error_stddev,
size=2)
# represents camera with errors
self.real_camera = Camera(position=position + offset_xyz,
orientation_pitch_deg=pitch_deg +
offset_rp[0],
orientation_yaw_deg=yaw_deg + offset_rp[1],
verbose=False,
model=model)
# perfectly positioned camera
self.ideal_camera = Camera(position, pitch_deg, yaw_deg, model=model)
self.real_camera.set_resolution(*resolution)
self.real_camera.set_fov(*fov)
self.ideal_camera.set_resolution(*resolution)
self.ideal_camera.set_fov(*fov)
def get_catchment_corners(self):
corners = np.array(self.real_camera.get_corners())
corners[:, 2] = 0
return corners
def get_camera_height(self):
return self.ideal_camera.position[2]
def get_id(self):
return self.id
def __init__(self,
id,
position,
pitch_deg,
yaw_deg,
model,
resolution,
fov,
positional_error_stddev=0.03 / 2.0,
orientation_error_stddev=0.5 / 2.0):
self.id = id
# this will encompass two Cameras
# one true positioned
# one with error
offset_xyz = np.random.normal(loc=0.0,
scale=positional_error_stddev,
size=3)
offset_rp = np.random.normal(loc=0.0,
scale=orientation_error_stddev,
size=2)
# represents camera with errors
self.real_camera = Camera(position=position + offset_xyz,
orientation_pitch_deg=pitch_deg +
offset_rp[0],
orientation_yaw_deg=yaw_deg + offset_rp[1],
verbose=False,
model=model)
# perfectly positioned camera
self.ideal_camera = Camera(position, pitch_deg, yaw_deg, model=model)
self.real_camera.set_resolution(*resolution)
self.real_camera.set_fov(*fov)
self.ideal_camera.set_resolution(*resolution)
self.ideal_camera.set_fov(*fov)
class Simulation:
def __init__(self,threadlock):
self.x,self.y,self.img = load_circuit('circuit1.png')
self.car = Car(128,400)
self.camera = Camera()
self.line = []
self.distance = []
self.distance.append(self.car.lineposition(self.x,self.y))
self.pilot = CarControl()
self.car.setspeed(10)
self.steps = 30000
self.step = 1.0/1000
self.elapsed = 0
self.camera_refresh_rate = 0.01
self.last_camera_refresh = 0
self.early_stop = False
self.threadlock = threadlock
self.t = threading.Thread(target=self.simulate)
self.t.start()
def simulate(self):
for i in range(self.steps):
# Cancel simulation on early stop
if self.early_stop:
break
percent = (i * 100.0) / self.steps
if percent > 0:
if percent % 10 == 0:
print("Simulated "+str(percent)+" %")
self.threadlock.acquire()
# Move simulator in time
self.car.step(self.step)
self.threadlock.release()
# Camera reading
if self.elapsed - self.last_camera_refresh > self.camera_refresh_rate:
self.line = self.camera.get_line((self.car.camleftx[-1],self.car.camlefty[-1]),(self.car.camrightx[-1],self.car.camrighty[-1]),self.img)
self.last_camera_refresh = self.elapsed
self.threadlock.acquire()
linepos = self.car.lineposition(self.x,self.y)
self.threadlock.release()
if linepos is np.NaN:
print("Line lost.")
self.early_stop = True
# Ask user program new values for direction and speed
direction,speed = self.pilot.mainloop(linepos)
# Update car settings according to user values
self.car.setspeed(speed)
self.car.setdirection(direction)
#car.setdirection(percent)
# Register data for plotting
self.distance.append(linepos)
self.elapsed += self.step
print("Simulation complete.")
def stop(self):
self.early_stop = True
