Fix safe env (#526)

* fix safe env

* fix safe env

* fix random vehicle

pgdrive/component/vehicle/base_vehicle.py

@@ -598,7 +598,12 @@ def update_map_info(self, map):
         else:
             lane, new_l_index = possible_lanes[idx][:-1]
         dest = self.config["destination_node"]
-        self.navigation.update(map, current_lane_index=new_l_index, final_road_node=dest if dest is not None else None)
+        self.navigation.update(
+            map,
+            current_lane_index=new_l_index,
+            final_road_node=dest if dest is not None else None,
+            random_seed=self.engine.global_random_seed
+        )
         assert lane is not None, "spawn place is not on road!"
         self.navigation.update_localization(self)
         self.lane_index = new_l_index

pgdrive/component/vehicle_module/navigation.py

@@ -96,7 +96,7 @@ def __init__(
             self._dest_node_path.show(CamMask.MainCam)
         logging.debug("Load Vehicle Module: {}".format(self.__class__.__name__))
 
-    def update(self, map: BaseMap, current_lane_index, final_road_node=None, random_seed=False):
+    def update(self, map: BaseMap, current_lane_index, final_road_node=None, random_seed=None):
         # TODO(pzh): We should not determine the destination of a vehicle in the navigation module.
         start_road_node = current_lane_index[0]
         self.map = map
@@ -105,12 +105,11 @@ def update(self, map: BaseMap, current_lane_index, final_road_node=None, random_
         if final_road_node is None:
             # auto find if PGMap
             current_road_negative = Road(*current_lane_index[:-1]).is_negative_road()
-            random_seed = random_seed if random_seed is not False else None
             # choose first block when born on negative road
             block = map.blocks[0] if current_road_negative else map.blocks[-1]
             sockets = block.get_socket_list()
+            socket = get_np_random(random_seed).choice(sockets)
             while True:
-                socket = get_np_random(random_seed).choice(sockets)
                 if not socket.is_socket_node(start_road_node) or len(sockets) == 1:
                     break
                 else:

pgdrive/envs/pgdrive_env.py

@@ -46,7 +46,7 @@
     # ===== Traffic =====
     traffic_density=0.1,
     traffic_mode=TrafficMode.Trigger,  # "Respawn", "Trigger", "Hybrid"
-    random_traffic=True,  # Traffic is randomized at default.
+    random_traffic=False,  # Traffic is randomized at default.
 
     # ===== Object =====
     accident_prob=0.,  # accident may happen on each block with this probability, except multi-exits block
@@ -328,11 +328,8 @@ def setup_engine(self):
         self.engine.accept("t", self.toggle_expert_takeover)
         self.engine.accept("b", self.bird_view_camera)
         self.engine.accept("q", self.chase_camera)
-
-        from pgdrive.manager.object_manager import TrafficObjectManager
         from pgdrive.manager.traffic_manager import TrafficManager
         self.engine.register_manager("traffic_manager", TrafficManager())
-        self.engine.register_manager("object_manager", TrafficObjectManager())
 
 
 if __name__ == '__main__':

pgdrive/envs/safe_pgdrive_env.py

@@ -2,7 +2,6 @@
 from pgdrive.component.blocks.first_block import FirstPGBlock
 from pgdrive.envs.pgdrive_env import PGDriveEnv
 from pgdrive.utils import Config
-from pgdrive.utils.math_utils import clip
 
 
 class SafePGDriveEnv(PGDriveEnv):
@@ -11,8 +10,10 @@ def default_config(self) -> Config:
         config.update(
             {
                 "environment_num": 100,
-                "accident_prob": 1.0,
+                "accident_prob": 0.5,
+                "traffic_density": 0.1,
                 "safe_rl_env": True,  # Should always be True. But we just leave it here for historical reason.
+                "cost_to_reward": False,
 
                 # ===== reward scheme =====
                 "crash_vehicle_penalty": 0.,
@@ -29,6 +30,14 @@ def default_config(self) -> Config:
         )
         return config
 
+    def _post_process_config(self, config):
+        config = super(SafePGDriveEnv, self)._post_process_config(config)
+        if config["cost_to_reward"]:
+            config["crash_vehicle_penalty"] = config["crash_vehicle_cost"]
+            config["crash_object_penalty"] = config["crash_object_cost"]
+            config["out_of_road_penalty"] = config["out_of_road_cost"]
+        return config
+
     def done_function(self, vehicle_id: str):
         done, done_info = super(SafePGDriveEnv, self).done_function(vehicle_id)
         if self.config["safe_rl_env"]:
@@ -38,64 +47,10 @@ def done_function(self, vehicle_id: str):
                 done = False
         return done, done_info
 
-    def reward_function(self, vehicle_id: str):
-        """
-        Override this func to get a new reward function
-        :param vehicle_id: id of BaseVehicle
-        :return: reward
-        """
-        vehicle = self.vehicles[vehicle_id]
-        step_info = dict()
-
-        # Reward for moving forward in current lane
-        current_lane = vehicle.lane if vehicle.lane in vehicle.navigation.current_ref_lanes else \
-            vehicle.navigation.current_ref_lanes[0]
-        long_last, _ = current_lane.local_coordinates(vehicle.last_position)
-        long_now, lateral_now = current_lane.local_coordinates(vehicle.position)
-
-        # reward for lane keeping, without it vehicle can learn to overtake but fail to keep in lane
-        reward = 0.0
-        if self.config["use_lateral"]:
-            lateral_factor = clip(1 - 2 * abs(lateral_now) / vehicle.navigation.get_current_lane_width(), 0.0, 1.0)
-        else:
-            lateral_factor = 1.0
-        reward += self.config["driving_reward"] * (long_now - long_last) * lateral_factor
-
-        # Penalty for waiting
-        low_speed_penalty = 0
-        if vehicle.speed < 1:
-            low_speed_penalty = self.config["low_speed_penalty"]  # encourage car
-        reward -= low_speed_penalty
-        reward -= self.config["general_penalty"]
-
-        reward += self.config["speed_reward"] * (vehicle.speed / vehicle.max_speed)
-        step_info["step_reward"] = reward
-
-        # for done
-        if vehicle.crash_vehicle:
-            reward = self.config["crash_vehicle_penalty"]
-        elif vehicle.crash_object:
-            reward = self.config["crash_object_penalty"]
-        elif vehicle.out_of_route:
-            reward = self.config["out_of_road_penalty"]
-        elif vehicle.crash_sidewalk:
-            reward = self.config["out_of_road_penalty"]
-        elif vehicle.arrive_destination:
-            reward += self.config["success_reward"]
-
-        return reward, step_info
-
-    def cost_function(self, vehicle_id: str):
-        vehicle = self.vehicles[vehicle_id]
-        step_info = dict()
-        step_info["cost"] = 0
-        if vehicle.crash_vehicle:
-            step_info["cost"] = self.config["crash_vehicle_cost"]
-        elif vehicle.crash_object:
-            step_info["cost"] = self.config["crash_object_cost"]
-        elif vehicle.out_of_route:
-            step_info["cost"] = self.config["out_of_road_cost"]
-        return step_info['cost'], step_info
+    def setup_engine(self):
+        from pgdrive.manager.object_manager import TrafficObjectManager
+        super(SafePGDriveEnv, self).setup_engine()
+        self.engine.register_manager("object_manager", TrafficObjectManager())
 
 
 if __name__ == "__main__":
@@ -104,10 +59,10 @@ def cost_function(self, vehicle_id: str):
             # "accident_prob": 1.0,
             "manual_control": True,
             "use_render": True,
-            "debug": True,
+            # "debug": True,
             'environment_num': 1,
             "start_seed": 22,
-            "traffic_density": 0.2,
+            # "traffic_density": 0.2,
             # "environment_num": 1,
             # # "start_seed": 187,
             # "out_of_road_cost": 1,
@@ -131,10 +86,9 @@ def cost_function(self, vehicle_id: str):
         o, r, d, info = env.step([0, 0])
         total_cost += info["cost"]
         env.render(text={"cost": total_cost, "seed": env.current_seed, "reward": r})
-        print(len(env.engine.traffic_manager.traffic_vehicles))
         if d:
             total_cost = 0
-            print("done_cost:{}".format(info["cost"]))
+            print("done_cost:{}".format(info["cost"]), "done_reward;{}".format(r))
             print("Reset")
             env.reset()
     env.close()

pgdrive/manager/object_manager.py

@@ -81,7 +81,7 @@ def reset(self):
             lateral_len = engine.current_map.config[engine.current_map.LANE_WIDTH]
 
             lane = engine.current_map.road_network.get_lane(accident_road.lane_index(accident_lane_idx))
-            self.accident_lanes.append(lane)
+            self.accident_lanes += accident_road.get_lanes(engine.current_map.road_network)
             if self.np_random.rand() > self.PROHIBIT_SCENE_PROB or _debug:
                 self.prohibit_scene(lane, longitude, lateral_len, on_left)
             else:

pgdrive/manager/traffic_manager.py

@@ -247,7 +247,7 @@ def _create_vehicles_once(self, map: BaseMap, traffic_density: float) -> None:
             potential_vehicle_configs = []
             for lanes in trigger_lanes:
                 for l in lanes:
-                    if l in self.engine.object_manager.accident_lanes:
+                    if hasattr(self.engine, "object_manager") and l in self.engine.object_manager.accident_lanes:
                         continue
                     potential_vehicle_configs += self._propose_vehicle_configs(l)
 

pgdrive/tests/test_env/test_naive_multi_agent.py

@@ -40,6 +40,7 @@ def test_naive_multi_agent_pgdrive():
         assert isinstance(env.action_space, gym.spaces.Dict)
         obs = env.reset()
         assert isinstance(obs, dict)
+        env.action_space.seed(0)
         for step in range(100):
             a = env.action_space.sample()
             assert isinstance(a, dict)

pgdrive/tests/vis_env/vis_pgdrive_env.py

@@ -8,20 +8,20 @@
             "environment_num": 1,
             "traffic_density": .2,
             "traffic_mode": "trigger",
-            "start_seed": 12,
+            "start_seed": 22,
             # "_disable_detector_mask":True,
             "onscreen_message": True,
             # "debug_physics_world": True,
             "global_light": True,
             # "debug_static_world":True,
             "cull_scene": False,
             # "controller": "joystick",
-            # "manual_control": True,
+            "manual_control": True,
             "use_render": True,
             "decision_repeat": 5,
             "rgb_clip": True,
             # "debug": True,
-            "random_lane_num": True,
+            # "random_lane_num": True,
             "fast": True,
 
             # "map_config": {
@@ -32,7 +32,7 @@
             # },
             "pstats": True,
             "discrete_action": True,
-            "map": "SSS",
+            # "map": "SSS",
             "random_traffic": False,
             "random_lane_width": True,
             "random_agent_model": True,