Unify all coordinates to right-handed/remove vehicle 90 degree offset (…

…#420)

* opendrive lane warning

* Fix Straight Lane

* add is_clockwise() function

* circular lane works

* fix show_coordinates/fix_main_camera/navi_info_rendering

* fix heading theta bug

* fix circular lane bug

* fix get_vertical_dir bug

* fix navi angle/direction bug

* merge/reimplement coordinate projection

* fix test component

* waymo env stop converting

* fix trajectoryIDM

* remove all coordinate_transform and convert_polyline_to_metadrive

* fix top down metadrive

* remove invalid coordinate transform

* waymo env example top-down

* restore

* remove all invalid coordinate trans

* waymo position 2 dim

* add top down to waymo

* fix import error

* fix test SD  position bug

* format

* wrap to pi when testing heading consistency

* show coordinate

* vehicle face x

* format

* remove offset 90 degree

* fix bug

* fix test

* fix test

* numpy expert obs correction

README.md

@@ -82,7 +82,7 @@ You can also launch an instance of Multi-Agent scenario as follows
 python -m metadrive.examples.drive_in_multi_agent_env --env roundabout
 ```
 ```--env```  accepts following parmeters: `roundabout` (default), `intersection`, `tollgate`, `bottleneck`, `parkinglot`, `pgmap`.
-Adding ```--pygame_render``` can launch top-down pygame renderer. 
+Adding ```--top_down``` can launch top-down pygame renderer. 
 
 
 

metadrive/base_class/base_object.py

@@ -283,32 +283,26 @@ def set_position(self, position, height=None):
     def position(self):
         return metadrive_vector(self.origin.getPos())
 
-    def set_velocity(self, direction: np.array, value=None, in_local_frame=False, offset_90_deg=False):
+    def set_velocity(self, direction: np.array, value=None, in_local_frame=False):
         """
         Set velocity for object including the direction of velocity and the value (speed)
         The direction of velocity will be normalized automatically, value decided its scale
         :param direction: 2d array or list
         :param value: speed [m/s]
         :param in_local_frame: True, apply speed to local fram
-        :param offset_90_deg: set to True, only if using vehicle which has a 90 degree offset
         """
         if in_local_frame:
             from metadrive.engine.engine_utils import get_engine
             engine = get_engine()
             direction = LVector3(*direction, 0.)
-            direction[1] *= -1
             direction = engine.worldNP.getRelativeVector(self.origin, direction)
 
-            # Vehicle coordinates has 90 degrees offset as its heading is in the panda's - y-direction.
-            if offset_90_deg:
-                direction = [-direction[1], -direction[0]]
-
         if value is not None:
             norm_ratio = value / (norm(direction[0], direction[1]) + 1e-6)
         else:
             norm_ratio = 1
         self._body.setLinearVelocity(
-            LVector3(direction[0] * norm_ratio, -direction[1] * norm_ratio,
+            LVector3(direction[0] * norm_ratio, direction[1] * norm_ratio,
                      self._body.getLinearVelocity()[-1])
         )
 
@@ -326,7 +320,7 @@ def velocity(self):
         Velocity, unit: m/s
         """
         velocity = self.body.get_linear_velocity()
-        return np.asarray([velocity[0], -velocity[1]])
+        return np.asarray([velocity[0], velocity[1]])
 
     @property
     def velocity_km_h(self):
@@ -370,7 +364,7 @@ def heading_theta(self):
         Get the heading theta of this object, unit [rad]
         :return:  heading in rad
         """
-        return wrap_to_pi(metadrive_heading(self.origin.getH()) / 180 * math.pi)
+        return wrap_to_pi(self.origin.getH() / 180 * math.pi)
 
     @property
     def heading(self):
@@ -456,7 +450,6 @@ def get_z(self):
         return self.origin.getZ()
 
     def set_angular_velocity(self, angular_velocity, in_rad=True):
-        angular_velocity *= -1  # to panda coordinates
         if not in_rad:
             angular_velocity = angular_velocity / 180 * np.pi
         self._body.setAngularVelocity(LVector3(0, 0, angular_velocity))
@@ -469,3 +462,25 @@ def rename(self, new_name):
 
     def random_rename(self):
         self.rename(random_string())
+
+    def convert_to_local_coordinates(self, vector):
+        """
+        Give a world position, and convert it to object coordinates
+        TODO(LQY): add test
+        """
+        vector = LVector3(*vector, 0.)
+        vector = self.origin.getRelativeVector(self.engine.origin, vector)
+        project_on_x = vector[0]
+        project_on_y = vector[1]
+        return np.array([project_on_x, project_on_y])
+
+    def convert_to_world_coordinates(self, vector):
+        """
+        Give a position in world coordinates, and convert it to object coordinates
+        TODO(LQY): add test
+        """
+        vector = LVector3(*vector, 0.)
+        vector = self.engine.origin.getRelativeVector(self.origin, vector)
+        project_on_x = vector[0]
+        project_on_y = vector[1]
+        return np.array([project_on_x, project_on_y])

metadrive/component/lane/abs_lane.py

@@ -342,10 +342,10 @@ def construct_sidewalk_segment(block, lane_start, lane_end, length_multiply=1, e
 
         direction_v = lane_end - lane_start
         if extra_thrust != 0:
-            vertical_v = Vector((-direction_v[1], direction_v[0])) / norm(*direction_v)
+            vertical_v = Vector((direction_v[1], -direction_v[0])) / norm(*direction_v)
             middle += vertical_v * extra_thrust
         side_np.setPos(panda_vector(middle, 0))
-        theta = panda_heading(math.atan2(direction_v[1], direction_v[0]))
+        theta = math.atan2(direction_v[1], direction_v[0])
         side_np.setQuat(LQuaternionf(math.cos(theta / 2), 0, 0, math.sin(theta / 2)))
         side_np.setScale(length * length_multiply, width, block.SIDEWALK_THICKNESS * (1 + 0.1 * np.random.rand()))
         if block.render:

metadrive/component/lane/circular_lane.py

@@ -19,40 +19,46 @@ def __init__(
         center: Vector,
         radius: float,
         start_phase: float,
-        end_phase: float,
+        angle: float,
         clockwise: bool = True,
         width: float = PGLane.DEFAULT_WIDTH,
         line_types: Tuple[PGLineType, PGLineType] = (PGLineType.BROKEN, PGLineType.BROKEN),
         forbidden: bool = False,
         speed_limit: float = 1000,
         priority: int = 0
     ) -> None:
+        assert angle > 0, "Angle should be greater than 0"
         super().__init__()
         self.set_speed_limit(speed_limit)
         self.center = Vector(center)
         self.radius = radius
+        self._clock_wise = clockwise
         self.start_phase = start_phase
-        self.end_phase = end_phase
-        self.direction = 1 if clockwise else -1
+        self.end_phase = self.start_phase + (-angle if self.is_clockwise() else angle)
+        self.angle = angle
+        self.direction = -1 if clockwise else 1
         self.width = width
         self.line_types = line_types
         self.forbidden = forbidden
         self.priority = priority
 
         self.length = self.radius * (self.end_phase - self.start_phase) * self.direction
+        assert self.length > 0, "end_phase should > (<) start_phase if anti-clockwise (clockwise)"
         self.start = self.position(0, 0)
         self.end = self.position(self.length, 0)
 
     def update_properties(self):
         self.length = self.radius * (self.end_phase - self.start_phase) * self.direction
+        assert self.length > 0, "end_phase should > (<) start_phase if anti-clockwise (clockwise)"
         self.start = self.position(0, 0)
         self.end = self.position(self.length, 0)
 
     # def position(self, longitudinal: float, lateral: float) -> np.ndarray:
     def position(self, longitudinal: float, lateral: float) -> Vector:
         phi = self.direction * longitudinal / self.radius + self.start_phase
         # return self.center + (self.radius - lateral * self.direction) * np.array([math.cos(phi), math.sin(phi)])
-        return self.center + (self.radius - lateral * self.direction) * Vector((math.cos(phi), math.sin(phi)))
+        # return self.center + (self.radius - lateral * self.direction) * Vector((math.cos(phi), math.sin(phi)))
+        return self.center + (self.radius + lateral * self.direction) * Vector((math.cos(phi), math.sin(phi)))
 
     def heading_theta_at(self, longitudinal: float) -> float:
         phi = self.direction * longitudinal / self.radius + self.start_phase
@@ -69,7 +75,8 @@ def local_coordinates(self, position: Tuple[float, float]) -> Tuple[float, float
         phi = self.start_phase + wrap_to_pi(phi - self.start_phase)
         r = norm(delta_x, delta_y)
         longitudinal = self.direction * (phi - self.start_phase) * self.radius
-        lateral = self.direction * (self.radius - r)
+        # lateral = self.direction * (self.radius - r)
+        lateral = -self.direction * (self.radius - r)
         return longitudinal, lateral
 
     def construct_lane_in_block(self, block, lane_index):
@@ -86,9 +93,7 @@ def construct_lane_in_block(self, block, lane_index):
 
         for i in range(segment_num):
             middle = self.position(self.length * (i + .5) / segment_num, 0)
-            end = self.position(self.length * (i + 1) / segment_num, 0)
-            direction_v = end - middle
-            theta = math.atan2(direction_v[1], direction_v[0])
+            theta = self.heading_theta_at(self.length * (i + .5) / segment_num)
             width = self.width_at(0) + DrivableAreaProperty.SIDEWALK_LINE_DIST * 2
             length = self.length
             self._construct_lane_only_vis_segment(block, middle, width, length * 1.3 / segment_num, theta)
@@ -164,3 +169,6 @@ def polygon(self):
                         polygon.append([point[0], point[1], -0.5])
             self._polygon = polygon
         return self._polygon
+
+    def is_clockwise(self):
+        return self._clock_wise

metadrive/component/lane/opendrive_lane.py

@@ -1,3 +1,4 @@
+import logging
 from typing import Tuple
 
 import math
@@ -14,6 +15,7 @@ class OpenDriveLane(AbstractLane, InterpolatingLine):
     """An OpenDrive Lane"""
     def __init__(self, width, lane_data) -> None:
         AbstractLane.__init__(self)
+        logging.warning("Refactor OpenDriveLane, make it inherit from PointLane!")
         self.lane_data = lane_data
         self.index = get_lane_id(lane_data)
         self._section_index = lane_data.lane_section.idx
@@ -81,18 +83,8 @@ def heading_theta_at(self, longitudinal: float) -> float:
     def position(self, longitudinal: float, lateral: float) -> np.ndarray:
         return self.get_point(longitudinal, lateral)
 
-    def local_coordinates(self, position: Tuple[float, float]):
-        ret = []  # ret_longitude, ret_lateral, sort_key
-        accumulate_len = 0
-        for seg in self.segment_property:
-            delta_x = position[0] - seg["start_point"][0]
-            delta_y = position[1] - seg["start_point"][1]
-            longitudinal = delta_x * seg["direction"][0] + delta_y * seg["direction"][1]
-            lateral = delta_x * seg["lateral_direction"][0] + delta_y * seg["lateral_direction"][1]
-            ret.append([accumulate_len + longitudinal, lateral])
-            accumulate_len += seg["length"]
-        ret.sort(key=lambda seg: abs(seg[-1]))
-        return ret[0][0], ret[0][1]
+    def local_coordinates(self, position: Tuple[float, float], only_in_lane_point=False):
+        return InterpolatingLine.local_coordinates(self, position, only_in_lane_point)
 
     def construct_lane_in_block(self, block, lane_index):
         """

metadrive/component/lane/pg_lane.py

@@ -24,7 +24,7 @@ def construct_sidewalk(self, block, lateral):
             if radius == 0:
                 factor = 1
             else:
-                if self.direction == 1:
+                if self.is_clockwise():
                     factor = (1 - block.SIDEWALK_LINE_DIST / radius)
                 else:
                     factor = (1 + block.SIDEWALK_WIDTH / radius) * (1 + block.SIDEWALK_LINE_DIST / radius)

metadrive/component/lane/scenario_lane.py

@@ -9,7 +9,7 @@
 from metadrive.scenario.scenario_description import ScenarioDescription
 from metadrive.engine.asset_loader import AssetLoader
 from metadrive.utils.math_utils import norm, mph_to_kmh
-from metadrive.scenario.utils import read_scenario_data, convert_polyline_to_metadrive
+from metadrive.scenario.utils import read_scenario_data
 
 
 # return the nearest point"s index of the line
@@ -22,17 +22,13 @@ def nearest_point(point, line):
 class ScenarioLane(PointLane):
     VIS_LANE_WIDTH = 6
 
-    def __init__(self, lane_id: int, map_data: dict, need_lane_localization, coordinate_transform):
+    def __init__(self, lane_id: int, map_data: dict, need_lane_localization):
         """
         Extract the lane information of one lane, and do coordinate shift if required
         """
-        center_line_points = convert_polyline_to_metadrive(
-            map_data[lane_id][ScenarioDescription.POLYLINE], coordinate_transform=coordinate_transform
-        )
+        center_line_points = np.asarray(map_data[lane_id][ScenarioDescription.POLYLINE])
         if ScenarioDescription.POLYGON in map_data[lane_id] and len(map_data[lane_id][ScenarioDescription.POLYGON]) > 3:
-            polygon = convert_polyline_to_metadrive(
-                map_data[lane_id][ScenarioDescription.POLYGON], coordinate_transform=coordinate_transform
-            )
+            polygon = np.asarray(map_data[lane_id][ScenarioDescription.POLYGON])
         else:
             polygon = None
         assert "speed_limit_kmh" in map_data[lane_id] or "speed_limit_mph" in map_data[lane_id]
@@ -54,7 +50,7 @@ def __init__(self, lane_id: int, map_data: dict, need_lane_localization, coordin
         self.right_lanes = map_data[lane_id].get(ScenarioDescription.RIGHT_NEIGHBORS, None)
 
     @staticmethod
-    def try_get_polygon(map_data, lane_id, coordinate_transform):
+    def try_get_polygon(map_data, lane_id):
         """
         This method tries to infer polygon from the boundaries
         """
@@ -92,7 +88,7 @@ def try_get_polygon(map_data, lane_id, coordinate_transform):
             return None
         else:
             polygon = np.concatenate([left_boundary_points, right_boundary_points], axis=0)[..., :2]
-            return convert_polyline_to_metadrive(polygon, coordinate_transform)
+            return np.asarray(polygon)
 
     def get_lane_width(self, lane_id, map_data):
         """
@@ -139,4 +135,4 @@ def destroy(self):
     file_path = AssetLoader.file_path("waymo", "test.pkl", return_raw_style=False)
     data = read_scenario_data(file_path)
     print(data)
-    lane = ScenarioLane(108, data["map_features"], coordinate_transform=True)
+    lane = ScenarioLane(108, data["map_features"])

metadrive/component/lane/straight_lane.py

@@ -41,14 +41,14 @@ def __init__(
         self.length = norm((self.end - self.start)[0], (self.end - self.start)[1])
         self.heading = math.atan2(self.end[1] - self.start[1], self.end[0] - self.start[0])
         self.direction = (self.end - self.start) / self.length
-        self.direction_lateral = np.array([-self.direction[1], self.direction[0]])
+        self.direction_lateral = np.array([self.direction[1], -self.direction[0]])
 
     def update_properties(self):
         super(StraightLane, self).__init__()
         self.length = norm((self.end - self.start)[0], (self.end - self.start)[1])
         self.heading = math.atan2(self.end[1] - self.start[1], self.end[0] - self.start[0])
         self.direction = (self.end - self.start) / self.length
-        self.direction_lateral = np.array([-self.direction[1], self.direction[0]])
+        self.direction_lateral = np.array([self.direction[1], -self.direction[0]])
 
     def position(self, longitudinal: float, lateral: float) -> np.ndarray:
         return self.start + longitudinal * self.direction + lateral * self.direction_lateral

metadrive/component/map/scenario_map.py

@@ -1,11 +1,12 @@
 import logging
 
+import numpy as np
+
 from metadrive.component.map.base_map import BaseMap
 from metadrive.component.road_network.edge_road_network import EdgeRoadNetwork
 from metadrive.component.scenario_block.scenario_block import ScenarioBlock
 from metadrive.engine.asset_loader import AssetLoader
 from metadrive.type import MetaDriveType
-from metadrive.scenario.utils import convert_polyline_to_metadrive, read_scenario_data
 from metadrive.scenario.scenario_description import ScenarioDescription
 
 
@@ -58,23 +59,17 @@ def get_boundary_line_vector(self, interval):
                     ret[map_feat_id] = {
                         "type": MetaDriveType.LINE_BROKEN_SINGLE_YELLOW
                         if MetaDriveType.is_yellow_line(type) else MetaDriveType.LINE_BROKEN_SINGLE_WHITE,
-                        "polyline": convert_polyline_to_metadrive(
-                            data[ScenarioDescription.POLYLINE], coordinate_transform=self.coordinate_transform
-                        )
+                        "polyline": np.asarray(data[ScenarioDescription.POLYLINE])
                     }
                 else:
                     ret[map_feat_id] = {
-                        "polyline": convert_polyline_to_metadrive(
-                            data[ScenarioDescription.POLYLINE], coordinate_transform=self.coordinate_transform
-                        ),
+                        "polyline": np.asarray(data[ScenarioDescription.POLYLINE]),
                         "type": MetaDriveType.LINE_SOLID_SINGLE_YELLOW
                         if MetaDriveType.is_yellow_line(type) else MetaDriveType.LINE_SOLID_SINGLE_WHITE
                     }
             elif MetaDriveType.is_road_edge(type):
                 ret[map_feat_id] = {
-                    "polyline": convert_polyline_to_metadrive(
-                        data[ScenarioDescription.POLYLINE], coordinate_transform=self.coordinate_transform
-                    ),
+                    "polyline": np.asarray(data[ScenarioDescription.POLYLINE]),
                     "type": MetaDriveType.BOUNDARY_LINE
                 }
             elif type == MetaDriveType.LANE_SURFACE_STREET:
@@ -84,10 +79,6 @@ def get_boundary_line_vector(self, interval):
             #     raise ValueError
         return ret
 
-    @property
-    def coordinate_transform(self):
-        return self.engine.data_manager.coordinate_transform
-
     def get_map_features(self, interval=2):
         map_features = super(ScenarioMap, self).get_map_features(interval=interval)
 
@@ -135,14 +126,15 @@ def get_map_features(self, interval=2):
 
     # # touch these items so that pickle can work
 
-    file_path = AssetLoader.file_path("waymo", "0.pkl", return_raw_style=False)
-    # file_path = "/home/shady/Downloads/test_processed/60.pkl"
-    data = read_scenario_data(file_path)
+    # file_path = AssetLoader.file_path("waymo", "0.pkl", return_raw_style=False)
+    # # file_path = "/home/shady/Downloads/test_processed/60.pkl"
+    # data = read_scenario_data(file_path)
 
     default_config = ScenarioEnv.default_config()
     default_config["use_render"] = True
     default_config["debug"] = True
     default_config["debug_static_world"] = True
+    default_config["allow_coordinate_transform"] = True
     default_config["data_directory"] = AssetLoader.file_path("waymo", return_raw_style=False)
     # default_config["data_directory"] = "/home/shady/Downloads/test_processed"
     default_config["num_scenarios"] = 1