Fix light (#661)

* add more sanity ceck

* fix potential bug in trafic lights

* format

metadrive/scenario/scenario_description.py

@@ -115,7 +115,7 @@
 import os
 from collections import defaultdict
 from typing import Optional
-
+from metadrive.utils.math import norm
 import numpy as np
 
 from metadrive.type import MetaDriveType
@@ -163,6 +163,7 @@ class ScenarioDescription(dict):
     COORDINATE = "coordinate"
     SDC_ID = "sdc_id"  # Not necessary, but can be stored in metadata.
     METADATA_KEYS = {METADRIVE_PROCESSED, COORDINATE, TIMESTEP}
+    OLD_ORIGIN_IN_CURRENT_COORDINATE = "old_origin_in_current_coordinate"
 
     ALLOW_TYPES = (int, float, str, np.ndarray, dict, list, tuple, type(None), set)
 
@@ -265,6 +266,13 @@ def _check_map_features(cls, map_feature):
                 assert isinstance(
                     feature[ScenarioDescription.POLYLINE], (np.ndarray, list, tuple)
                 ), "lane center line is in invalid type"
+            if ScenarioDescription.POLYGON in feature and ScenarioDescription.POLYLINE in feature:
+                line_centroid = np.mean(feature["polyline"], axis=0)[:2]
+                polygon_centroid = np.mean(feature["polygon"], axis=0)[:2]
+                diff = line_centroid - polygon_centroid
+                assert norm(diff[0], diff[
+                    1]) < 100, "The distance between centroids of polyline and polygon is greater than 100m. " \
+                               "The map converter should be wrong!"
 
     @classmethod
     def _check_object_state_dict(cls, obj_state, scenario_length, object_id, valid_check=True):
@@ -659,6 +667,11 @@ def offset_scenario_with_new_origin(scenario, new_origin):
             if "polygon" in map_feature:
                 map_feature["polygon"] = np.asarray(map_feature["polygon"])
                 map_feature["polygon"][..., :2] -= new_origin
+
+        for light in scenario[ScenarioDescription.DYNAMIC_MAP_STATES].values():
+            if ScenarioDescription.TRAFFIC_LIGHT_POSITION in light:
+                light[ScenarioDescription.TRAFFIC_LIGHT_POSITION][..., :2] -= new_origin
+
         scenario["metadata"]["old_origin_in_current_coordinate"] = -new_origin
         return scenario