adding test for utilities

Signed-off-by: Mamoru Sobue <[email protected]>

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,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)

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):

tmp/lanelet2_extension_python/src/utility.cpp

@@ -122,7 +122,7 @@ std::vector<double> getClosestCenterPose(
   static rclcpp::Serialization<geometry_msgs::msg::Point> 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_<std::vector<double>>("[position, quarternion]")
+  bp::class_<std::vector<double>>("[position, quaternion]")
     .def(bp::vector_indexing_suite<std::vector<double>>());
   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>("lanelet::ConstLanelets")
     .def(bp::vector_indexing_suite<lanelet::ConstLanelets>());
-  // NOTE: this is required for return-type of getsucceeding/PrecedingLaneletSequences
+  // NOTE: this is required for return-type of getSucceeding/PrecedingLaneletSequences
   bp::class_<std::vector<lanelet::ConstLanelets>>("std::vector<lanelet::ConstLanelets>")
     .def(bp::vector_indexing_suite<std::vector<lanelet::ConstLanelets>>());
   bp::def("getSucceedingLaneletSequences", lanelet::utils::query::getSucceedingLaneletSequences);