def __init__(self,
vision=False,
throttle=True,
gear_change=False,
port=60934,
pid_assist=False,
CLIENT_MAX_STEPS=np.inf,
visualise=True,
no_of_visualisations=1):
# If `visualise` is set to False torcs simulator will run in headless mode
"""Init Method."""
self.torcs_proc = None
self.pid_assist = pid_assist
if self.pid_assist:
self.action_dim = 2 # LanePos, Velocity
else:
self.action_dim = 3 # Accel, Steer, Brake
TorcsEnv.__init__(self,
vision=False,
throttle=True,
gear_change=False,
visualise=visualise,
no_of_visualisations=no_of_visualisations)
self.state_dim = 29 # No. of sensors input
self.env_name = 'Madras_Env'
self.port = port
self.visualise = visualise
self.no_of_visualisations = no_of_visualisations
self.CLIENT_MAX_STEPS = CLIENT_MAX_STEPS
self.client_type = 0 # Snakeoil client type
self.initial_reset = True
self.early_stop = True
if self.pid_assist:
self.PID_latency = 10
else:
self.PID_latency = 1
self.accel_PID = PID(np.array([10.5, 0.05, 2.8])) # accel PID
self.steer_PID = PID(np.array([5.1, 0.001, 0.000001])) # steer PID
self.prev_lane = 0
self.prev_angle = 0
self.prev_vel = 0
self.prev_dist = 0
self.ob = None
self.track_len = 7014.6
self.seed()
self.start_torcs_process()
def playTraffic(port=3101, target_vel=50.0, angle=0.0, sleep=0):
"""Traffic Play function."""
env = TorcsEnv(vision=False, throttle=True, gear_change=False)
ob = None
while ob is None:
try:
client = snakeoil3.Client(p=port, vision=False)
client.MAX_STEPS = np.inf
client.get_servers_input(step=0)
obs = client.S.d
ob = env.make_observation(obs)
except:
pass
episode_count = cfg['traffic']['max_eps']
max_steps = cfg['traffic']['max_steps_eps']
early_stop = 0
velocity_i = target_vel / 300.0
accel_pid = PID(np.array([10.5, 0.05, 2.8]))
steer_pid = PID(np.array([5.1, 0.001, 0.000001]))
steer = 0.0
accel = 0.0
brake = 0
A = cfg['traffic']['amplitude'] / 300.0
T = cfg['traffic']['time_period']
# print(velocity_i)
for i in range(episode_count):
info = {'termination_cause': 0}
steer = 0.0
accel = 0.0
brake = 0
S = 0.0
for step in range(max_steps):
a_t = np.asarray([steer, accel, brake]) # [steer, accel, brake]
try:
ob, r_t, done, info = env.step(step, client, a_t, early_stop)
if done:
pass
except Exception as e:
print("Exception caught at point A at port " + str(i) + str(e))
ob = None
while ob is None:
try:
client = snakeoil3.Client(p=port, vision=False)
client.MAX_STEPS = np.inf
client.get_servers_input(step=0)
obs = client.S.d
ob = env.make_observation(obs)
except:
pass
continue
if (step <= sleep):
print("WAIT" + str(port))
continue
velocity = velocity_i + A * math.sin(step / T)
opp = ob.opponents
front = np.array([opp[15], opp[16], opp[17], opp[18], opp[19]])
closest_front = np.min(front)
if (step % T == 0):
print("VEL_CHANGE: ", velocity * 300.0)
vel_error = velocity - ob.speedX
angle_error = -(ob.trackPos - angle) / 10 + ob.angle
steer_pid.update_error(angle_error)
accel_pid.update_error(vel_error)
accel = accel_pid.output()
steer = steer_pid.output()
if accel < 0:
brake = 1
else:
brake = 0
if closest_front < ((float(0.5 * ob.speedX * 100) + 10.0) / 200.0):
brake = 1
else:
brake = 0
try:
if 'termination_cause' in info.keys() and info['termination_cause'] == 'hardReset':
print('Hard reset by some agent')
ob, client = env.reset(client=client, relaunch=True)
except Exception as e:
print("Exception caught at point B at port " + str(i) + str(e) )
ob = None
while ob is None:
try:
client = snakeoil3.Client(p=port, vision=False) # Open new UDP in vtorcs
client.MAX_STEPS = np.inf
client.get_servers_input(0) # Get the initial input from torcs
obs = client.S.d # Get the current full-observation from torcs
ob = env.make_observation(obs)
except:
print("Exception caught at at point C at port " + str(i) + str(e))
class MadrasEnv(TorcsEnv, gym.Env):
"""Definition of the Gym Madras Env."""
def __init__(self,
vision=False,
throttle=True,
gear_change=False,
port=60934,
pid_assist=False,
CLIENT_MAX_STEPS=np.inf,
visualise=True,
no_of_visualisations=1):
# If `visualise` is set to False torcs simulator will run in headless mode
"""Init Method."""
self.torcs_proc = None
self.pid_assist = pid_assist
if self.pid_assist:
self.action_dim = 2 # LanePos, Velocity
else:
self.action_dim = 3 # Accel, Steer, Brake
TorcsEnv.__init__(self,
vision=False,
throttle=True,
gear_change=False,
visualise=visualise,
no_of_visualisations=no_of_visualisations)
self.state_dim = 29 # No. of sensors input
self.env_name = 'Madras_Env'
self.port = port
self.visualise = visualise
self.no_of_visualisations = no_of_visualisations
self.CLIENT_MAX_STEPS = CLIENT_MAX_STEPS
self.client_type = 0 # Snakeoil client type
self.initial_reset = True
self.early_stop = True
if self.pid_assist:
self.PID_latency = 10
else:
self.PID_latency = 1
self.accel_PID = PID(np.array([10.5, 0.05, 2.8])) # accel PID
self.steer_PID = PID(np.array([5.1, 0.001, 0.000001])) # steer PID
self.prev_lane = 0
self.prev_angle = 0
self.prev_vel = 0
self.prev_dist = 0
self.ob = None
self.track_len = 7014.6
self.seed()
self.start_torcs_process()
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def get_free_udp_port(self):
udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
udp.bind(('', 0))
addr, port = udp.getsockname()
udp.close()
return port
def start_torcs_process(self):
if self.torcs_proc is not None:
os.killpg(os.getpgid(self.torcs_proc.pid), signal.SIGKILL)
time.sleep(0.5)
self.torcs_proc = None
self.port = self.get_free_udp_port()
window_title = str(self.port)
command = None
rank = MPI.COMM_WORLD.Get_rank()
if rank < self.no_of_visualisations and self.visualise:
command = 'export TORCS_PORT={} && vglrun torcs -t 10000000 -nolaptime'.format(
self.port)
else:
command = 'export TORCS_PORT={} && torcs -t 10000000 -r ~/.torcs/config/raceman/quickrace.xml -nolaptime'.format(
self.port)
if self.vision is True:
command += ' -vision'
self.torcs_proc = subprocess.Popen([command],
shell=True,
preexec_fn=os.setsid)
time.sleep(1)
#if self.visualise:
# os.system('sh autostart.sh {}'.format(window_title))
def reset(self, prev_step_info=None):
"""Reset Method. To be called at the end of each episode"""
if self.initial_reset:
while self.ob is None:
try:
self.client = snakeoil3.Client(p=self.port,
vision=self.vision,
visualise=self.visualise)
# Open new UDP in vtorcs
self.client.MAX_STEPS = self.CLIENT_MAX_STEPS
self.client.get_servers_input(step=0)
# Get the initial input from torcs
raw_ob = self.client.S.d
# Get the current full-observation
self.ob = self.make_observation(raw_ob)
except:
pass
self.initial_reset = False
else:
try:
self.ob, self.client = TorcsEnv.reset(self,
client=self.client,
relaunch=True)
except Exception as e:
self.ob = None
while self.ob is None:
try:
print("Hard Reset")
# self.end(self.client)
self.client = snakeoil3.Client(p=self.port,
vision=self.vision)
# Open new UDP in vtorcs
self.client.MAX_STEPS = self.CLIENT_MAX_STEPS
self.client.get_servers_input(step=0)
# Get the initial input from torcs
raw_ob = self.client.S.d
# Get the current full-observation from torcs
self.ob = self.make_observation(raw_ob)
except:
pass
self.distance_traversed = 0
s_t = np.hstack((self.ob.angle, self.ob.track, self.ob.trackPos,
self.ob.speedX, self.ob.speedY, self.ob.speedZ,
self.ob.wheelSpinVel / 100.0, self.ob.rpm))
self.prev_angle = 0.0
self.prev_dist = 0.0
self.prev_lane = 0.0
self.accel_PID.reset_pid()
self.steer_PID.reset_pid()
return s_t
def step(self, desire):
"""Step method to be called at each time step."""
r_t = 0
for PID_step in range(self.PID_latency):
# Implement the desired trackpos and velocity using PID
if self.pid_assist:
self.accel_PID.update_error((desire[1] - self.prev_vel))
self.steer_PID.update_error(
(-(self.prev_lane - desire[0]) / 10 + self.prev_angle))
if self.accel_PID.output() < 0.0:
brake = 1
else:
brake = 0
a_t = np.asarray(
[self.steer_PID.output(),
self.accel_PID.output(), brake])
else:
a_t = desire
try:
self.ob, r, done, info = TorcsEnv.step(self, PID_step,
self.client, a_t,
self.early_stop)
except Exception as e:
print(("Exception caught at port " + str(e)))
self.ob = None
while self.ob is None:
try:
self.client = snakeoil3.Client(p=self.port,
vision=self.vision)
# Open new UDP in vtorcs
self.client.MAX_STEPS = self.CLIENT_MAX_STEPS
self.client.get_servers_input(0)
# Get the initial input from torcs
raw_ob = self.client.S.d
# Get the current full-observation from torcs
self.ob = self.make_observation(raw_ob)
except:
pass
continue
self.prev_vel = self.ob.speedX
self.prev_angle = self.ob.angle
self.prev_lane = self.ob.trackPos
if (math.isnan(r)):
r = 0.0
r_t += r # accumulate rewards over all the time steps
self.distance_traversed = self.client.S.d['distRaced']
r_t += (self.distance_traversed - self.prev_dist) /\
self.track_len
self.prev_dist = deepcopy(self.distance_traversed)
if self.distance_traversed >= self.track_len:
done = True
if done:
# self.reset()
break
s_t1 = np.hstack((self.ob.angle, self.ob.track, self.ob.trackPos,
self.ob.speedX, self.ob.speedY, self.ob.speedZ,
self.ob.wheelSpinVel / 100.0, self.ob.rpm))
return s_t1, r_t, done, info