def act(self, action: Union[dict, str] = None):
"""
Execute an action.
For now, no action is supported because the vehicle takes all decisions
of acceleration and lane changes on its own, based on the IDM and MOBIL models.
:param action: the action
"""
if self.crashed:
return
action = {}
front_vehicle, rear_vehicle = self.road.neighbour_vehicles(self)
# Lateral: MOBIL
self.follow_road()
if self.enable_lane_change:
self.change_lane_policy()
action['steering'] = self.steering_control(self.target_lane_index)
action['steering'] = np.clip(action['steering'],
-self.MAX_STEERING_ANGLE,
self.MAX_STEERING_ANGLE)
# Longitudinal: IDM
action['acceleration'] = self.acceleration(ego_vehicle=self,
front_vehicle=front_vehicle,
rear_vehicle=rear_vehicle)
# action['acceleration'] = self.recover_from_stop(action['acceleration'])
action['acceleration'] = np.clip(action['acceleration'], -self.ACC_MAX,
self.ACC_MAX)
Vehicle.act(
self, action
) # Skip ControlledVehicle.act(), or the command will be overriden.
def _create_vehicles(self) -> None:
self.vehicle = Vehicle(self.road, [0, 0],
2 * np.pi * self.np_random.rand(), 0)
self.road.vehicles.append(self.vehicle)
"""
positions = self.np_random.choice(self.road.network.lanes_list(), size=car_count+1, replace=False)
for x in range(car_count):
lane2 = positions[x]
vehicle2 = Vehicle(self.road, lane2.position(lane2.length/2, 0), lane2.heading, 0)
self.road.vehicles.append(vehicle2)
lane = positions[-1]
self.goal = Landmark(self.road, lane.position(lane.length/2, 0), heading=lane.heading)
self.road.objects.append(self.goal)
"""
"""
add vehicels to free parking slots
"""
spots = len(self.road.network.lanes_list())
lane_pos = self.np_random.choice(spots)
lane = self.road.network.lanes_list()[lane_pos]
self.goal = Landmark(self.road,
lane.position(lane.length / 2, 0),
heading=lane.heading)
self.road.objects.append(self.goal)
for x in range(spots):
if x != lane_pos:
lane_tmp = self.road.network.lanes_list()[x]
vehicle_tmp = Vehicle(
self.road, lane_tmp.position(lane_tmp.length / 2, 0),
lane_tmp.heading, 0)
self.road.vehicles.append(vehicle_tmp)
def dynamics_event(cls, vehicle: Vehicle,
event: pygame.event.EventType) -> None:
"""
Map the pygame keyboard events to dynamics actuation
:param vehicle: the vehicle receiving the event
:param event: the pygame event
"""
action = vehicle.action.copy()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_RIGHT:
action['steering'] = 45 * np.pi / 180
if event.key == pygame.K_LEFT:
action['steering'] = -45 * np.pi / 180
if event.key == pygame.K_DOWN:
action['acceleration'] = -6
if event.key == pygame.K_UP:
action['acceleration'] = 5
elif event.type == pygame.KEYUP:
if event.key == pygame.K_RIGHT:
action['steering'] = 0
if event.key == pygame.K_LEFT:
action['steering'] = 0
if event.key == pygame.K_DOWN:
action['acceleration'] = 0
if event.key == pygame.K_UP:
action['acceleration'] = 0
if action != vehicle.action:
vehicle.act(action)
def test_step():
v = Vehicle(road=None, position=[0, 0], speed=20, heading=0)
for _ in range(2 * FPS):
v.step(dt=1 / FPS)
assert v.position[0] == pytest.approx(40)
assert v.position[1] == pytest.approx(0)
assert v.speed == pytest.approx(20)
assert v.heading == pytest.approx(0)
def test_act():
v = Vehicle(road=None, position=[0, 0], speed=20, heading=0)
v.act({'acceleration': 1, 'steering': 0})
for _ in range(1 * FPS):
v.step(dt=1 / FPS)
assert v.speed == pytest.approx(21)
v.act({'acceleration': 0, 'steering': 0.5})
for _ in range(1 * FPS):
v.step(dt=1 / FPS)
assert v.speed == pytest.approx(21)
assert v.position[1] > 0
def act(self, action: Union[dict, str] = None):
"""
Execute an action.
For now, no action is supported because the vehicle takes all decisions
of acceleration and lane changes on its own, based on the IDM and MOBIL models.
:param action: the action
"""
if self.crashed:
return
action = {}
# Lateral: MOBIL
self.follow_road()
if self.enable_lane_change:
self.change_lane_policy()
action['steering'] = self.steering_control(self.target_lane_index)
action['steering'] = np.clip(action['steering'],
-self.MAX_STEERING_ANGLE,
self.MAX_STEERING_ANGLE)
action['steering'] = 0
# if self.sadism:
# if np.abs(self.position[0]-self.road.vehicles[0].position[0])<5:
# if self.road.vehicles[0].position[1]-self.position[1]<0:
# action['steering']=-0.01
# elif self.road.vehicles[0].position[1]-self.position[1]>0:
# action['steering']=0.01
# Longitudinal: IDM
front_vehicle, rear_vehicle = self.road.neighbour_vehicles(
self, self.lane_index)
action['acceleration'] = self.acceleration(ego_vehicle=self,
front_vehicle=front_vehicle,
rear_vehicle=rear_vehicle)
# When changing lane, check both current and target lanes
if self.lane_index != self.target_lane_index:
front_vehicle, rear_vehicle = self.road.neighbour_vehicles(
self, self.target_lane_index)
target_idm_acceleration = self.acceleration(
ego_vehicle=self,
front_vehicle=front_vehicle,
rear_vehicle=rear_vehicle)
action['acceleration'] = min(action['acceleration'],
target_idm_acceleration)
# action['acceleration'] = self.recover_from_stop(action['acceleration'])
action['acceleration'] = 0
# else:
# action['acceleration'] = np.clip(action['acceleration'], -self.ACC_MAX, self.ACC_MAX)
Vehicle.act(
self, action
) # Skip ControlledVehicle.act(), or the command will be overriden.
def respect_priorities(v1: Vehicle, v2: Vehicle) -> Vehicle:
"""
Resolve a conflict between two vehicles by determining who should yield
:param v1: first vehicle
:param v2: second vehicle
:return: the yielding vehicle
"""
if v1.lane.priority > v2.lane.priority:
return v2
elif v1.lane.priority < v2.lane.priority:
return v1
else: # The vehicle behind should yield
return v1 if v1.front_distance_to(v2) > v2.front_distance_to(v1) else v2
def _create_vehicles(self) -> None:
"""Create some new random vehicles of a given type, and add them on the road."""
other_vehicles_type = utils.class_from_path(
self.config["other_vehicles_type"])
other_per_controlled = near_split(
self.config["vehicles_count"],
num_bins=self.config["controlled_vehicles"])
self.controlled_vehicles = []
for others in other_per_controlled:
vehicle = Vehicle.create_random(
self.road,
speed=25,
lane_id=self.config["initial_lane_id"],
spacing=self.config["ego_spacing"])
vehicle = self.action_type.vehicle_class(self.road,
vehicle.position,
vehicle.heading,
vehicle.speed)
self.controlled_vehicles.append(vehicle)
self.road.vehicles.append(vehicle)
for _ in range(others):
vehicle = other_vehicles_type.create_random(
self.road, spacing=1 / self.config["vehicles_density"])
vehicle.randomize_behavior()
self.road.vehicles.append(vehicle)
def _create_vehicles(self, parked_probability: float = 0.75) -> None:
"""
Create some new random vehicles of a given type, and add them on the road.
:param parked_probability: probability that a spot is occupied
"""
self.vehicle = self.action_type.vehicle_class(self.road,
self.vehicle_starting,
2 * np.pi * self.np_random.rand(), 0)
self.road.vehicles.append(self.vehicle)
goal_position = [self.np_random.choice([-2 * self.spots - 10, 2 * self.spots + 10]), 0]
self.goal = Landmark(self.road, goal_position, heading=0)
self.road.objects.append(self.goal)
vehicles_type = utils.class_from_path(self.config["other_vehicles_type"])
for i in range(self.config["vehicles_count"]):
is_parked = self.np_random.rand() <= parked_probability
if not is_parked:
# Just an effort to spread the vehicles out
idx = self.np_random.randint(0, self.num_middle_lanes)
longitudinal = (i * 5) - (self.x_range / 8) * self.np_random.randint(-1, 1)
self.road.vehicles.append(
vehicles_type.make_on_lane(self.road, ("d", "e", idx), longitudinal, speed=2))
else:
lane = ("a", "b", i) if self.np_random.rand() >= 0.5 else ("b", "c", i)
self.road.vehicles.append(Vehicle.make_on_lane(self.road, lane, 4, speed=0))
for v in self.road.vehicles: # Prevent early collisions
if v is not self.vehicle and np.linalg.norm(v.position - self.vehicle.position) < 20:
self.road.vehicles.remove(v)
def _create_vehicles(self):
"""
Create some new random vehicles of a given type, and add them on the road.
"""
self.vehicle = Vehicle(self.road, [0, 0], 2*np.pi*self.np_random.rand(), 0)
self.road.vehicles.append(self.vehicle)
lane = self.np_random.choice(self.road.network.lanes_list())
self.goal = Obstacle(self.road, lane.position(lane.length/2, 0), heading=lane.heading)
self.goal.COLLISIONS_ENABLED = False
self.road.vehicles.insert(0, self.goal)
lane_index = self.road.network.get_closest_lane_index(lane.position(lane.length/2, 0))
side_lanes = [self.road.network.get_lane(side_lane_index) for side_lane_index in self.road.network.side_lanes(lane_index)]
i = 1
self.obstacles_features = []
for side_lane in side_lanes:
obstacle = Obstacle(self.road, side_lane.position(side_lane.length/2, 0), heading=side_lane.heading)
obstacle.COLLISIONS_ENABLED = True
self.road.vehicles.insert(i, obstacle)
i += 1
obstacle_feature = np.array([obstacle.position[0], obstacle.position[1], 0, 0,
math.cos(obstacle.heading), math.sin(obstacle.heading)]) / self.config['observation']['scales']
self.obstacles_features.append(obstacle_feature)
def test():
from highway_env.vehicle.kinematics import Vehicle
r = None
v = Vehicle(r, [0, 0], 0, 20)
v.dump()
v.dump()
v.dump()
v.dump()
print(v.get_log())
def _create_vehicles(self) -> None:
self.vehicle = Vehicle(self.road, [0, 0],
2 * np.pi * self.np_random.rand(), 0)
self.road.vehicles.append(self.vehicle)
positions = self.np_random.choice(self.road.network.lanes_list(),
size=car_count + 1,
replace=False)
for x in range(car_count):
lane2 = positions[x]
vehicle2 = Vehicle(self.road, lane2.position(lane2.length / 2, 0),
lane2.heading, 0)
self.road.vehicles.append(vehicle2)
lane = positions[-1]
self.goal = Landmark(self.road,
lane.position(lane.length / 2, 0),
heading=lane.heading)
self.road.objects.append(self.goal)
def _create_vehicles(self) -> None:
"""
Create some new random vehicles of a given type, and add them on the road.
"""
self.vehicle = Vehicle(self.road, [0, 0],
2 * np.pi * self.np_random.rand(), 0)
self.road.vehicles.append(self.vehicle)
lane = self.np_random.choice(self.road.network.lanes_list())
self.goal = Landmark(self.road,
lane.position(lane.length / 2, 0),
heading=lane.heading)
self.road.objects.append(self.goal)
def test_front():
r = Road(RoadNetwork.straight_road_network(1))
v1 = Vehicle(road=r, position=[0, 0], speed=20)
v2 = Vehicle(road=r, position=[10, 0], speed=10)
r.vehicles.extend([v1, v2])
assert v1.lane_distance_to(v2) == pytest.approx(10)
assert v2.lane_distance_to(v1) == pytest.approx(-10)
def _create_vehicles(self):
"""
Create some new random vehicles of a given type, and add them on the road.
"""
self.vehicle = Vehicle(self.road, [0, 0],
2 * np.pi * self.np_random.rand(), 0)
self.road.vehicles.append(self.vehicle)
lane = self.np_random.choice(self.road.network.lanes_list())
self.goal = Obstacle(self.road,
lane.position(lane.length / 2, 0),
heading=lane.heading)
self.goal.COLLISIONS_ENABLED = False
self.road.obstacles.append(self.goal)
def test_brake():
v = Vehicle(road=None, position=[0, 0], velocity=20, heading=0)
for _ in range(10 * FPS):
v.act({
'acceleration': min(max(-1 * v.velocity, -6), 6),
'steering': 0
})
v.step(dt=1 / FPS)
assert v.velocity == pytest.approx(0, abs=0.01)
def control_event(cls, vehicle: Vehicle,
event: pygame.event.EventType) -> None:
"""
Map the pygame keyboard events to control decisions
:param vehicle: the vehicle receiving the event
:param event: the pygame event
"""
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_RIGHT:
vehicle.act("FASTER")
if event.key == pygame.K_LEFT:
vehicle.act("SLOWER")
if event.key == pygame.K_DOWN:
vehicle.act("LANE_RIGHT")
if event.key == pygame.K_UP:
vehicle.act("LANE_LEFT")
def test_collision():
# Collision between two vehicles
r = Road(RoadNetwork.straight_road_network(1))
v1 = Vehicle(road=r, position=[0, 0], speed=10)
v2 = Vehicle(road=r, position=[4, 0], speed=20)
v1.check_collision(v2)
assert v1.crashed and v2.crashed
assert v1.speed == v2.speed == 10
# Collision between a vehicle and an obstacle
v3 = Vehicle(road=r, position=[20, 0], speed=10)
o = Obstacle(road=r, position=[23, 0])
v3.check_collision(o)
assert v3.crashed and o.hit
assert v3.speed == 0
# Collision between a vehicle and a landmark
v4 = Vehicle(road=r, position=[40, 0], speed=10)
l = Landmark(road=r, position=[43, 0])
v4.check_collision(l)
assert v4.crashed is False
assert l.hit