modify (#57)

metadrive/base_class/base_object.py

@@ -52,6 +52,8 @@ class BaseObject(BaseRunnable):
     sample some special configs for it ,Properties and parameters in PARAMETER_SPACE of the object are fixed after
     calling __init__().
     """
+    MASS = None  # if object has a body, the mass will be set automatically
+
     def __init__(self, name=None, random_seed=None, config=None, escape_random_seed_assertion=False):
         """
         Config is a static conception, which specified the parameters of one element.
@@ -89,9 +91,15 @@ def add_body(self, physics_body):
                 raise ValueError("The physics body is not BulletBodyNode type")
             self._body = physics_body
             new_origin = NodePath(self._body)
+            new_origin.setH(self.origin.getH())
+            new_origin.setPos(self.origin.getPos())
             self.origin.getChildren().reparentTo(new_origin)
             self.origin = new_origin
             self.dynamic_nodes.append(physics_body)
+            if self.MASS is not None:
+                assert isinstance(self.MASS,
+                                  int) or isinstance(self.MASS, float), "MASS should be a float or an integer"
+                self._body.setMass(self.MASS)
         else:
             raise AttributeError("You can not set the object body for twice")
 

metadrive/component/buildings/tollgate_building.py

@@ -7,6 +7,7 @@
 class TollGateBuilding(BaseBuilding):
     BUILDING_LENGTH = 10
     BUILDING_HEIGHT = 5
+    MASS = 0
 
     def __init__(self, lane, position, heading_theta, random_seed):
         super(TollGateBuilding, self).__init__(lane, position, heading_theta, random_seed)

metadrive/component/static_object/base_static_object.py

@@ -17,11 +17,7 @@ def __init__(self, lane, position: Sequence[float], heading_theta: float = 0., r
         :param heading_theta: the angle from positive direction of horizontal axis
         """
         super(BaseStaticObject, self).__init__(random_seed=random_seed)
-        self.set_position(position, 0)
+        self.set_position(position, self.HEIGHT / 2 if hasattr(self, "HEIGHT") else 0)
         self.set_heading_theta(heading_theta)
         self.lane_index = lane.index
         self.lane = lane
-
-    def set_static(self, static: bool = False):
-        mass = 0 if static else self.MASS
-        self._body.setMass(mass)

metadrive/component/static_object/traffic_object.py

@@ -46,12 +46,12 @@ def __init__(self, lane, longitude: float, lateral: float, static: bool = False,
         self.add_body(BaseRigidBodyNode(self.name, self.NAME))
         self.body.addShape(BulletCylinderShape(self.RADIUS, self.HEIGHT))
         self.body.setIntoCollideMask(self.COLLISION_GROUP)
+        self.set_static(static)
         if self.render:
             model = self.loader.loadModel(AssetLoader.file_path("models", "traffic_cone", "scene.gltf"))
             model.setScale(0.02)
             model.setPos(0, 0, -self.HEIGHT / 2)
             model.reparentTo(self.origin)
-        self.set_static(static)
 
 
 class TrafficWarning(TrafficObject):
@@ -66,13 +66,13 @@ def __init__(self, lane, longitude: float, lateral: float, static: bool = False,
         self.add_body(BaseRigidBodyNode(self.name, self.NAME))
         self.body.addShape(BulletCylinderShape(self.RADIUS, self.HEIGHT))
         self.body.setIntoCollideMask(self.COLLISION_GROUP)
+        self.set_static(static)
         if self.render:
             model = self.loader.loadModel(AssetLoader.file_path("models", "warning", "warning.gltf"))
             model.setScale(0.02)
             model.setH(-90)
             model.setPos(0, 0, -self.HEIGHT / 2)
             model.reparentTo(self.origin)
-        self.set_static(static)
 
 
 class TrafficBarrier(TrafficObject):
@@ -88,8 +88,8 @@ def __init__(self, lane, longitude: float, lateral: float, static: bool = False,
         self.add_body(BaseRigidBodyNode(self.name, self.NAME))
         self.body.addShape(BulletBoxShape((self.WIDTH / 2, self.LENGTH / 2, self.HEIGHT / 2)))
         self.body.setIntoCollideMask(self.COLLISION_GROUP)
+        self.set_static(static)
         if self.render:
             model = self.loader.loadModel(AssetLoader.file_path("models", "barrier", "scene.gltf"))
             model.setH(-90)
             model.reparentTo(self.origin)
-        self.set_static(static)

metadrive/component/vehicle/base_vehicle.py

@@ -489,7 +489,6 @@ def _create_vehicle_chassis(self):
         chassis_shape = BulletBoxShape(Vec3(self.WIDTH / 2, self.LENGTH / 2, self.HEIGHT / 2))
         ts = TransformState.makePos(Vec3(0, 0, self.HEIGHT / 2))
         chassis.addShape(chassis_shape, ts)
-        chassis.setMass(self.MASS)
         chassis.setDeactivationEnabled(False)
         chassis.notifyCollisions(True)  # advance collision check, do callback in pg_collision_callback
 

metadrive/envs/safe_metadrive_env.py

@@ -70,7 +70,7 @@ def setup_engine(self):
             "use_render": True,
             # "debug": True,
             'environment_num': 10,
-            "start_seed": 123,
+            "start_seed": 129,
             # "traffic_density": 0.2,
             # "environment_num": 1,
             # # "start_seed": 187,
@@ -81,7 +81,7 @@ def setup_engine(self):
             # "cost_to_reward":True,
             "vehicle_config": {
                 "spawn_lane_index": (FirstPGBlock.NODE_2, FirstPGBlock.NODE_3, 2),
-                "show_lidar": True,
+                # "show_lidar": True,
                 # "show_side_detector": True,
                 # "show_lane_line_detector": True,
                 # "side_detector": dict(num_lasers=2, distance=50),  # laser num, distance

metadrive/examples/drive_in_safe_metadrive_env.py

@@ -7,20 +7,18 @@
 """
 import logging
 
-from metadrive.envs.safe_metadrive_env import SafeMetaDriveEnv
+from metadrive.tests.test_functionality.test_object_collision_detection import ComplexEnv
 
 if __name__ == "__main__":
-    env = SafeMetaDriveEnv(dict(
-        use_render=True,
-        manual_control=True,
-    ))
+    env = ComplexEnv(dict(use_render=True, manual_control=True, vehicle_config={"show_navi_mark": False}))
     env.reset()
+    env.vehicle.expert_takeover = True
     for i in range(1, 1000000000):
         previous_takeover = env.current_track_vehicle.expert_takeover
         o, r, d, info = env.step([0, 0])
         env.render(
             text={
-                "Auto-Drive (Press T)": env.current_track_vehicle.expert_takeover,
+                "Auto-Drive (Switch mode: T)": "on" if env.current_track_vehicle.expert_takeover else "off",
                 "Total episode cost": env.episode_cost
             }
         )

metadrive/examples/drive_in_single_agent_env.py

@@ -32,6 +32,7 @@
         config.update(dict(offscreen_render=True))
     env = MetaDriveEnv(config)
     o = env.reset()
+    env.vehicle.expert_takeover = True
     if args.observation == "rgb_camera":
         assert isinstance(o, dict)
         print("The observation is a dict with numpy arrays as values: ", {k: v.shape for k, v in o.items()})
@@ -40,9 +41,11 @@
         print("The observation is an numpy array with shape: ", o.shape)
     for i in range(1, 1000000000):
         o, r, d, info = env.step([0, 0])
-        env.render(text={
-            "Auto-Drive (Press T)": env.current_track_vehicle.expert_takeover,
-        })
+        env.render(
+            text={
+                "Auto-Drive (Switch mode: T)": "on" if env.current_track_vehicle.expert_takeover else "off",
+            }
+        )
         if d and info["arrive_dest"]:
             env.reset()
     env.close()

metadrive/tests/test_functionality/test_object_collision_detection.py

@@ -1,21 +1,21 @@
 from metadrive.component.vehicle.vehicle_type import LVehicle
 from metadrive.component.static_object.traffic_object import TrafficCone, TrafficWarning
 from metadrive.constants import BodyName, TerminationState, DEFAULT_AGENT
-from metadrive.envs import MetaDriveEnv
+from metadrive.envs.safe_metadrive_env import SafeMetaDriveEnv
 
 
-class TestCollisionEnv(MetaDriveEnv):
+class ComplexEnv(SafeMetaDriveEnv):
     """
     now for test use and demo use only
     """
-    @classmethod
-    def default_config(cls):
-        config = super(TestCollisionEnv, cls).default_config()
+    def default_config(self):
+        config = super(ComplexEnv, self).default_config()
         config.update(
             {
                 "environment_num": 1,
-                "traffic_density": 0.1,
+                "traffic_density": 0.02,
                 "start_seed": 5,
+                "accident_prob": 0.0,
                 # "traffic_mode":"respawn",
                 "debug_physics_world": False,
                 "debug": False,
@@ -25,31 +25,34 @@ def default_config(cls):
         return config
 
     def __init__(self, config=None):
-        super(TestCollisionEnv, self).__init__(config)
+        super(ComplexEnv, self).__init__(config)
         self.breakdown_vehicle = None
         self.alert = None
 
     def reset(self, episode_data: dict = None, force_seed=None):
-        ret = super(TestCollisionEnv, self).reset(episode_data)
+        ret = super(ComplexEnv, self).reset(episode_data)
         self.vehicle.max_speed = 60
         lane = self.current_map.road_network.graph[">>>"]["1C0_0_"][0]
-        self.breakdown_vehicle = self.engine.spawn_object(
+        self.breakdown_vehicle = self.engine.object_manager.spawn_object(
             self.engine.traffic_manager.random_vehicle_type(),
             vehicle_config={
                 "spawn_lane_index": lane.index,
                 "spawn_longitude": 30
             }
         )
-
+        self.engine.object_manager.accident_lanes.append(lane)
         lane_ = self.current_map.road_network.graph[">>>"]["1C0_0_"][1]
-        breakdown_vehicle = self.engine.spawn_object(
+        breakdown_vehicle = self.engine.object_manager.spawn_object(
             LVehicle, vehicle_config={
                 "spawn_lane_index": lane_.index,
                 "spawn_longitude": 30
             }
         )
+        self.engine.object_manager.accident_lanes.append(lane_)
 
-        self.alert = self.engine.spawn_object(TrafficWarning, lane=lane, longitude=22, lateral=0, pbr_model=False)
+        self.alert = self.engine.object_manager.spawn_object(
+            TrafficWarning, lane=lane, longitude=22, lateral=0, pbr_model=False
+        )
 
         # part 1
         lane = self.current_map.road_network.graph["1C0_1_"]["2S0_0_"][2]
@@ -61,12 +64,13 @@ def reset(self, episode_data: dict = None, force_seed=None):
         ]
 
         for p in pos:
-            cone = self.engine.spawn_object(TrafficCone, lane=lane, longitude=p[0], lateral=p[1])
+            cone = self.engine.object_manager.spawn_object(TrafficCone, lane=lane, longitude=p[0], lateral=p[1])
+        self.engine.object_manager.accident_lanes.append(lane)
         from metadrive.component.vehicle.vehicle_type import SVehicle, XLVehicle
         v_pos = [8, 14]
         v_type = [SVehicle, XLVehicle]
         for v_long, v_t in zip(v_pos, v_type):
-            v = self.engine.spawn_object(
+            v = self.engine.object_manager.spawn_object(
                 v_t, vehicle_config={
                     "spawn_lane_index": lane.index,
                     "spawn_longitude": v_long
@@ -75,6 +79,7 @@ def reset(self, episode_data: dict = None, force_seed=None):
 
         # part 2
         lane = self.current_map.road_network.graph["3R0_0_"]["3R0_1_"][0]
+        self.engine.object_manager.accident_lanes.append(lane)
         pos = [
             (-20, lane.width / 3), (-15.6, lane.width / 4), (-12.1, 0), (-8.7, -lane.width / 4),
             (-4.2, -lane.width / 2), (-0.7, -lane.width * 3 / 4), (4.1, -lane.width), (7.3, -lane.width),
@@ -84,24 +89,25 @@ def reset(self, episode_data: dict = None, force_seed=None):
 
         for p in pos:
             p_ = (p[0] + 5, -p[1])
-            cone = self.engine.spawn_object(TrafficCone, lane=lane, longitude=p_[0], lateral=p_[1])
+            cone = self.engine.object_manager.spawn_object(TrafficCone, lane=lane, longitude=p_[0], lateral=p_[1])
 
         v_pos = [14, 19]
         for v_long in v_pos:
-            v = self.engine.spawn_object(
+            v = self.engine.object_manager.spawn_object(
                 self.engine.traffic_manager.random_vehicle_type(),
                 vehicle_config={
                     "spawn_lane_index": lane.index,
                     "spawn_longitude": v_long
                 }
             )
 
-        alert = self.engine.spawn_object(TrafficWarning, lane=lane, longitude=-35, lateral=0)
+        alert = self.engine.object_manager.spawn_object(TrafficWarning, lane=lane, longitude=-35, lateral=0)
 
-        alert = self.engine.spawn_object(TrafficWarning, lane=lane, longitude=-60, lateral=0)
+        alert = self.engine.object_manager.spawn_object(TrafficWarning, lane=lane, longitude=-60, lateral=0)
 
         # part 3
         lane = self.current_map.road_network.graph["4C0_0_"]["4C0_1_"][2]
+        self.engine.object_manager.accident_lanes.append(lane)
         pos = [
             (-12.1, 0), (-8.7, -lane.width / 4), (-4.2, -lane.width / 2), (-0.7, -lane.width * 3 / 4),
             (4.1, -lane.width), (7.3, -lane.width), (11.5, -lane.width), (15.5, -lane.width), (20.0, -lane.width),
@@ -110,11 +116,11 @@ def reset(self, episode_data: dict = None, force_seed=None):
 
         for p in pos:
             p_ = (p[0] + 5, p[1] * 3.5 / 3)
-            cone = self.engine.spawn_object(TrafficCone, lane=lane, longitude=p_[0], lateral=p_[1])
+            cone = self.engine.object_manager.spawn_object(TrafficCone, lane=lane, longitude=p_[0], lateral=p_[1])
 
         v_pos = [14, 19]
         for v_long in v_pos:
-            v = self.engine.spawn_object(
+            v = self.engine.object_manager.spawn_object(
                 self.engine.traffic_manager.random_vehicle_type(),
                 vehicle_config={
                     "spawn_lane_index": lane.index,
@@ -124,17 +130,18 @@ def reset(self, episode_data: dict = None, force_seed=None):
 
         # part 4
         lane = self.current_map.road_network.graph["4C0_1_"]["5R0_0_"][0]
+        self.engine.object_manager.accident_lanes.append(lane)
         pos = [(-12, lane.width / 4), (-8.1, 0), (-4, -lane.width / 4), (-0.1, -lane.width / 2), (4, -lane.width)]
 
         for p in pos:
             p_ = (p[0] + 60, -p[1] * 3.5 / 3)
-            cone = self.engine.spawn_object(TrafficCone, lane=lane, longitude=p_[0], lateral=p_[1])
+            cone = self.engine.object_manager.spawn_object(TrafficCone, lane=lane, longitude=p_[0], lateral=p_[1])
 
         return ret
 
 
 def test_object_collision_detection(render=False):
-    env = TestCollisionEnv(
+    env = ComplexEnv(
         {
             "manual_control": True,
             "traffic_density": 0.0,
@@ -148,11 +155,11 @@ def test_object_collision_detection(render=False):
     try:
         o = env.reset()
         lane_index = (">>", ">>>", 0)
-        alert = env.engine.spawn_object(
+        alert = env.engine.object_manager.spawn_object(
             TrafficWarning, lane=env.current_map.road_network.get_lane(lane_index), longitude=22, lateral=0
         )
         lane_index = (">>", ">>>", 2)
-        alert = env.engine.spawn_object(
+        alert = env.engine.object_manager.spawn_object(
             TrafficCone, lane=env.current_map.road_network.get_lane(lane_index), longitude=22, lateral=0
         )
         crash_obj = False