Skip to content

Commit

Permalink
feat: add parking space
Browse files Browse the repository at this point in the history
daohu527 committed Sep 10, 2024
1 parent 18837c4 commit 4c3704a
Showing 5 changed files with 53 additions and 17 deletions.
30 changes: 30 additions & 0 deletions imap/lib/common.py
Original file line number Diff line number Diff line change
@@ -106,6 +106,17 @@ def __str__(self):
self.y, self.z, self.s, self.yaw)


def binary_search(arr, val):
left, right = 0, len(arr) - 1
while left <= right:
mid = math.floor((left + right)/2)
if arr[mid] <= val:
left = mid + 1
else:
right = mid - 1
return left - 1


def shift_t(point3d, offset):
npoint = copy.deepcopy(point3d)
npoint.shift_t(offset)
@@ -134,6 +145,25 @@ def calc_length(points):
return length


def get_rotated_rectangle_points(center, hdg, height, width):
cx, cy = center
half_height, half_width = height / 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
]

# Rotate each corner and translate by the center
return [
(cx + x * math.cos(hdg) - y * math.sin(hdg),
cy + x * math.sin(hdg) + y * math.cos(hdg))
for x, y in corners
]

if __name__ == '__main__':
vec_x = Vector3d(0.9201668879354276, -0.3915263699257437, 0)
vec_z = Vector3d(0, 0, 1)
14 changes: 10 additions & 4 deletions imap/lib/convertor.py
Original file line number Diff line number Diff line change
@@ -550,10 +550,16 @@ def convert_signals(self, xodr_road, pb_last_section):
for pb_lane in pb_last_section:
self.construct_signal_overlap(pb_lane, pb_signal)

def convert_objects(self):
# xodr_road.reference_line
# pb_parking_space = self.pb_map.parking_space.add()
pass
def convert_objects(self, xodr_road):
reference_line = xodr_road.reference_line
for obj in xodr_road.objects.objects:
pb_parking_space = self.pb_map.parking_space.add()
# Not sure if this is correct, need to pay attention to the units
pb_parking_space.heading = obj.hdg
for corner in obj.process_corners(reference_line):
point = pb_parking_space.polygon.point.add()
point.x, point.y = corner


def convert_roads(self):
for _, xodr_road in self.xodr_map.roads.items():
13 changes: 1 addition & 12 deletions imap/lib/opendrive/lanes.py
Original file line number Diff line number Diff line change
@@ -19,23 +19,12 @@

import imap.global_var as global_var

from imap.lib.common import shift_t, calc_length
from imap.lib.common import shift_t, calc_length, binary_search
from imap.lib.draw import draw_line

from imap.lib.opendrive.common import convert_speed


def binary_search(arr, val):
left, right = 0, len(arr) - 1
while left <= right:
mid = math.floor((left + right)/2)
if arr[mid] <= val:
left = mid + 1
else:
right = mid - 1
return left - 1


def is_adjacent(road_marks) -> bool:
if not road_marks:
return True
11 changes: 11 additions & 0 deletions imap/lib/opendrive/objects.py
Original file line number Diff line number Diff line change
@@ -17,6 +17,7 @@
import abc
from enum import Enum

from imap.lib.common import shift_t, binary_search

class ObjectType(Enum):
BARRIER = "barrier"
@@ -83,6 +84,16 @@ class ParkingSpace(Object):
def __init__(self) -> None:
pass

def process_corners(self, reference_line):
# Find the point closest to s, considering adding interpolation
idx = binary_search([point3d.s for point3d in reference_line], self.s)
reference_point3d = reference_line[idx]
inertial_point3d = shift_t(point3d, self.t * self.direction)
center = [inertial_point3d.x, inertial_point3d.y]
corners = get_rotated_rectangle_points(center, self.hdg, self.height, self.width)
return corners


class Pole(Object):
def __init__(self) -> None:
pass
2 changes: 1 addition & 1 deletion setup.py
Original file line number Diff line number Diff line change
@@ -5,7 +5,7 @@

setuptools.setup(
name="imap_box",
version="0.1.9",
version="0.1.10",
author="daohu527",
author_email="[email protected]",
description="High-resolution map visualization and conversion tool",

0 comments on commit 4c3704a

Please sign in to comment.