diff --git a/tmp/lanelet2_extension_python/.gitignore b/tmp/lanelet2_extension_python/.gitignore deleted file mode 100644 index 8ba839c5..00000000 --- a/tmp/lanelet2_extension_python/.gitignore +++ /dev/null @@ -1 +0,0 @@ -example/sample-map-planning diff --git a/tmp/lanelet2_extension_python/example/example.py b/tmp/lanelet2_extension_python/example/example.py index 5db2cf4c..2ba57f47 100644 --- a/tmp/lanelet2_extension_python/example/example.py +++ b/tmp/lanelet2_extension_python/example/example.py @@ -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, **kwargs): + xs = [pt.x for pt in linestring] + ys = [pt.y for pt in linestring] + ax.plot(xs, ys, color=color, linestyle=linestyle, label=label, **kwargs) + + +def test_utility_utilities(lanelet_map, routing_graph): + lanelet108 = lanelet_map.laneletLayer.get(108) + lanelet108_next = query.getSucceedingLaneletSequences(routing_graph, lanelet108, 100.0) + lanelet108_seq = [lanelet108, *lanelet108_next[0]] + 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,23 +171,18 @@ 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("", proj) traffic_rules = lanelet2.traffic_rules.create( lanelet2.traffic_rules.Locations.Germany, @@ -100,5 +190,5 @@ def test_utility_utilities(): ) 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) diff --git a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py index 12031dc8..27acf993 100644 --- a/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py +++ b/tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py @@ -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): diff --git a/tmp/lanelet2_extension_python/src/utility.cpp b/tmp/lanelet2_extension_python/src/utility.cpp index 44dedad9..fa69bc34 100644 --- a/tmp/lanelet2_extension_python/src/utility.cpp +++ b/tmp/lanelet2_extension_python/src/utility.cpp @@ -122,7 +122,7 @@ std::vector getClosestCenterPose( static rclcpp::Serialization serializer_point; serializer_point.deserialize_message(&serialized_point_msg, &search_point); const geometry_msgs::msg::Pose pose = lanelet::utils::getClosestCenterPose(lanelet, search_point); - // NOTE: it was difficult to return the deserialized pose_byte and seriealize the pose_byte on + // NOTE: it was difficult to return the deserialized pose_byte and serialize the pose_byte on // python-side. So this function returns [*position, *quaternion] as double array const auto & xyz = pose.position; const auto & quat = pose.orientation; @@ -429,7 +429,7 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) bp::def("isInLanelet", ::isInLanelet, isInLanelet_overload()); // depends ros msg bp::def("getClosestCenterPose", ::getClosestCenterPose); // depends ros msg // NOTE: required for the return-value of getClosestCenterPose - bp::class_>("[position, quarternion]") + bp::class_>("[position, quaternion]") .def(bp::vector_indexing_suite>()); bp::def("getLateralDistanceToCenterline", ::getLateralDistanceToCenterline); // depends ros msg bp::def( @@ -550,7 +550,7 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) // NOTE: this is required for iterating getCurrentLanelets return value directly bp::class_("lanelet::ConstLanelets") .def(bp::vector_indexing_suite()); - // NOTE: this is required for return-type of getsucceeding/PrecedingLaneletSequences + // NOTE: this is required for return-type of getSucceeding/PrecedingLaneletSequences bp::class_>("std::vector") .def(bp::vector_indexing_suite>()); bp::def("getSucceedingLaneletSequences", lanelet::utils::query::getSucceedingLaneletSequences);