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 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 _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 _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