Fix side detector & lane line detector rendering (#522)

* fix a long lasting side detector / lane line detector

* fix

* fix

* format

pgdrive/component/vehicle/base_vehicle.py

@@ -643,12 +643,15 @@ def destroy(self):
         self.navigation.destroy()
         self.navigation = None
 
-        self.side_detector.destroy()
-        self.lane_line_detector.destroy()
-        self.lidar.destroy()
-        self.side_detector = None
-        self.lane_line_detector = None
-        self.lidar = None
+        if self.side_detector is not None:
+            self.side_detector.destroy()
+            self.side_detector = None
+        if self.lane_line_detector is not None:
+            self.lane_line_detector.destroy()
+            self.lane_line_detector = None
+        if self.lidar is not None:
+            self.lidar.destroy()
+            self.lidar = None
         if len(self.image_sensors) != 0:
             for sensor in self.image_sensors.values():
                 sensor.destroy()
@@ -765,10 +768,22 @@ def last_current(self):
     def detach_from_world(self, physics_world):
         super(BaseVehicle, self).detach_from_world(physics_world)
         self.navigation.detach_from_world()
+        if self.lidar is not None:
+            self.lidar.detach_from_world()
+        if self.side_detector is not None:
+            self.side_detector.detach_from_world()
+        if self.lane_line_detector is not None:
+            self.lane_line_detector.detach_from_world()
 
     def attach_to_world(self, parent_node_path, physics_world):
         super(BaseVehicle, self).attach_to_world(parent_node_path, physics_world)
         self.navigation.attach_to_world(self.engine)
+        if self.lidar is not None:
+            self.lidar.attach_to_world(self.engine)
+        if self.side_detector is not None:
+            self.side_detector.attach_to_world(self.engine)
+        if self.lane_line_detector is not None:
+            self.lane_line_detector.attach_to_world(self.engine)
 
     def set_break_down(self, break_down=True):
         self.break_down = break_down

pgdrive/component/vehicle_module/distance_detector.py

@@ -125,6 +125,14 @@ def set_start_phase_offset(self, angle: float):
     def __del__(self):
         logging.debug("Lidar is destroyed.")
 
+    def detach_from_world(self):
+        if isinstance(self.origin, NodePath):
+            self.origin.detachNode()
+
+    def attach_to_world(self, engine):
+        if isinstance(self.origin, NodePath):
+            self.origin.reparentTo(engine.render)
+
 
 class SideDetector(DistanceDetector):
     def __init__(self, num_lasers: int = 2, distance: float = 50, enable_show=False):

pgdrive/component/vehicle_module/navigation.py

@@ -234,12 +234,13 @@ def _get_info_for_checkpoint(self, lanes_id, lanes, ego_vehicle):
                 angle = ref_lane.end_phase - ref_lane.start_phase
             elif dir == -1:
                 angle = ref_lane.start_phase - ref_lane.end_phase
-        return (
-            clip((proj_heading / self.NAVI_POINT_DIST + 1) / 2, 0.0,
-                 1.0), clip((proj_side / self.NAVI_POINT_DIST + 1) / 2, 0.0,
-                            1.0), clip(bendradius, 0.0, 1.0), clip((dir + 1) / 2, 0.0, 1.0),
-            clip((np.rad2deg(angle) / BlockParameterSpace.CURVE[Parameter.angle].max + 1) / 2, 0.0, 1.0)
-        ), lanes_heading, check_point
+        ret = []
+        ret.append(clip((proj_heading / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0))
+        ret.append(clip((proj_side / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0))
+        ret.append(clip(bendradius, 0.0, 1.0))
+        ret.append(clip((dir + 1) / 2, 0.0, 1.0))
+        ret.append(clip((np.rad2deg(angle) / BlockParameterSpace.CURVE[Parameter.angle].max + 1) / 2, 0.0, 1.0))
+        return ret, lanes_heading, check_point
 
     def _update_target_checkpoints(self, ego_lane_index, ego_lane_longitude):
         """

pgdrive/tests/vis_env/vis_pgdrive_env_v2.py

@@ -10,12 +10,18 @@
             "fast": True,
             "use_render": True,
             "manual_control": True,
-            # "vehicle_config": {
-            #     "show_side_detector": True,
-            #     "show_lane_line_detector": True,
-            #     "show_navi_mark": True,
-            #     "show_lidar": True,
-            # }
+            "vehicle_config": {
+                "side_detector": {
+                    "num_lasers": 120
+                },
+                "lane_line_detector": {
+                    "num_lasers": 120
+                },
+                "show_side_detector": True,
+                "show_lane_line_detector": True,
+                "show_navi_mark": True,
+                "show_lidar": True,
+            },
             "random_agent_model": True,
             "random_lane_width": True,
             "load_map_from_json": False