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

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
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.