def test_network(): # Diamond net = RoadNetwork() net.add_lane(0, 1, StraightLane([0, 0], [10, 0])) net.add_lane(1, 2, StraightLane([10, 0], [5, 5])) net.add_lane(2, 0, StraightLane([5, 5], [0, 0])) net.add_lane(1, 3, StraightLane([10, 0], [5, -5])) net.add_lane(3, 0, StraightLane([5, -5], [0, 0])) print(net.graph) # Road road = Road(network=net) v = ControlledVehicle(road, [5, 0], heading=0, target_velocity=2) road.vehicles.append(v) assert v.lane_index == (0, 1, 0) # Lane changes dt = 1 / 15 lane_index = v.target_lane_index lane_changes = 0 for _ in range(int(20 / dt)): road.act() road.step(dt) if lane_index != v.target_lane_index: lane_index = v.target_lane_index lane_changes += 1 assert lane_changes >= 3
def test_stop_before_obstacle(vehicle_type): road = Road(RoadNetwork.straight_road_network(lanes=1)) vehicle = vehicle_type(road=road, position=[0, 0], speed=20, heading=0) obstacle = Obstacle(road=road, position=[80, 0]) road.vehicles.append(vehicle) road.objects.append(obstacle) for _ in range(10 * FPS): road.act() road.step(dt=1/FPS) assert not vehicle.crashed assert vehicle.position[0] == pytest.approx(obstacle.position[0] - vehicle_type.DISTANCE_WANTED, abs=1) assert vehicle.position[1] == pytest.approx(0) assert vehicle.speed == pytest.approx(0, abs=1) assert vehicle.heading == pytest.approx(0)
class HighwayEnv(AbstractEnv): """ A highway driving environment. The vehicle is driving on a straight highway with several lanes, and is rewarded for reaching a high velocity, staying on the rightmost lanes and avoiding collisions. """ COLLISION_REWARD = -1 """ The reward received when colliding with a vehicle.""" RIGHT_LANE_REWARD = 0.1 """ The reward received when driving on the right-most lanes, linearly mapped to zero for other lanes.""" HIGH_VELOCITY_REWARD = 0.2 """ The reward received when driving at full speed, linearly mapped to zero for lower speeds.""" LANE_CHANGE_REWARD = -0 """ The reward received at each lane change action.""" def __init__(self): super(HighwayEnv, self).__init__() self.DIFFICULTY_LEVELS = DIFFICULTY_LEVELS self.config = self.DIFFICULTY_LEVELS["ORIGIN"].copy() self.steps = 0 self.reset() Vehicle.ID = 0 def reset(self): self._create_road() self._create_vehicles() # print(m.move_carsim( # { # "command": "move_object", # "object_id": str(len(self.road.vehicles)-1), # "location": self.vehicle.position.tolist() + [0] # } # )) self.steps = 0 return self._observation() def step(self, action): self.steps += 1 return super(HighwayEnv, self).step(action) def set_difficulty_level(self, level): if level in self.DIFFICULTY_LEVELS: logger.info("Set difficulty level to: {}".format(level)) self.config.update(self.DIFFICULTY_LEVELS[level]) self.reset() else: raise ValueError( "Invalid difficulty level, choose among {}".format( str(self.DIFFICULTY_LEVELS.keys()))) def configure(self, config): self.config.update(config) def _create_road(self): """ Create a road composed of straight adjacent lanes. """ self.road = Road(network=RoadNetwork.straight_road_network( self.config["lanes_count"]), np_random=self.np_random) def _create_vehicles(self): """ Create some new random vehicles of a given type, and add them on the road. """ self.vehicle = IDMVehicle.create_random( self.road, 25, spacing=self.config["initial_spacing"]) self.road.vehicles.append(self.vehicle) vehicles_type = utils.class_from_path( self.config["other_vehicles_type"]) for _ in range(self.config["vehicles_count"]): self.road.vehicles.append(vehicles_type.create_random(self.road)) """ def _create_road(self): road = Road.create_random_road(lanes_count=self.config["lanes_count"], vehicles_count=self.config["vehicles_count"], vehicles_type=utils.class_from_path(self.config["other_vehicles_type"]), np_random=self.np_random) vehicle = MDPVehicle.create_random(road, 25, spacing=self.config["initial_spacing"], np_random=self.np_random) #vehicle = ControlledVehicle.create_random(road, 25, spacing=self.config["initial_spacing"], np_random=self.np_random) #vehicle = CarSim.create_random(road, 10, spacing=self.config["initial_spacing"], np_random=self.np_random) #vehicle.position[0] =100 road.vehicles.append(vehicle) #road.vehicles[len(road.vehicles) // 2].velocity=50 # return road, road.vehicles[len(road.vehicles) // 2] return road, vehicle """ def _reward(self, action): """ The reward is defined to foster driving at high speed, on the rightmost lanes, and to avoid collisions. :param action: the last action performed :return: the corresponding reward """ action_reward = { 0: self.LANE_CHANGE_REWARD, 1: 0, 2: self.LANE_CHANGE_REWARD, 3: 0, 4: 0 } state_reward = \ + self.COLLISION_REWARD * self.vehicle.crashed \ + self.RIGHT_LANE_REWARD * self.vehicle.target_lane_index / (len(self.road.network.LANES) - 1) \ + self.HIGH_VELOCITY_REWARD * self.vehicle.velocity_index / (self.vehicle.SPEED_COUNT - 1) return action_reward[action] + state_reward def _observation(self): # return ext.FeatureExtractor(self.road.vehicles,self.vehicle.id) return super(HighwayEnv, self)._observation() def _is_terminal(self): """ The episode is over if the ego vehicle crashed or the time is out. """ return self.vehicle.crashed or self.steps > self.config["duration"] # return self.vehicle.crashed or self.vehicle.out or self.steps > self.config["duration"] def fake_step(self): """ :return: """ for k in range(int(self.SIMULATION_FREQUENCY // self.POLICY_FREQUENCY)): self.road.act() self.road.step(1 / self.SIMULATION_FREQUENCY) # Automatically render intermediate simulation steps if a viewer has been launched self._automatic_rendering() # Stop at terminal states if self.done or self._is_terminal(): break self.enable_auto_render = False self.steps += 1 from highway_env.extractors import Extractor extractor = Extractor() extractor_features = extractor.FeatureExtractor( self.road.vehicles, 0, 1) for i in range(4): birth_place = [(0, 1, 0), (0, 1, 1), (0, 1, 2), (0, 1, 3)] # destinations = ["e1", "e2"] position_deviation = 5 velocity_deviation = 1.5 other_vehicles_type = utils.class_from_path( self.config["other_vehicles_type"]) birth = birth_place[np.random.randint(0, 4)] lane = self.road.network.get_lane(birth) car = other_vehicles_type.make_on_lane( self.road, birth, longitudinal=100 + np.random.randint(1, 10) * position_deviation, velocity=5 + np.random.randint(1, 10) * velocity_deviation) destination = 1 car.plan_route_to(destination) car.randomize_behavior() self.road.vehicles.append(car) lane.vehicles.append(car) #obs = self._observation() #reward = self._reward(action) terminal = self._is_terminal() info = {} return terminal, extractor_features