Skip to content
This repository has been archived by the owner on Jul 1, 2024. It is now read-only.

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
adding test for utilities
Browse files Browse the repository at this point in the history
Signed-off-by: Mamoru Sobue <[email protected]>
soblin committed Jan 15, 2024

Verified

This commit was signed with the committer’s verified signature.
jodal Stein Magnus Jodal
1 parent ae9dc5b commit ea14a14
Showing 2 changed files with 105 additions and 16 deletions.
114 changes: 102 additions & 12 deletions tmp/lanelet2_extension_python/example/example.py
Original file line number Diff line number Diff line change
@@ -4,6 +4,8 @@
import lanelet2.geometry
from lanelet2_extension_python.projection import MGRSProjector
import lanelet2_extension_python.utility.query as query
import lanelet2_extension_python.utility.utilities as utilities
import matplotlib.pyplot as plt
import numpy as np


@@ -15,11 +17,104 @@ def test_io(map_path, projection):
return lanelet2.io.load(map_path, projection)


def plot_ll2_id(ll2, ax, text):
xs, ys = np.array([pt.x for pt in ll2.centerline]), np.array([pt.y for pt in ll2.centerline])
x, y = np.average(xs), np.average(ys)
ax.text(x, y, text)


def plot_linestring(linestring, ax, color, linestyle, label, **kargs):

Check warning on line 26 in tmp/lanelet2_extension_python/example/example.py

GitHub Actions / spell-check-differential

Unknown word (kargs)
xs = [pt.x for pt in linestring]
ys = [pt.y for pt in linestring]
ax.plot(xs, ys, color=color, linestyle=linestyle, label=label, **kargs)

Check warning on line 29 in tmp/lanelet2_extension_python/example/example.py

GitHub Actions / spell-check-differential

Unknown word (kargs)


def test_utility_utilities(lanelet_map, routing_graph):
lanelet108 = lanelet_map.laneletLayer.get(108)
lanelet108_nexts = query.getSucceedingLaneletSequences(routing_graph, lanelet108, 100.0)

Check warning on line 34 in tmp/lanelet2_extension_python/example/example.py

GitHub Actions / spell-check-differential

Unknown word (nexts)
lanelet108_seq = [lanelet108, *lanelet108_nexts[0]]

Check warning on line 35 in tmp/lanelet2_extension_python/example/example.py

GitHub Actions / spell-check-differential

Unknown word (nexts)
lanelet108_seq_combined = utilities.combineLaneletsShape(lanelet108_seq)
lanelet108_seq_combined_fine_centerline = utilities.generateFineCenterline(
lanelet108_seq_combined, 1.0
)
lanelet108_seq_combined_right_offset = utilities.getRightBoundWithOffset(
lanelet108_seq_combined, 1.0, 1.0
)
lanelet108_seq_combined_left_offset = utilities.getLeftBoundWithOffset(
lanelet108_seq_combined, 1.0, 1.0
)
fig = plt.figure()
ax = fig.add_subplot(1, 3, 1)
ax.axis("equal")
plot_linestring(lanelet108_seq_combined.leftBound, ax, "orange", "-", "108 left")
plot_linestring(lanelet108_seq_combined.rightBound, ax, "orange", "-", "108 right")
plot_linestring(lanelet108_seq_combined_fine_centerline, ax, "orange", "--", "108 center")
plot_linestring(lanelet108_seq_combined_right_offset, ax, "cyan", "--", "108 right offset")
plot_linestring(lanelet108_seq_combined_left_offset, ax, "red", "--", "108 left offset")
[plot_ll2_id(ll2, ax, str(ll2.id)) for ll2 in lanelet108_seq]
ax.set_title("test of combineLaneletsShape ~ getLeftBoundWithOffset")

expanded_lanelet108 = utilities.getExpandedLanelet(lanelet108, 1.0, -1.0)
expanded_lanelet108_seq = utilities.getExpandedLanelets(lanelet108_seq, 2.0, -2.0)
ax = fig.add_subplot(1, 3, 2)
ax.axis("equal")
plot_linestring(expanded_lanelet108.leftBound, ax, "orange", "-", "expanded 108 left(1.0)")
plot_linestring(expanded_lanelet108.rightBound, ax, "orange", "-", "expanded 108 right(-1.0)")
plot_linestring(expanded_lanelet108.centerline, ax, "orange", "--", "expanded 108 center")
[
(
plot_linestring(ll2.leftBound, ax, "orange", "-", None),
plot_linestring(ll2.rightBound, ax, "orange", "-", None),
)
for ll2 in expanded_lanelet108_seq
]
[plot_ll2_id(ll2, ax, f"{ll2.id}(2.0)") for ll2 in expanded_lanelet108_seq]
ax.set_title("test of getExpandedLanelet(s)")

print(f"""{utilities.getLaneletLength2d(lanelet108)=}""")
print(f"""{utilities.getLaneletLength2d(lanelet108_seq)=}""")
print(f"""{utilities.getLaneletLength3d(lanelet108)=}""")
print(f"""{utilities.getLaneletLength3d(lanelet108_seq)=}""")

search_pose = Pose(position=Point(x=3685.0, y=73750.0))
arc_coords = utilities.getArcCoordinates(lanelet108_seq, search_pose)
print(
f"""utilities.getArcCoordinates(lanelet108_seq, search_pose) = (length: {arc_coords.length}, distance: {arc_coords.distance})"""
)
closest_lanelet = query.getClosestLanelet(lanelet108_seq, search_pose)
assert closest_lanelet.id == 156
closest_segment = utilities.getClosestSegment(
lanelet2.core.BasicPoint2d(x=search_pose.position.x, y=search_pose.position.y),
closest_lanelet.centerline,
)
ax = fig.add_subplot(1, 3, 3)
ax.axis("equal")
plot_linestring(closest_lanelet.leftBound, ax, "orange", "--", "156 left")
plot_linestring(closest_lanelet.rightBound, ax, "orange", "--", "156 right")
plot_linestring(closest_lanelet.centerline, ax, "cyan", "--", "156 center")
plot_linestring(closest_segment, ax, "red", "-", "closest segment", linewidth=2)
ax.scatter([search_pose.position.x], [search_pose.position.y], marker="o", label="search_pose")

print(f"""{utilities.getLaneletAngle(closest_lanelet, search_pose.position)=}""")
print(f"""{utilities.isInLanelet(search_pose, closest_lanelet)=}""")
print(f"""{utilities.isInLanelet(search_pose, closest_lanelet, 5.0)=}""")
closest_center_pose = utilities.getClosestCenterPose(closest_lanelet, search_pose.position)
ax.scatter(
[closest_center_pose.position.x],
[closest_center_pose.position.y],
marker="o",
label="closest_center_pose",
)
print(f"""{utilities.getLateralDistanceToCenterline(closest_lanelet, search_pose)=}""")
print(f"""{utilities.getLateralDistanceToClosestLanelet(lanelet108_seq, search_pose)=}""")
plt.legend()
plt.show()


def test_utility_query(lanelet_map, routing_graph):
lanelets = query.laneletLayer(lanelet_map)
lanelet56 = lanelet_map.laneletLayer.get(56)
lanelet108 = lanelet_map.laneletLayer.get(108)
print(f"{len(lanelets)=}")
print(f"""{len(query.subtypeLanelets(lanelets, "road"))=}""")
print(f"""{len(query.subtypeLanelets(lanelets, "road"))=}""")
print(f"""{len(query.crosswalkLanelets(lanelets))=}""")
@@ -76,29 +171,24 @@ def test_utility_query(lanelet_map, routing_graph):
)
print(f"""{[ll2.id for ll2 in query.getCurrentLanelets(lanelets, search_point)]=}""")
print(
f"""{[[ll2.id for ll2 in ll2s] for ll2s in query.getSucceedingLaneletSequences(routing_graph, lanelet108, 100.0)]}"""
f"""{[[ll2.id for ll2 in ll2s] for ll2s in query.getSucceedingLaneletSequences(routing_graph, lanelet108, 100.0)]=}"""
)
print(
f"""{[[ll2.id for ll2 in ll2s] for ll2s in query.getPrecedingLaneletSequences(routing_graph, lanelet108, 100.0)]}"""
f"""{[[ll2.id for ll2 in ll2s] for ll2s in query.getPrecedingLaneletSequences(routing_graph, lanelet108, 100.0)]=}"""
)


def test_utility_utilities():
pass


if __name__ == "__main__":
proj = test_projection()
lanelet_map = test_io(
"/home/mamorusobue/workspace/pilot-auto.xx1/src/autoware/common/tmp/lanelet2_extension_python/example/sample-map-planning/lanelet2_map.osm",
proj,
)
# https://docs.google.com/uc?export=download&id=1499_nsbUbIeturZaDj7jhUownh5fvXHd.
# See https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/
lanelet_map = test_io("<path to lanelet2_map.osm>", proj)

traffic_rules = lanelet2.traffic_rules.create(
lanelet2.traffic_rules.Locations.Germany,
lanelet2.traffic_rules.Participants.Vehicle,
)
routing_graph = lanelet2.routing.RoutingGraph(lanelet_map, traffic_rules)

test_utility_utilities()
test_utility_utilities(lanelet_map, routing_graph)
test_utility_query(lanelet_map, routing_graph)
Original file line number Diff line number Diff line change
@@ -14,8 +14,8 @@
getExpandedLanelets = _utility_cpp.getExpandedLanelets
overwriteLaneletsCenterline = _utility_cpp.overwriteLaneletsCenterline
getConflictingLanelets = _utility_cpp.getConflictingLanelets
lineStringWithWidthToPolygon = _utility_cpp._utility_cpp.lineStringWithWidthToPolygon
lineStringToPolygon = _utility_cpp._utility_cpp.lineStringToPolygon
lineStringWithWidthToPolygon = _utility_cpp.lineStringWithWidthToPolygon
lineStringToPolygon = _utility_cpp.lineStringToPolygon


def getLaneletLength2d(*args):
@@ -58,12 +58,11 @@ def isInLanelet(pose: Pose, lanelet, radius=0.0):


def getClosestCenterPose(lanelet, point: Point):
# https://dev.to/pgradot/sharing-strings-between-c-and-python-through-byte-buffers-1nj0
point_byte = serialize_message(point)
pose_array = _utility_cpp.getClosestCenterPose(lanelet, point_byte)
pos = Point(x=pose_array[0], y=pose_array[1], z=pose_array[2])
quat = Quaternion(x=pose_array[3], y=pose_array[4], z=pose_array[5], w=pose_array[6])
return Pose(position=pos, quaternion=quat)
return Pose(position=pos, orientation=quat)


def getLateralDistanceToCenterline(lanelet, pose: Pose):

0 comments on commit ea14a14

Please sign in to comment.