def __init__(self, vr: Any, car: Car, target_speed: float) -> None:
# You may initialize additional state variables here
self.last_sim_time = car.get_sim_time()
self.target_speed = target_speed
self.old_lat_err = 0.0
self.int_err = 0.0 # integral error
def setup_car(self, vr: Any, car: Car) -> None:
"""Sets up the car's physical parameters.
"""
#
# ASSIGNMENT: You may want to tune these paraneters.
#
# car.set_boom_sensor_offset(0.1)
# In the scene, we provide two cameras.
# Camera 0 is used by default in the control loop and is the "near" one.
# Some teams have also used a "far" camera in the past (Camera 1).
# You can use additional cameras, but will have to add then in the V-REP scene
# and bind the handle in Car.__init__. The V-REP remote API doesn't provide a
# way to instantiate additional vision sensors.
car.set_line_camera_parameters(0, height=0.3, orientation=55, fov=90)
car.set_line_camera_parameters(1, height=0.4, orientation=15, fov=60)
# You should measure the steering servo limit and set it here.
# A more accurate approach would be to implement servo slew limiting.
car.set_steering_limit(30)
def control_loop(self, vr: Any, car: Car,
csvfile: Optional[SimpleCsvDictWriter]) -> None:
"""Control iteration. This is called on a regular basis.
Args:
vr -- VRepInterface object, which is an abstraction on top of the VREP
Python API. See the class docstring near the top of this file.
car -- Car object, providing abstractions for the car (like steering and
velocity control).
csvfile -- Optional to log data. None to disable.
"""
sim_time = car.get_sim_time()
dt = sim_time - self.last_sim_time
self.last_sim_time = sim_time
#
# ASSIGNMENT: Tune / implement a better controller loop here.
#
# Here are two different sensors you can play with.
# One provides an ideal shortest-distance-to-path (in meters), but isn't
# path-following and may jump at crossings.
# The other uses a more realistic line sensor (in pixels), which you can
# plug your track detection algorithm into.
line_camera_image0 = car.get_line_camera_image(0)
# line0_err = self.get_line_camera_error(car.get_line_camera_image(0))
line0_err = self.get_line_camera_error(self.oldimage,
car.get_line_camera_image(0),
self.old_lat_err)
#line1_err = self.get_line_camera_error(line_camera_image1, car.get_line_camera_image(1), car.old_lat_err)
line1_err = 0
self.oldimage = line_camera_image0
# line camera has 0.7 m field of view
lat_err = -(np.float(line0_err) /
128) * 0.7 # pixel to meter conversion
if dt > 0.0:
lat_vel = (lat_err - self.old_lat_err) / dt
else:
lat_vel = 0.0
self.old_lat_err = lat_err
# calculate integral error
self.int_err = self.int_err + dt * lat_err
# Proportional gain in steering control (degrees) / lateral error (meters)
kp = 195
kd = 0.5 * kp # deg per m/s
ki = 0.4 * kp # deg per m-s
steer_angle = -kp * lat_err - kd * lat_vel - ki * self.int_err
steer_angle = car.set_steering(
steer_angle,
dt) # use set_steering to include servo slew rate limit
# steer_angle = car.set_steering_fast(steer_angle,dt) # use set_steering_fast for no delay
# Constant speed for now. You can tune this and/or implement advanced controllers.
car.set_speed(3)
# Print out and record debugging info
pos = car.get_position()
vel_vector = car.get_velocity()
vel = math.sqrt(vel_vector[0]**2 + vel_vector[1]**2 + vel_vector[2]**2)
print(
't=%6.3f (x=%5.2f, y=%5.2f, sp=%5.2f): lat_err=%5.2f, int_err=%5.2f, line0_err=%3i, steer_angle=%3.1f'
% (sim_time, pos[0], pos[1], vel, lat_err, self.int_err,
(line0_err or 0), steer_angle))
if csvfile is not None:
csvfile.writerow({
't': sim_time,
'x': pos[0],
'y': pos[1],
'linescan': line_camera_image0,
'line_pos': line0_err +
63, # needs to be in camera pixels so overlaid plots work
#'linescan_far': line_camera_image1,
'line_pos_far': line1_err + 63,
'speed': vel,
'lat_err': lat_err,
'steer_angle': steer_angle,
})
# Stop the existing simulation if requested. Not using a separate
# VRepInterface (and hence VREP API client id) seems to cause crashes.
if args.restart:
with vrepInterface.VRepInterface.open() as vr:
vr.simxStopSimulation(vrep.simx_opmode_oneshot_wait)
# This needs not be in the main body, or the API throws an exception a split second after simulation start.
with vrepInterface.VRepInterface.open() as vr:
ret = vr.simxStartSimulation(vrep.simx_opmode_oneshot_wait)
# Open a V-REP API connection and get the car.
with vrepInterface.VRepInterface.open() as vr:
if args.synchronous:
vr.simxSynchronous(1)
car = Car(vr)
wire = Tripwire(vr)
success = False
while not success:
try:
car.get_sim_time()
success = True
except vrepInterface.VRepAPIError:
print("waiting for simulation start")
time.sleep(1)
assignment = SimulationAssignment(vr, car, args.velocity)
assignment.setup_car(vr, car)
csvfile = None
# Stop the existing simulation if requested. Not using a separate
# VRepInterface (and hence VREP API client id) seems to cause crashes.
if args.restart:
with vrepInterface.VRepInterface.open() as vr:
vr.simxStopSimulation(vrep.simx_opmode_oneshot_wait)
# This needs not be in the main body, or the API throws an exception a split second after simulation start.
with vrepInterface.VRepInterface.open() as vr:
ret = vr.simxStartSimulation(vrep.simx_opmode_oneshot_wait)
# Open a V-REP API connection and get the car.
with vrepInterface.VRepInterface.open() as vr:
if args.synchronous:
vr.simxSynchronous(1)
car = Car(vr)
finish_wire = Tripwire(vr, 'Proximity_sensor_StartLine')
stopping_wire = Tripwire(vr, 'Proximity_sensor')
success = False
while not success:
try:
sim_start_time = car.get_sim_time()
success = True
except vrepInterface.VRepAPIError:
print("waiting for simulation start")
time.sleep(1)
assignment = SimulationAssignment(vr, car, args.velocity)
assignment.setup_car(vr, car)
# Stop the existing simulation if requested. Not using a separate
# VRepInterface (and hence VREP API client id) seems to cause crashes.
if args.restart:
with vrepInterface.VRepInterface.open() as vr:
vr.simxStopSimulation(vrep.simx_opmode_oneshot_wait)
# This needs not be in the main body, or the API throws an exception a split second after simulation start.
with vrepInterface.VRepInterface.open() as vr:
ret = vr.simxStartSimulation(vrep.simx_opmode_oneshot_wait)
# Open a V-REP API connection and get the car.
with vrepInterface.VRepInterface.open() as vr:
if args.synchronous:
vr.simxSynchronous(1)
car = Car(vr)
wire = Tripwire(vr)
success = False
while not success:
try:
vr.simxGetFloatSignal('simTime', vrep.simx_opmode_oneshot_wait)
success = True
except vrepInterface.VRepAPIError:
print("waiting for simulation start")
time.sleep(1)
assignment = SimulationAssignment(vr, car, args.velocity)
assignment.setup_car(vr, car)
csvfile = None
# Stop the existing simulation if requested. Not using a separate
# VRepInterface (and hence VREP API client id) seems to cause crashes.
if args.restart:
with vrepInterface.VRepInterface.open() as vr:
vr.simxStopSimulation(vrep.simx_opmode_oneshot_wait)
# This needs not be in the main body, or the API throws an exception a split second after simulation start.
with vrepInterface.VRepInterface.open() as vr:
ret = vr.simxStartSimulation(vrep.simx_opmode_oneshot_wait)
# Open a V-REP API connection and get the car.
with vrepInterface.VRepInterface.open() as vr:
if args.synchronous:
vr.simxSynchronous(1)
car = Car(vr)
finish_wire = Tripwire(vr, 'Proximity_sensor_StartLine')
stopping_wire = Tripwire(vr, 'Proximity_sensor')
success = False
while not success:
try:
sim_start_time = car.get_sim_time()
success = True
except vrepInterface.VRepAPIError:
print("waiting for simulation start")
time.sleep(1)
assignment = SimulationAssignment(vr, car, args.velocity)
assignment.setup_car(vr, car)