def add_car(self, x, y, vx, vy, x_des, y_des, target_lane, theta):
if y <= 4.:
idx = self._lower_lane_next_idx
self._lower_lane_next_idx += 1
if self._lower_lane_next_idx > int(self.max_veh_num/2.):
self._lower_lane_next_idx = 1
elif y > 4.:
idx = self._upper_lane_next_idx
self._upper_lane_next_idx += 1
if self._upper_lane_next_idx > self.max_veh_num:
self._upper_lane_next_idx = int(self.max_veh_num/2.)+1
car = Car(idx=idx, length=self.car_length, width=self.car_width, color=random.choice(RED_COLORS),
max_accel=self.car_max_accel, max_speed=self.car_max_speed,
max_rotation=self.car_max_rotation,
expose_level=self.car_expose_level)
driver = EnvDriver(target_lane=target_lane,
min_front_x=self.min_front_x,
min_back_x=self.min_back_x,
x_des=x_des, y_des=y_des,
x_sigma=self.driver_sigma, y_sigma=0.,
idx=idx, car=car, dt=self.dt
)
car.set_position(np.array([x, y]))
car.set_velocity(np.array([vx, vy]))
car.set_rotation(theta)
self._cars.append(car)
self._drivers.append(driver)
return car, driver
def add_car(self, x, y, vx, vy, target_lane, theta):
idx = self._empty_indices.pop()
car = Car(idx=idx,
length=self.car_length,
width=self.car_width,
color=random.choice(RED_COLORS),
max_accel=self.car_max_accel,
max_speed=self.car_max_speed,
max_rotation=self.car_max_rotation,
expose_level=self.car_expose_level)
driver = EnvDriver(target_lane=target_lane,
min_x=self.min_x,
min_y=self.min_y,
x_sigma=self.driver_sigma,
y_sigma=0.,
idx=idx,
car=car,
dt=self.dt)
car.set_position(np.array([x, y]))
car.set_velocity(np.array([vx, vy]))
car.set_rotation(theta)
self._cars.append(car)
self._drivers.append(driver)
# driver.observe(self._cars, self._road)
return car, driver
def _reset(self):
self._collision = False
self._goal = False
self._terminal = False
self._intentions = []
self._empty_indices = list(range(1,self.max_veh_num+1))
self._cars, self._drivers = [], []
x_0 = (self.x_start + self.x_goal)/2.
y_0 = self.lane_start * 4.0 + 2.0
car = Car(idx=0, length=self.car_length, width=self.car_width, color=random.choice(BLUE_COLORS),
max_accel=self.car_max_accel, max_speed=self.ego_max_speed,
max_rotation=self.car_max_rotation,
expose_level=self.car_expose_level)
driver = EgoDriver(left_bound=self.x_start,min_x=self.min_x, min_y = self.min_y,
x_sigma=self.driver_sigma, y_sigma=0.,
idx=0,car=car,dt=self.dt)
car.set_position(np.array([x_0, y_0]))
car.set_velocity(np.array([0., 0.]))
car.set_rotation(0.)
self._cars.append(car)
self._drivers.append(driver)
# randomly generate surrounding cars and drivers
for lane_id in range(1,3):
x = self.left_bound + np.random.uniform(self.gap_min,self.gap_max)
y = lane_id*4.0 + 2.0
while (x <= self.right_bound):
target_lane = np.random.choice([1,2])
self.add_car(x, y, 0., 0., target_lane, 0.)
x += (np.random.uniform(self.gap_min,self.gap_max) + self.car_length)
for driver in self._drivers[1:]:
driver.observe(self._cars, self._road)
self._sup_labels = self.get_sup_labels()
return None
def _reset(self):
self._collision = False
self._goal = False
self._intentions = []
self._lower_lane_next_idx = 1
self._upper_lane_next_idx = int(self.max_veh_num / 2.) + 1
self._cars, self._drivers = [], []
x_0 = self.left_bound
y_0 = np.random.choice([2., 6.])
car = Car(idx=0,
length=self.car_length,
width=self.car_width,
color=random.choice(BLUE_COLORS),
max_accel=self.car_max_accel,
max_speed=self.car_max_speed,
max_rotation=self.car_max_rotation,
expose_level=self.car_expose_level)
driver = EgoDriver(x_sigma=self.driver_sigma,
y_sigma=0.,
idx=0,
car=car,
dt=self.dt)
car.set_position(np.array([x_0, y_0]))
car.set_velocity(np.array([0., 0.]))
car.set_rotation(0.)
self._cars.append(car)
self._drivers.append(driver)
# randomly generate surrounding cars and drivers
# lower lane
x = self.right_bound - np.random.rand() * (self.gap_max - self.gap_min)
if y_0 == 2.0:
x_min = x_0 + self.car_length + self.gap_min
else:
x_min = self.left_bound
y = 2.0
while (x >= x_min):
aggressive = np.random.choice([True, False])
self.add_car(x, y, 0., 0., aggressive, 0.)
x -= (np.random.uniform(self.gap_min, self.gap_max) +
self.car_length)
# upper lane
x = self.right_bound - np.random.rand() * (self.gap_max - self.gap_min)
if y_0 == 6.0:
x_min = x_0 + self.car_length + self.gap_min
else:
x_min = self.left_bound
y = 6.0
while (x >= x_min):
aggressive = np.random.choice([True, False])
self.add_car(x, y, 0., 0., aggressive, 0.)
x -= (np.random.uniform(self.gap_min, self.gap_max) +
self.car_length)
self._sup_labels = self.get_sup_labels()
return None
def build_car_classes(num_cars=30):
car_list = []
counter = 33
for car in range(num_cars):
car = Car()
car.location += counter
car_list.append(car)
counter += 33
return car_list
def car_factory(car_fleet=30):
car_list = []
counter = 33
for car in range(car_fleet):
car = Car()
car.location += counter
car_list.append(car)
counter += 33
return car_list
def _reset(self):
self._collision = False
self._outroad = False
self._goal = False
self._intentions = []
self._lower_lane_next_idx = 1
self._upper_lane_next_idx = int(self.max_veh_num / 2.) + 1
self._cars, self._drivers = [], []
car = Car(idx=0,
length=self.car_length,
width=self.car_width,
color=random.choice(BLUE_COLORS),
max_accel=self.car_max_accel,
max_speed=self.car_max_speed,
expose_level=self.car_expose_level)
driver = EgoDriver(trajectory=EgoTrajectory(),
idx=0,
car=car,
dt=self.dt)
car.set_position(np.array([0., -5.0]))
car.set_velocity(np.array([0., 0.]))
car.set_rotation(np.pi / 2.)
driver.v_des = 0.
driver.t_des = 0.
self._cars.append(car)
self._drivers.append(driver)
# randomly generate surrounding cars and drivers
# lower lane
x = self.left_bound + np.random.rand() * (self.gap_max - self.gap_min)
while (x < self.right_bound):
v_des = self.desire_speed
p_des = 2.
direction = -1
self.add_car(x, p_des, -self.desire_speed, 0., v_des, p_des,
direction, np.pi)
x += (np.random.rand() * (self.gap_max - self.gap_min) +
self.gap_min + self.car_length)
# upper lane
x = self.right_bound - np.random.rand() * (self.gap_max - self.gap_min)
while (x > self.left_bound):
v_des = self.desire_speed
p_des = 6.
direction = 1
self.add_car(x, p_des, self.desire_speed, 0., v_des, p_des,
direction, 0.)
x -= (np.random.rand() * (self.gap_max - self.gap_min) +
self.gap_min + self.car_length)
self._sup_labels = self.get_sup_labels()
return None
def add_car(self, x, y, vx, vy, v_des, p_des, direction, theta):
if y < 4.:
idx = self._lower_lane_next_idx
self._lower_lane_next_idx += 1
if self._lower_lane_next_idx > int(self.max_veh_num / 2.):
self._lower_lane_next_idx = 1
elif y > 4.:
idx = self._upper_lane_next_idx
self._upper_lane_next_idx += 1
if self._upper_lane_next_idx > self.max_veh_num:
self._upper_lane_next_idx = int(self.max_veh_num / 2.) + 1
car = Car(idx=idx,
length=self.car_length,
width=self.car_width,
color=random.choice(RED_COLORS),
max_accel=self.car_max_accel,
max_speed=self.car_max_speed,
max_rotation=0.,
expose_level=self.car_expose_level)
driver = YNYDriver(idx=idx,
car=car,
dt=self.dt,
x_driver=IDMDriver(idx=idx,
car=car,
sigma=self.driver_sigma,
s_des=self.s_des,
s_min=self.s_min,
axis=0,
min_overlap=self.min_overlap,
dt=self.dt),
y_driver=PDDriver(idx=idx,
car=car,
sigma=0.,
axis=1,
dt=self.dt))
car.set_position(np.array([x, y]))
car.set_velocity(np.array([vx, vy]))
car.set_rotation(theta)
driver.x_driver.set_v_des(v_des)
driver.x_driver.set_direction(direction)
driver.y_driver.set_p_des(p_des)
if np.random.rand() < self.yld:
driver.set_yld(True)
else:
driver.set_yld(False)
self._cars.append(car)
self._drivers.append(driver)
return car, driver
def add_car(self, idx, x, y, vx, vy, v_des, p_des, direction, theta):
car = Car(idx=idx,
length=self.car_length,
width=self.car_width,
color=random.choice(RED_COLORS),
max_accel=self.car_max_accel,
max_speed=self.car_max_speed,
expose_level=self.car_expose_level)
driver = TwoTDriver(idx=idx,
car=car,
dt=self.dt,
x_driver=IDMDriver(idx=idx,
car=car,
sigma=self.driver_sigma,
s_min=self.s_min,
axis=0,
min_overlap=self.min_overlap,
dt=self.dt),
y_driver=PDDriver(idx=idx,
car=car,
sigma=0.,
axis=1,
dt=self.dt))
car.set_position(np.array([x, y]))
car.set_velocity(np.array([vx, vy]))
car.set_rotation(theta)
driver.x_driver.set_v_des(v_des)
driver.x_driver.set_direction(direction)
driver.y_driver.set_p_des(p_des)
driver.t1 = np.random.rand()
if np.random.rand() < self.yld:
driver.yld = True
driver.t2 = np.random.rand() * 1.5 - 0.5
else:
driver.yld = False
driver.t2 = None
self._cars.append(car)
self._drivers.append(driver)
return car, driver
def __init__(self,
yld=0.5,
vs_actions=[0.,0.5,3.],
t_actions=[-1.5,0.,1.5],
desire_speed=3.,
speed_cost=0.01,
t_cost=0.01,
control_cost=0.01,
collision_cost=1.,
outroad_cost=1.,
goal_reward=1.,
road=Road([RoadSegment([(-100.,0.),(100.,0.),(100.,8.),(-100.,8.)]),
RoadSegment([(-2,-10.),(2,-10.),(2,0.),(-2,0.)])]),
n_cars=3,
num_updates=1,
dt=0.1,
**kwargs):
self.yld = yld
self.vs_actions = vs_actions
self.t_actions = t_actions
# we use target value instead of target change so system is Markovian
self.rl_actions = list(itertools.product(vs_actions,t_actions))
self.num_updates = num_updates
self.desire_speed = desire_speed
self.speed_cost = speed_cost
self.t_cost = t_cost
self.control_cost = control_cost
self.collision_cost = collision_cost
self.outroad_cost = outroad_cost
self.goal_reward = goal_reward
self._collision = False
self._outroad = False
self._goal = False
self._intentions = []
car_length=5.0
car_width=2.0
car_max_accel=10.0
car_max_speed=40.0
car_expose_level=4
cars = [Car(idx=cid, length=car_length, width=car_width, color=random.choice(BLUE_COLORS),
max_accel=car_max_accel, max_speed=car_max_speed,
expose_level=car_expose_level) for cid in range(n_cars)
]
driver_sigma = 0.0
s_min = 2.0
min_overlap = 1.0
drivers = []
drivers.append(EgoDriver(trajectory=EgoTrajectory(),idx=0,car=cars[0],dt=dt))
for did in range(1,n_cars):
driver = TwoTDriver(idx=did, car=cars[did], dt=dt,
x_driver=IDMDriver(idx=did, car=cars[did], sigma=driver_sigma, s_min=s_min, axis=0, min_overlap=min_overlap, dt=dt),
y_driver=PDDriver(idx=did, car=cars[did], sigma=driver_sigma, axis=1, dt=dt))
drivers.append(driver)
super(TIntersection, self).__init__(
road=road,
cars=cars,
drivers=drivers,
dt=dt,
**kwargs,)
def __init__(self, in_file='./hashcode.in', truncate_cars=False):
self.streets = {}
streets_tmp = {}
self.street_detail = []
self.intersections = []
self.cars = {}
streets_in_path = set()
self.total_intersections = None
self.end_time = 0
# Open file ready to read
#f = open(ROOT_DIR + '/hashcode.in', "r")
f = open(in_file, "r")
# Read simulation configuration
simulation_key = ['D', 'I', 'S', 'V', 'F']
simulation_val = map(int, f.readline().split())
self.simulation_config = dict(zip(simulation_key, simulation_val))
self.total_intersections = self.simulation_config['I']
self.end_time = self.simulation_config['D']
# Read streets detail
for _ in range(self.simulation_config['S']):
type_converted_line = list(
map(self.convert_type,
f.readline().split()))
self.street_detail.append(type_converted_line)
new_street = Street(type_converted_line[0], type_converted_line[1],
type_converted_line[2], type_converted_line[3])
streets_tmp[type_converted_line[2]] = new_street
self.street_detail = pd.DataFrame(
self.street_detail,
columns=['start_int', 'end_int', 'name', 'time_used'])
print("Successfully read {} streets detail".format(
self.simulation_config['S']))
# Read cars path
for i in range(self.simulation_config['V']):
type_converted_line = list(
map(self.convert_type,
f.readline().split()))
type_converted_line = [
type_converted_line[0], type_converted_line[1:]
]
streets_in_path = streets_in_path.union(type_converted_line[1])
car_name = 'car' + str(i)
new_car = Car(car_name, type_converted_line[0],
type_converted_line[1])
self.cars[car_name] = new_car
print("Successfully read {} cars paths".format(len(self.cars)))
# Remove street with no cars
orig_street_cnt = len(streets_tmp)
self.street_detail = self.street_detail[
self.street_detail['name'].isin(streets_in_path)]
self.streets = {
street: streets_tmp[street]
for street in streets_in_path
}
self.simulation_config['S'] = len(self.streets)
self.generate_intersection()
print("Successfully removed {} streets with no cars".format(
orig_street_cnt - len(self.streets)))
print("Total streets: {}".format(len(self.streets)))
print("Total cars: {}".format(len(self.cars)))
print("Total intersections: {}".format(self.total_intersections))
print("Original endtime: {}".format(self.end_time))
if truncate_cars:
self.cars = dict(itertools.islice(self.cars.items(),
truncate_cars))
print('Truncated cars to total of: {}'.format(
len(self.cars.items())))
def do_command_line_run():
"""
Just a test run
"""
first_car = Car(velocity=np.array([0, 0.1]), acceleration=np.array([0, 5]))
print 'Mycar %s' % first_car
data = np.zeros((ticks, 6))
for i in range(ticks/2):
data[i] = first_car.update()
print 'Interim val:\t%s' % first_car
print 'Car state: %s' % first_car.state
print '\n\n'
first_car.acceleration = np.array([0, -5])
first_car.turn('EAST')
print 'Car accel = %s now' % first_car.acceleration
for i in range(ticks/2, (ticks/2)+100):
data[i] = first_car.update()
print 'Interim val:\t%s' % first_car
first_car.turn('SOUTH')
print 'Car accel = %s now' % first_car.acceleration
for i in range((ticks/2)+100, ticks):
data[i] = first_car.update()
print 'Final val:\t%s' % first_car
time = np.linspace(0, (0.01*(ticks-1)), num=ticks)
norm_vel = zeros(ticks)
for k, i in enumerate(data[:, [2, 3]]):
norm_vel[k] = norm(i)
print data[0]
figure(0)
# plot(data[:, 0], data[:, 1], '.', label='Position')
# plot(time, norm_vel, '.r', label="Normalized vel")
subplot(211)
plot(time, data[:, 2], label='V_x')
plot(time, data[:, 3], label='V_y')
plot(time, data[:, 4], label='a_x')
plot(time, data[:, 5], label='a_y')
xlabel('time(s)')
ylabel('stuff')
title('Car test velocity and acceleration')
legend()
subplot(212)
plot(data[:, 0], data[:, 1], '.', label='Position')
title("Position")
fm = get_current_fig_manager()
fm.full_screen_toggle()
show()
