Fix a bug in visualization & Update observation (#529)

* fix a bug in visualization * update the obs
decisionforce · Aug 23, 2021 · e7ab12c · e7ab12c
1 parent 6be95b0
commit e7ab12c
Show file tree

Hide file tree

Showing 10 changed files with 75 additions and 52 deletions.
diff --git a/pgdrive/component/vehicle/base_vehicle.py b/pgdrive/component/vehicle/base_vehicle.py
@@ -199,19  199,23 @@ def _add_modules_for_vehicle(self, ):
         config = self.config
 
         # add routing module
-        self.add_navigation(config["show_navi_mark"])  # default added
         self.add_navigation()  # default added
 
         # add distance detector/lidar
         self.side_detector = SideDetector(
-            config["side_detector"]["num_lasers"], config["side_detector"]["distance"], config["show_side_detector"]
             config["side_detector"]["num_lasers"], config["side_detector"]["distance"],
             self.engine.global_config["vehicle_config"]["show_side_detector"]
         )
 
         self.lane_line_detector = LaneLineDetector(
             config["lane_line_detector"]["num_lasers"], config["lane_line_detector"]["distance"],
-            config["show_lane_line_detector"]
             self.engine.global_config["vehicle_config"]["show_lane_line_detector"]
         )
 
-        self.lidar = Lidar(config["lidar"]["num_lasers"], config["lidar"]["distance"], config["show_lidar"])
         self.lidar = Lidar(
             config["lidar"]["num_lasers"], config["lidar"]["distance"],
             self.engine.global_config["vehicle_config"]["show_lidar"]
         )
 
         # vision modules
         self.add_image_sensor("rgb_camera", RGBCamera())
@@ -573,14  577,13 @@ def _add_wheel(self, pos: Vec3, radius: float, front: bool, left):
     def add_image_sensor(self, name: str, sensor: ImageBuffer):
         self.image_sensors[name] = sensor
 
-    def add_navigation(self, show_navi_mark: bool = False):
-        config = self.config
     def add_navigation(self):
         self.navigation = Navigation(
             self.engine,
-            show_navi_mark=show_navi_mark,
-            random_navi_mark_color=config["random_navi_mark_color"],
-            show_dest_mark=config["show_dest_mark"],
-            show_line_to_dest=config["show_line_to_dest"]
             show_navi_mark=self.engine.global_config["vehicle_config"]["show_navi_mark"],
             random_navi_mark_color=self.engine.global_config["vehicle_config"]["random_navi_mark_color"],
             show_dest_mark=self.engine.global_config["vehicle_config"]["show_dest_mark"],
             show_line_to_dest=self.engine.global_config["vehicle_config"]["show_line_to_dest"]
         )
 
     def update_map_info(self, map):
@@ -774,24  777,25 @@ def last_current(self):
         return self.last_current_action[0]
 
     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()
         super(BaseVehicle, self).detach_from_world(physics_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:
         if self.config["show_navi_mark"]:
             self.navigation.attach_to_world(self.engine)
         if self.lidar is not None and self.config["show_lidar"]:
             self.lidar.attach_to_world(self.engine)
-        if self.side_detector is not None:
         if self.side_detector is not None and self.config["show_side_detector"]:
             self.side_detector.attach_to_world(self.engine)
-        if self.lane_line_detector is not None:
         if self.lane_line_detector is not None and self.config["show_lane_line_detector"]:
             self.lane_line_detector.attach_to_world(self.engine)
         super(BaseVehicle, self).attach_to_world(parent_node_path, physics_world)
 
     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
@@ -29,7  29,7 @@ class DistanceDetector:
     MARK_COLOR = (51 / 255, 221 / 255, 1)
     ANGLE_FACTOR = False
 
-    def __init__(self, num_lasers: int = 16, distance: float = 50, enable_show=False):
     def __init__(self, num_lasers: int = 16, distance: float = 50, enable_show=True):
         # properties
         self.available = True if num_lasers > 0 and distance > 0 else False
         parent_node_np: NodePath = get_engine().render
@@ -135,7  135,7 @@ def attach_to_world(self, engine):
 
 
 class SideDetector(DistanceDetector):
-    def __init__(self, num_lasers: int = 2, distance: float = 50, enable_show=False):
     def __init__(self, num_lasers: int = 2, distance: float = 50, enable_show=True):
         super(SideDetector, self).__init__(num_lasers, distance, enable_show)
         self.set_start_phase_offset(90)
         self.origin.hide(CamMask.RgbCam | CamMask.Shadow | CamMask.Shadow | CamMask.DepthCam)
@@ -145,7  145,7 @@ def __init__(self, num_lasers: int = 2, distance: float = 50, enable_show=False)
 class LaneLineDetector(SideDetector):
     MARK_COLOR = (1, 77 / 255, 77 / 255)
 
-    def __init__(self, num_lasers: int = 2, distance: float = 50, enable_show=False):
     def __init__(self, num_lasers: int = 2, distance: float = 50, enable_show=True):
         super(SideDetector, self).__init__(num_lasers, distance, enable_show)
         self.set_start_phase_offset(90)
         self.origin.hide(CamMask.RgbCam | CamMask.Shadow | CamMask.Shadow | CamMask.DepthCam)

diff --git a/pgdrive/component/vehicle_module/navigation.py b/pgdrive/component/vehicle_module/navigation.py
@@ -185,11  185,15 @@ def update_localization(self, ego_vehicle):
         self._navi_info.fill(0.0)
         half = self.navigation_info_dim // 2
         self._navi_info[:half], lanes_heading1, checkpoint = self._get_info_for_checkpoint(
-            lanes_id=0, lanes=target_lanes_1, ego_vehicle=ego_vehicle
             lanes_id=0,
             lanes=target_lanes_1,
             ego_vehicle=ego_vehicle  #, print_info=False
         )
 
         self._navi_info[half:], lanes_heading2, _ = self._get_info_for_checkpoint(
-            lanes_id=1, lanes=target_lanes_2, ego_vehicle=ego_vehicle
             lanes_id=1,
             lanes=target_lanes_2,
             ego_vehicle=ego_vehicle  #, print_info=True
         )
 
         if self._show_navi_info:
@@ -206,7  210,7 @@ def update_localization(self, ego_vehicle):
 
         return lane, lane_index
 
-    def _get_info_for_checkpoint(self, lanes_id, lanes, ego_vehicle):
     def _get_info_for_checkpoint(self, lanes_id, lanes, ego_vehicle, print_info=False):
         ref_lane = lanes[0]
         later_middle = (float(self.get_current_lane_num()) / 2 - 0.5) * self.get_current_lane_width()
         check_point = ref_lane.position(ref_lane.length, later_middle)
@@ -239,6  243,20 @@ def _get_info_for_checkpoint(self, lanes_id, lanes, ego_vehicle):
         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))
 
         # if print_info:
         #     print("[2ND] Distance to this navigation: {:.3f}, Angle: {:.3f}. Return value: {}".format(
         #         norm(proj_heading, proj_side),
         #         np.rad2deg(np.arctan2(proj_side, proj_heading)),
         #         ret
         #     ))
         # else:
         #     print("[1ND] Distance to this navigation: {:.3f}, Angle: {:.3f}. Return value: {}".format(
         #         norm(proj_heading, proj_side),
         #         np.rad2deg(np.arctan2(proj_side, proj_heading)),
         #         ret
         #     ))
 
         return ret, lanes_heading, check_point
 
     def _update_target_checkpoints(self, ego_lane_index, ego_lane_longitude):

diff --git a/pgdrive/engine/base_engine.py b/pgdrive/engine/base_engine.py
@@ -138,7  138,6 @@ def clear_objects(self, filter: Optional[Union[Callable, List]], force_destroy=F
         Since we don't expect a iterator, and the number of objects is not so large, we don't use built-in filter()
         If force_destroy=True, we will destroy this element instead of storing them for next time using
         """
-        clean_id = []
         if isinstance(filter, list):
             exclude_objects = {obj_id: self._spawned_objects[obj_id] for obj_id in filter}
         elif callable(filter):

diff --git a/pgdrive/envs/pgdrive_env.py b/pgdrive/envs/pgdrive_env.py
@@ -104,7  104,8 @@
     # ===== Cost Scheme =====
     crash_vehicle_cost=1,
     crash_object_cost=1,
-    out_of_road_cost=1.
     out_of_road_cost=1.,
     out_of_route_done=False,
 )
 
 
@@ -210,6  211,8 @@ def _is_out_of_road(self, vehicle):
         # return vehicle.on_yellow_continuous_line or (not vehicle.on_lane) or vehicle.crash_sidewalk
         ret = vehicle.on_yellow_continuous_line or vehicle.on_white_continuous_line or \
               (not vehicle.on_lane) or vehicle.crash_sidewalk
         if self.config["out_of_route_done"]:
             ret = ret or vehicle.out_of_route
         return ret
 
     def reward_function(self, vehicle_id: str):

diff --git a/pgdrive/envs/safe_pgdrive_env.py b/pgdrive/envs/safe_pgdrive_env.py
@@ -22,26  22,13 @@ def default_config(self) -> Config:
 
                 # ===== cost scheme =====
                 "crash_vehicle_cost": 1,
-                "crash_object_cost": 0.5,
                 "crash_object_cost": 1,
                 "out_of_road_cost": 1.,  # only give penalty for out_of_road
                 "use_lateral": False
             },
             allow_add_new_key=True
         )
         return config
-    def __init__(self,config):
-        super(SafePGDriveEnv, self).__init__(config)
-        self.episode_cost = 0
-
-    def reset(self, *args, **kwargs):
-        self.episode_cost = 0
-        return super(SafePGDriveEnv, self).reset(*args, **kwargs)
-
-    def cost_function(self, vehicle_id: str):
-        cost, step_info = super(SafePGDriveEnv, self).cost_function(vehicle_id)
-        self.episode_cost  = cost
-        step_info["total_cost"] = self.episode_cost
-        return cost, step_info
 
     def _post_process_config(self, config):
         config = super(SafePGDriveEnv, self)._post_process_config(config)
@@ -65,9  52,6 @@ def setup_engine(self):
         super(SafePGDriveEnv, self).setup_engine()
         self.engine.register_manager("object_manager", TrafficObjectManager())
 
-    def _is_out_of_road(self, vehicle):
-        return vehicle.out_of_route
-
 
 if __name__ == "__main__":
     env = SafePGDriveEnv(
@@ -83,7  67,7 @@ def _is_out_of_road(self, vehicle):
             # # "start_seed": 187,
             # "out_of_road_cost": 1,
             # "debug": True,
-            "map": "X",
             # "map": "CCC",
             # # "cull_scene": True,
             "vehicle_config": {
                 "spawn_lane_index": (FirstPGBlock.NODE_2, FirstPGBlock.NODE_3, 2)

diff --git a/pgdrive/manager/traffic_manager.py b/pgdrive/manager/traffic_manager.py
@@ -209,7  209,11 @@ def _create_vehicles_on_lane(self, traffic_density: float, lane: AbstractLane, i
                 vehicle_config={
                     "spawn_lane_index": lane.index,
                     "spawn_longitude": long,
-                    "enable_reverse": False
                     "enable_reverse": False,
                     "show_lidar": False,
                     "show_lane_line_detector": False,
                     "show_side_detector": False,
                     "show_navi_mark": False
                 }
             )
             from pgdrive.policy.idm_policy import IDMPolicy
@@ -265,6  269,15 @@ def _create_vehicles_once(self, map: BaseMap, traffic_density: float) -> None:
             from pgdrive.policy.idm_policy import IDMPolicy
             for v_config in selected:
                 vehicle_type = self.random_vehicle_type()
                 v_config.update(
                     {
                         "enable_reverse": False,
                         "show_lidar": False,
                         "show_lane_line_detector": False,
                         "show_side_detector": False,
                         "show_navi_mark": False
                     }
                 )
                 random_v = self.spawn_object(vehicle_type, vehicle_config=v_config)
                 self.engine.add_policy(random_v.id, IDMPolicy(random_v, self.generate_seed()))
                 vehicles_on_block.append(random_v)

diff --git a/pgdrive/obs/state_obs.py b/pgdrive/obs/state_obs.py
@@ -95,9  95,9 @@ def vehicle_state(self, vehicle):
 
         if vehicle.lane_line_detector.available:
             info  = vehicle.lane_line_detector.perceive(vehicle, vehicle.engine.physics_world.static_world).cloud_points
-        else:
-            _, lateral = vehicle.lane.local_coordinates(vehicle.position)
-            info.append(clip((lateral * 2 / vehicle.navigation.map.MAX_LANE_WIDTH   1.0) / 2.0, 0.0, 1.0))
         # else:
         #     _, lateral = vehicle.lane.local_coordinates(vehicle.position)
         #     info.append(clip((lateral * 2 / vehicle.navigation.map.MAX_LANE_WIDTH   1.0) / 2.0, 0.0, 1.0))
 
         # add vehicle length/width
         if self.config["random_agent_model"]:
@@ -109,7  109,7 @@ def get_side_detector_dim(self):
         dim = 0
         dim  = 2 if self.config["side_detector"]["num_lasers"] == 0 else \
             self.config["side_detector"]["num_lasers"]
-        dim  = 1 if self.config["lane_line_detector"]["num_lasers"] == 0 else \
         dim  = 0 if self.config["lane_line_detector"]["num_lasers"] == 0 else \
             self.config["lane_line_detector"]["num_lasers"]
         return dim
 

diff --git a/pgdrive/tests/test_functionality/test_ego_vehicle.py b/pgdrive/tests/test_functionality/test_ego_vehicle.py
@@ -34,8  34,8 @@ def test_base_vehicle():
         v_config = Config(BASE_DEFAULT_CONFIG["vehicle_config"]).update(PGDriveEnv_DEFAULT_CONFIG["vehicle_config"])
         v = engine.spawn_object(DefaultVehicle, vehicle_config=v_config, random_seed=0)
 
-        v.add_navigation(True)
-        v.add_navigation(False)
         v.add_navigation()
         v.add_navigation()
         v.navigation.set_force_calculate_lane_index(True)
         v.update_map_info(map)
 

diff --git a/pgdrive/tests/vis_env/vis_pgdrive_env_v2.py b/pgdrive/tests/vis_env/vis_pgdrive_env_v2.py
@@ -8,6  8,8 @@
             "start_seed": 0,
             "environment_num": 10,
             "fast": True,
             "map": 1,
             "traffic_density": 0.2,
             "use_render": True,
             "manual_control": True,
             "vehicle_config": {
@@ -17,9  19,9 @@
                 "lane_line_detector": {
                     "num_lasers": 120
                 },
-                "show_side_detector": True,
-                "show_lane_line_detector": True,
-                "show_navi_mark": True,
                 # "show_side_detector": True,
                 # "show_lane_line_detector": True,
                 # "show_navi_mark": True,
                 "show_lidar": True,
             },
             "random_agent_model": True,