Fix safe env (#521)

* fix collision group

* format

pgdrive/component/static_object/__init__.py

@@ -1,2 +1,2 @@
 from pgdrive.component.static_object.base_static_object import BaseStaticObject
-from pgdrive.component.static_object.traffic_object import TrafficSign
+from pgdrive.component.static_object.traffic_object import TrafficObject

pgdrive/component/static_object/traffic_object.py

@@ -1,4 +1,5 @@
 from typing import Tuple
+from pgdrive.constants import CollisionGroup
 
 import numpy as np
 from panda3d.bullet import BulletCylinderShape
@@ -13,14 +14,15 @@
 LaneIndex = Tuple[str, str, int]
 
 
-class TrafficSign(BaseStaticObject):
+class TrafficObject(BaseStaticObject):
     """
     Common interface for objects that appear on the road, beside vehicles.
     """
     NAME = None
     RADIUS = 0.25
     HEIGHT = 1.2
     MASS = 1
+    COLLISION_GROUP = CollisionGroup.TrafficObject
 
     COST_ONCE = True  # cost will give at the first time
 
@@ -33,11 +35,11 @@ def __init__(self, lane, longitude: float, lateral: float, random_seed):
         position = lane.position(longitude, lateral)
         heading = lane.heading_at(longitude)
         assert self.NAME is not None, "Assign a name for this class for finding it easily"
-        super(TrafficSign, self).__init__(lane, position, heading, random_seed)
+        super(TrafficObject, self).__init__(lane, position, heading, random_seed)
         self.crashed = False
 
 
-class TrafficCone(TrafficSign):
+class TrafficCone(TrafficObject):
     """Placed near the construction section to indicate that traffic is prohibited"""
 
     NAME = BodyName.Traffic_cone
@@ -46,6 +48,7 @@ def __init__(self, lane, longitude: float, lateral: float, static: bool = False,
         super(TrafficCone, self).__init__(lane, longitude, lateral, random_seed)
         self.add_body(BaseRigidBodyNode(self.name, self.NAME))
         self.body.addShape(BulletCylinderShape(self.RADIUS, self.HEIGHT))
+        self.body.setIntoCollideMask(self.COLLISION_GROUP)
         self.origin.setPos(panda_position(self.position, self.HEIGHT / 2))
         self.origin.setH(panda_heading(self.heading))
         if self.render:
@@ -56,7 +59,7 @@ def __init__(self, lane, longitude: float, lateral: float, static: bool = False,
         self.set_static(static)
 
 
-class TrafficTriangle(TrafficSign):
+class TrafficTriangle(TrafficObject):
     """Placed behind the vehicle when it breaks down"""
 
     NAME = BodyName.Traffic_triangle
@@ -66,6 +69,7 @@ def __init__(self, lane, longitude: float, lateral: float, static: bool = False,
         super(TrafficTriangle, self).__init__(lane, longitude, lateral, random_seed)
         self.add_body(BaseRigidBodyNode(self.name, self.NAME))
         self.body.addShape(BulletCylinderShape(self.RADIUS, self.HEIGHT))
+        self.body.setIntoCollideMask(self.COLLISION_GROUP)
         self.origin.setPos(panda_position(self.position, self.HEIGHT / 2))
         self.origin.setH(panda_heading(self.heading))
         if self.render:

pgdrive/constants.py

@@ -116,7 +116,7 @@ class CollisionGroup(Mask):
     Vehicle = BitMask32.bit(1)
     Terrain = BitMask32.bit(2)
     BrokenLaneLine = BitMask32.bit(3)
-    # TrafficVehicle = BitMask32.bit(4)
+    TrafficObject = BitMask32.bit(4)
     LaneSurface = BitMask32.bit(5)  # useless now, since it is in another bullet world
     Sidewalk = BitMask32.bit(6)
     ContinuousLaneLine = BitMask32.bit(7)
@@ -135,6 +135,7 @@ def collision_rules(cls):
             (cls.Terrain, cls.InvisibleWall, False),
             (cls.Terrain, cls.Sidewalk, True),
             (cls.Terrain, cls.LidarBroadDetector, False),
+            (cls.Terrain, cls.TrafficObject, True),
 
             # block collision
             (cls.BrokenLaneLine, cls.BrokenLaneLine, False),
@@ -145,6 +146,7 @@ def collision_rules(cls):
             (cls.BrokenLaneLine, cls.InvisibleWall, False),
             (cls.BrokenLaneLine, cls.Sidewalk, False),
             (cls.BrokenLaneLine, cls.LidarBroadDetector, False),
+            (cls.BrokenLaneLine, cls.TrafficObject, True),
 
             # ego vehicle collision
             (cls.Vehicle, cls.Vehicle, True),
@@ -153,31 +155,37 @@ def collision_rules(cls):
             (cls.Vehicle, cls.InvisibleWall, True),
             (cls.Vehicle, cls.Sidewalk, True),
             (cls.Vehicle, cls.LidarBroadDetector, True),
+            (cls.Vehicle, cls.TrafficObject, True),
 
             # lane surface
             (cls.LaneSurface, cls.LaneSurface, False),
             (cls.LaneSurface, cls.ContinuousLaneLine, False),
             (cls.LaneSurface, cls.InvisibleWall, False),
             (cls.LaneSurface, cls.Sidewalk, False),
             (cls.LaneSurface, cls.LidarBroadDetector, False),
+            (cls.LaneSurface, cls.TrafficObject, True),
 
             # continuous lane line
             (cls.ContinuousLaneLine, cls.ContinuousLaneLine, False),
             (cls.ContinuousLaneLine, cls.InvisibleWall, False),
             (cls.ContinuousLaneLine, cls.Sidewalk, False),
             (cls.ContinuousLaneLine, cls.LidarBroadDetector, False),
+            (cls.ContinuousLaneLine, cls.TrafficObject, False),
 
             # invisible wall
             (cls.InvisibleWall, cls.InvisibleWall, False),
             (cls.InvisibleWall, cls.Sidewalk, False),
             (cls.InvisibleWall, cls.LidarBroadDetector, True),
+            (cls.InvisibleWall, cls.TrafficObject, False),
 
             # side walk
             (cls.Sidewalk, cls.Sidewalk, False),
             (cls.Sidewalk, cls.LidarBroadDetector, False),
+            (cls.Sidewalk, cls.TrafficObject, True),
 
             #
             (cls.LidarBroadDetector, cls.LidarBroadDetector, False),
+            (cls.LidarBroadDetector, cls.TrafficObject, True),
         ]
 
     @classmethod

pgdrive/engine/base_engine.py

@@ -337,12 +337,12 @@ def global_seed(self):
     def _object_clean_check(self):
         if self.global_config["debug"]:
             from pgdrive.component.vehicle.base_vehicle import BaseVehicle
-            from pgdrive.component.static_object.traffic_object import TrafficSign
+            from pgdrive.component.static_object.traffic_object import TrafficObject
             for manager in self._managers.values():
                 assert len(manager.spawned_objects) == 0
 
             objs_need_to_release = self.get_objects(
-                filter=lambda obj: isinstance(obj, BaseVehicle) or isinstance(obj, TrafficSign)
+                filter=lambda obj: isinstance(obj, BaseVehicle) or isinstance(obj, TrafficObject)
             )
             assert len(
                 objs_need_to_release) == 0, "You should clear all generated objects by using engine.clear_objects " \

pgdrive/envs/pgdrive_env.py

@@ -469,10 +469,10 @@ def setup_engine(self):
         self.engine.accept("b", self.bird_view_camera)
         self.engine.accept("q", self.chase_camera)
 
-        from pgdrive.manager.object_manager import TrafficSignManager
+        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", TrafficSignManager())
+        self.engine.register_manager("object_manager", TrafficObjectManager())
 
     @property
     def main_camera(self):

pgdrive/envs/safe_pgdrive_env.py

@@ -1,4 +1,5 @@
 from pgdrive.constants import TerminationState
+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
@@ -18,7 +19,7 @@ def default_config(self) -> Config:
                 "crash_object_penalty": 0.,
                 "out_of_road_penalty": 0.,
 
-                # ===== cost scheme
+                # ===== cost scheme =====
                 "crash_vehicle_cost": 1,
                 "crash_object_cost": 1,
                 "out_of_road_cost": 1.,  # only give penalty for out_of_road
@@ -92,7 +93,7 @@ def cost_function(self, vehicle_id: str):
             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 or vehicle.crash_sidewalk:
+        elif vehicle.out_of_route:
             step_info["cost"] = self.config["out_of_road_cost"]
         return step_info['cost'], step_info
 
@@ -103,13 +104,18 @@ def cost_function(self, vehicle_id: str):
             # "accident_prob": 1.0,
             "manual_control": True,
             "use_render": True,
-            # "environment_num": 1,
-            # "start_seed": 187,
-            "out_of_road_cost": 1,
             "debug": True,
-            "map": "CCC",
-            # "cull_scene": True,
+            'environment_num': 1,
+            "start_seed": 22,
+            "traffic_density": 0.2,
+            # "environment_num": 1,
+            # # "start_seed": 187,
+            # "out_of_road_cost": 1,
+            # "debug": True,
+            # "map": "CCC",
+            # # "cull_scene": True,
             "vehicle_config": {
+                "spawn_lane_index": (FirstPGBlock.NODE_2, FirstPGBlock.NODE_3, 2)
                 # "show_lidar": True,
                 # "show_side_detector": True,
                 # "show_lane_line_detector": True,

pgdrive/manager/object_manager.py

@@ -7,12 +7,12 @@
 from pgdrive.component.lane.abs_lane import AbstractLane
 from pgdrive.component.road.road import Road
 from pgdrive.component.road.road_network import LaneIndex
-from pgdrive.component.static_object.traffic_object import TrafficSign
+from pgdrive.component.static_object.traffic_object import TrafficObject
 from pgdrive.engine.engine_utils import get_engine
 from pgdrive.manager.base_manager import BaseManager
 
 
-class TrafficSignManager(BaseManager):
+class TrafficObjectManager(BaseManager):
     """
     This class is used to manager all static object, such as traffic cones, warning tripod.
     """
@@ -31,15 +31,15 @@ class TrafficSignManager(BaseManager):
     PROHIBIT_SCENE_PROB = 0.5  # the reset is the probability of break_down_scene
 
     def __init__(self):
-        super(TrafficSignManager, self).__init__()
+        super(TrafficObjectManager, self).__init__()
         self.accident_prob = 0.
         self.accident_lanes = []
 
     def before_reset(self):
         """
         Clear all objects in th scene
         """
-        super(TrafficSignManager, self).before_reset()
+        super(TrafficObjectManager, self).before_reset()
         self.accident_prob = self.engine.global_config["accident_prob"]
 
     def reset(self):