diff --git a/pgdrive/component/vehicle/base_vehicle.py b/pgdrive/component/vehicle/base_vehicle.py index 9232c9f72..14b30d0a8 100644 --- a/pgdrive/component/vehicle/base_vehicle.py +++ b/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 diff --git a/pgdrive/component/vehicle_module/distance_detector.py b/pgdrive/component/vehicle_module/distance_detector.py index baa8e70bd..669b2eaa2 100644 --- a/pgdrive/component/vehicle_module/distance_detector.py +++ b/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): diff --git a/pgdrive/component/vehicle_module/navigation.py b/pgdrive/component/vehicle_module/navigation.py index 8efc98d2a..951d6f965 100644 --- a/pgdrive/component/vehicle_module/navigation.py +++ b/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): """ diff --git a/pgdrive/tests/vis_env/vis_pgdrive_env_v2.py b/pgdrive/tests/vis_env/vis_pgdrive_env_v2.py index 4910e39bb..f0313e80a 100644 --- a/pgdrive/tests/vis_env/vis_pgdrive_env_v2.py +++ b/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