From df2be2b026e9439ab22c4f296e3f44a1c28cafc1 Mon Sep 17 00:00:00 2001
From: daohu527 <daohu527@gmail.com>
Date: Wed, 11 Sep 2024 11:15:30 +0800
Subject: [PATCH] fix: fix parking space hdg

---
 imap/lib/common.py            | 12 ++++++------
 imap/lib/opendrive/objects.py |  7 +++++--
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/imap/lib/common.py b/imap/lib/common.py
index 91813f0..15a8ab4 100644
--- a/imap/lib/common.py
+++ b/imap/lib/common.py
@@ -145,16 +145,16 @@ def calc_length(points):
     return length
 
 
-def get_rotated_rectangle_points(center, hdg, height, width):
+def get_rotated_rectangle_points(center, hdg, length, width):
     cx, cy = center
-    half_height, half_width = height / 2, width / 2
+    half_length, half_width = length / 2, width / 2
 
     # Rectangle's 4 corners relative to the center
     corners = [
-        (-half_width, -half_height),  # bottom-left
-        (half_width, -half_height),   # bottom-right
-        (half_width, half_height),    # top-right
-        (-half_width, half_height)    # top-left
+        (-half_length, -half_width),  # bottom-left
+        (half_length, -half_width),   # bottom-right
+        (half_length, half_width),    # top-right
+        (-half_length, half_width)    # top-left
     ]
 
     # Rotate each corner and translate by the center
diff --git a/imap/lib/opendrive/objects.py b/imap/lib/opendrive/objects.py
index c236abb..f8974e3 100644
--- a/imap/lib/opendrive/objects.py
+++ b/imap/lib/opendrive/objects.py
@@ -50,7 +50,7 @@ def parse_from(self, raw_object):
         self.hdg = float(raw_object.attrib.get('hdg'))
         self.pitch = float(raw_object.attrib.get('pitch'))
         self.roll = float(raw_object.attrib.get('roll'))
-        self.radius = float(raw_object.attrib.get('radius'))
+        self.radius = raw_object.attrib.get('radius')
         self.validLength = float(raw_object.attrib.get('validLength'))
         self.dynamic = raw_object.attrib.get('dynamic')
         self.perpToRoad = raw_object.attrib.get('perpToRoad')
@@ -91,8 +91,11 @@ def process_corners(self, reference_line):
         reference_point3d = reference_line[idx]
         inertial_point3d = shift_t(reference_point3d, self.t)
         center = [inertial_point3d.x, inertial_point3d.y]
+        # todo(zero): Because the object's angle is relative to the road,
+        # the road's angle needs to be added 'self.hdg + reference_point3d.yaw',
+        # but do we need to consider the inclination of the road?
         corners = get_rotated_rectangle_points(
-            center, self.hdg, self.height, self.width)
+            center, self.hdg + reference_point3d.yaw, self.length, self.width)
         return corners