Fix obs lidar (#397)

* fix cost value in safe pgdrive env

* fix lidar detection error

* full test

* format

* add test script

pgdrive/envs/generation_envs/safe_pgdrive_env.py

@@ -70,18 +70,39 @@ def reward_function(self, vehicle_id: str):
             reward = -self.config["crash_object_penalty"]
         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 or vehicle.crash_sidewalk:
+            step_info["cost"] = self.config["out_of_road_cost"]
+        return step_info['cost'], step_info
+
 
 if __name__ == "__main__":
     env = SafePGDriveEnv(
         {
+            "accident_prob": 1.0,
             "manual_control": True,
             "use_render": True,
             "environment_num": 100,
-            "start_seed": 75,
+            "start_seed": 139,
+            "out_of_road_cost": 1,
             # "debug": True,
             "cull_scene": True,
             "pg_world_config": {
                 "pstats": True
+            },
+            "vehicle_config": {
+                "show_lidar": True,
+                "show_side_detector": True,
+                "show_lane_line_detector": True,
+                "side_detector": dict(num_lasers=2, distance=50),  # laser num, distance
+                "lane_line_detector": dict(num_lasers=2, distance=20),  # laser num, distance
             }
         }
     )
@@ -94,6 +115,7 @@ def reward_function(self, vehicle_id: str):
         env.render(text={"cost": total_cost, "seed": env.current_map.random_seed, "reward": r})
         if d:
             total_cost = 0
+            print("done_cost:{}".format(info["cost"]))
             print("Reset")
             env.reset()
     env.close()

pgdrive/scene_creator/object/traffic_object.py

@@ -7,6 +7,7 @@
 from pgdrive.utils.asset_loader import AssetLoader
 from pgdrive.utils.coordinates_shift import panda_position, panda_heading
 from pgdrive.utils.element import Element
+from pgdrive.utils import get_np_random, random_string, distance_greater, norm
 
 LaneIndex = Tuple[str, str, int]
 
@@ -32,7 +33,7 @@ class Object(Element):
     HEIGHT = 1.2
     MASS = 1
 
-    def __init__(self, lane, lane_index: LaneIndex, position: Sequence[float], heading: float = 0.):
+    def __init__(self, lane, lane_index: LaneIndex, position: Sequence[float], heading: float = 0., name=None):
         """
        :param lane: the lane to spawn object
         :param lane_index: the lane_index of the spawn point
@@ -47,6 +48,7 @@ def __init__(self, lane, lane_index: LaneIndex, position: Sequence[float], headi
         self.lane_index = lane_index
         self.lane = lane
         self.body_node = None
+        self.name = random_string() if name is None else name
 
     @classmethod
     def make_on_lane(cls, lane, lane_index: LaneIndex, longitudinal: float, lateral: float):

pgdrive/scene_creator/vehicle/base_vehicle_node.py

@@ -41,4 +41,4 @@ def destroy(self):
         self._base_vehicle = None
 
     def get_vehicle(self):
-        return self._base_vehicle
+        return self._base_vehicle

pgdrive/scene_manager/object_manager.py

@@ -182,3 +182,7 @@ def prohibit_scene(
     def destroy(self, pg_world: PGWorld):
         self._clear_objects(pg_world)
         self._spawned_objects = None
+
+    @property
+    def objects(self):
+        return self._spawned_objects

pgdrive/scene_manager/scene_manager.py

@@ -185,12 +185,19 @@ def update_state_for_all_target_vehicles(self):
             # a = set([v.name for v in self.traffic_mgr.vehicles])
             # b = set([v.name for v in self.target_vehicles.values()])
             # assert b.issubset(a)  # This may only happen during episode replays!
-            is_target_vehicle_dict = {v.name: self.traffic_mgr.is_target_vehicle(v) for v in self.traffic_mgr.vehicles}
+            is_target_vehicle_dict = {
+                v_obj.name: self.traffic_mgr.is_target_vehicle(v_obj)
+                for v_obj in self.traffic_mgr.vehicles + self.objects_mgr.objects
+            }
             self.detector_mask.update_mask(
-                position_dict={v.name: v.position
-                               for v in self.traffic_mgr.vehicles},
-                heading_dict={v.name: v.heading_theta
-                              for v in self.traffic_mgr.vehicles},
+                position_dict={
+                    v_obj.name: v_obj.position
+                    for v_obj in self.traffic_mgr.vehicles + self.objects_mgr.objects
+                },
+                heading_dict={
+                    v_obj.name: v_obj.heading_theta
+                    for v_obj in self.traffic_mgr.vehicles + self.objects_mgr.objects
+                },
                 is_target_vehicle_dict=is_target_vehicle_dict
             )
         step_infos = self.for_each_target_vehicle(

pgdrive/tests/test_funtionality/test_object_collision_detection.py

@@ -0,0 +1,46 @@
+from pgdrive.envs.generation_envs.side_pass_env import SidePassEnv
+from pgdrive.constants import BodyName
+
+
+def test_object_collision_detection(render=False):
+    env = SidePassEnv(
+        {
+            "manual_control": render,
+            "use_render": render,
+            "debug": False,
+            "vehicle_config": {
+                "show_lidar": True
+            }
+        }
+    )
+    try:
+        o = env.reset()
+        lane_index = (">>", ">>>", 0)
+        alert = env.scene_manager.objects_mgr.spawn_one_object(
+            "Traffic Triangle", env.current_map.road_network.get_lane(lane_index), lane_index, 22, 0
+        )
+        env.alert.attach_to_pg_world(env.pg_world.pbr_worldNP, env.pg_world.physics_world)
+        lane_index = (">>", ">>>", 2)
+        alert = env.scene_manager.objects_mgr.spawn_one_object(
+            BodyName.Traffic_cone, env.current_map.road_network.get_lane(lane_index), lane_index, 22, 0
+        )
+        env.alert.attach_to_pg_world(env.pg_world.pbr_worldNP, env.pg_world.physics_world)
+        crash_obj = False
+        detect_obj = False
+        for i in range(1, 100000 if render else 2000):
+            o, r, d, info = env.step([0, 1])
+            for obj in env.vehicle.lidar.get_detected_objects():
+                if obj.getNode().hasPythonTag(BodyName.Traffic_cone):
+                    detect_obj = True
+            if render:
+                env.render()
+            if info["crash_object"]:
+                crash_obj = True
+                break
+        assert crash_obj and detect_obj, "Can not crash with object!"
+    finally:
+        env.close()
+
+
+if __name__ == "__main__":
+    test_object_collision_detection(render=True)