finished except for getClosestCenterPose and query is tested complete…

…ly. return vector<double> for getClosestCenterPose at cpp

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

tmp/lanelet2_extension_python/example/example.py

@@ -1,5 +1,3 @@
-import math
-
 from geometry_msgs.msg import Point
 from geometry_msgs.msg import Pose
 import lanelet2
@@ -42,44 +40,47 @@ def test_utility_query(lanelet_map, routing_graph):
     print(f"""{len(query.getAllParkingSpaces(lanelet_map))=}""")
     print(f"""{len(query.getAllFences(lanelet_map))=}""")
     print(f"""{len(query.getAllPedestrianMarkings(lanelet_map))=}""")
-    print(f"""{len(query.getLinkedParkingSpaces(lanelet56, lanelet_map))=}""")
-    print(
-        f"""{len(query.getLinkedParkingSpaces(lanelet56, query.getAllParkingSpaces(lanelet_map), query.getAllParkingLots(lanelet_map)))=}"""
-    )
     print(f"""{len(query.stopLinesLanelets(lanelets))=}""")
     print(f"""{len(query.stopLinesLanelet(lanelet108))=}""")
     print(f"""{len(query.stopSignStopLines(lanelets))=}""")
-    lanelet56_centerline_basic_points = [p.basicPoint() for p in lanelet56.centerline]
-    lanelet56_centerline_center = np.sum(lanelet56_centerline_basic_points) * (
-        1.0 / len(lanelet56_centerline_basic_points)
+    lanelet56_centerline_center = np.sum([p.basicPoint() for p in lanelet56.centerline]) * (
+        1.0 / len(lanelet56.centerline)
+    )
+    search_point = Point(
+        x=lanelet56_centerline_center.x,
+        y=lanelet56_centerline_center.y,
+        z=lanelet56_centerline_center.z,
     )
-    search_point = Point()
-    search_point.x = lanelet56_centerline_center.x
-    search_point.y = lanelet56_centerline_center.y
-    search_point.z = lanelet56_centerline_center.z
+    print(f"""{search_point=}""")
     print(f"""{[ll2.id for ll2 in query.getLaneletsWithinRange(lanelets, search_point, 15.0)]=}""")
-    search_point_2d = lanelet2.core.BasicPoint2d()
-    search_point_2d.x = lanelet56_centerline_center.x
-    search_point_2d.y = lanelet56_centerline_center.y
+    search_point_2d = lanelet2.core.BasicPoint2d(
+        x=lanelet56_centerline_center.x, y=lanelet56_centerline_center.y
+    )
     print(
         f"""{[ll2.id for ll2 in query.getLaneletsWithinRange(lanelets, search_point_2d, 15.0)]=}"""
     )
     print(f"""{[ll2.id for ll2 in query.getLaneChangeableNeighbors(routing_graph, lanelet108)]=}""")
+    print(
+        f"""{[ll2.id for ll2 in query.getLaneChangeableNeighbors(routing_graph, query.roadLanelets(lanelets), search_point)]=}"""
+    )
     print(f"""{[ll2.id for ll2 in query.getAllNeighbors(routing_graph, lanelet56)]=}""")
+    print(
+        f"""{[ll2.id for ll2 in query.getAllNeighbors(routing_graph, query.roadLanelets(lanelets), search_point)]=}"""
+    )
     print(f"""{[ll2.id for ll2 in query.getAllNeighborsLeft(routing_graph, lanelet56)]=}""")
     print(f"""{[ll2.id for ll2 in query.getAllNeighborsRight(routing_graph, lanelet56)]=}""")
     search_pose = Pose()
     search_pose.position = search_point
-    temp_lanelet = lanelet2.core.Lanelet(0, lanelet56.leftBound, lanelet56.rightBound)
-    if (
-        query.getClosestLaneletWithConstrains(lanelets, search_pose, temp_lanelet, 10.0, math.pi)
-        and temp_lanelet is not None
-    ):
-        print(f"""{temp_lanelet.id}""")
-    # lanelet::ConstLaneletsへのpointerだからこの関数は無理
-    temp_lanelets = []
-    if query.getCurrentLanelets(lanelets, search_point, temp_lanelets):
-        print(f"""{[ll2.id for ll2 in temp_lanelets]}""")
+    print(
+        f"""{(query.getClosestLanelet(lanelets, search_pose).id if query.getClosestLanelet(lanelets, search_pose) else None)=}"""
+    )
+    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)]}"""
+    )
+    print(
+        f"""{[[ll2.id for ll2 in ll2s] for ll2s in query.getPrecedingLaneletSequences(routing_graph, lanelet108, 100.0)]}"""
+    )
 
 
 def test_utility_utilities():

tmp/lanelet2_extension_python/lanelet2_extension_python/utility/query.py

@@ -27,19 +27,10 @@
 getAllFences = _utility_cpp.getAllFences
 getAllPedestrianMarkings = _utility_cpp.getAllPedestrianMarkings
 getAllParkingSpaces = _utility_cpp.getAllParkingSpaces
-
-# TODO(Mamoru Sobue): how to dispatch overloads
 getLinkedParkingSpaces = _utility_cpp.getLinkedParkingSpaces
-
-# TODO(Mamoru Sobue): how to dispatch overloads
 getLinkedLanelet = _utility_cpp.getLinkedLanelet
-
-# TODO(Mamoru Sobue): how to dispatch overloads
 getLinkedLanelets = _utility_cpp.getLinkedLanelets
-
-# TODO(Mamoru Sobue): how to dispatch overloads
 getLinkedParkingLot = _utility_cpp.getLinkedParkingLot
-
 stopLinesLanelets = _utility_cpp.stopLinesLanelets
 stopLinesLanelet = _utility_cpp.stopLinesLanelet
 stopSignStopLines = _utility_cpp.stopSignStopLines
@@ -59,7 +50,7 @@ def getLaneChangeableNeighbors(*args):
         return _utility_cpp.getLaneChangeableNeighbors(args[0], args[1])
     if len(args) == 3 and isinstance(args[2], Point):
         point_byte = serialize_message(args[2])
-        return _utility_cpp.getLaneChangeableNeighbor_point(args[0], args[1], point_byte)
+        return _utility_cpp.getLaneChangeableNeighbors_point(args[0], args[1], point_byte)
     raise TypeError("argument number does not match or 3rd argument is not Point type")
 
 
@@ -68,13 +59,18 @@ def getAllNeighbors(*args):
         return _utility_cpp.getAllNeighbors(args[0], args[1])
     if len(args) == 3 and isinstance(args[2], Point):
         point_byte = serialize_message(args[2])
-        return _utility_cpp.getAllNeighbors(args[0], args[1], point_byte)
+        return _utility_cpp.getAllNeighbors_point(args[0], args[1], point_byte)
 
 
 getAllNeighborsLeft = _utility_cpp.getAllNeighborsLeft
 getAllNeighborsRight = _utility_cpp.getAllNeighborsRight
 
 
+def getClosestLanelet(lanelets, pose: Pose):
+    pose_byte = serialize_message(pose)
+    return _utility_cpp.getClosestLanelet(lanelets, pose_byte)
+
+
 def getClosestLaneletWithConstrains(
     lanelets, pose: Pose, closest_lanelet, dist_threshold, yaw_threshold
 ):
@@ -84,14 +80,14 @@ def getClosestLaneletWithConstrains(
     )
 
 
-def getCurrentLanelets(lanelets, point: Point, current_lanelets):
+def getCurrentLanelets(lanelets, point: Point):
     if isinstance(point, Point):
         point_byte = serialize_message(point)
-        return _utility_cpp.getCurrentLanelets_point(lanelets, point_byte, current_lanelets)
+        return _utility_cpp.getCurrentLanelets_point(lanelets, point_byte)
     if isinstance(point, Pose):
         pose_byte = serialize_message(point)
-        return _utility_cpp.getCurrentLanelets_pose(lanelets, pose_byte, current_lanelets)
-    raise TypeError("argument number does not match or 3rd argument is not Pose type")
+        return _utility_cpp.getCurrentLanelets_pose(lanelets, pose_byte)
+    raise TypeError("argument number does not match or 2nd argument is not Point/Pose type")
 
 
 getSucceedingLaneletSequences = _utility_cpp.getSucceedingLaneletSequences

tmp/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py

@@ -1,7 +1,8 @@
 from geometry_msgs.msg import Point
 from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Quaternion
+import lanelet2
 import lanelet2_extension_python._lanelet2_extension_python_boost_python_utility as _utility_cpp
-from rclpy.serialization import deserialize_message
 from rclpy.serialization import serialize_message
 
 combineLaneletsShape = _utility_cpp.combineLaneletsShape
@@ -12,8 +13,29 @@
 getExpandedLanelet = _utility_cpp.getExpandedLanelet
 getExpandedLanelets = _utility_cpp.getExpandedLanelets
 overwriteLaneletsCenterline = _utility_cpp.overwriteLaneletsCenterline
-getLaneletLength2d = _utility_cpp.getLaneletLength2d
-getLaneletLength3d = _utility_cpp.getLaneletLength3d
+getConflictingLanelets = _utility_cpp.getConflictingLanelets
+lineStringWithWidthToPolygon = _utility_cpp._utility_cpp.lineStringWithWidthToPolygon
+lineStringToPolygon = _utility_cpp._utility_cpp.lineStringToPolygon
+
+
+def getLaneletLength2d(*args):
+    if len(args) == 1 and isinstance(args[0], lanelet2.core.Lanelet):
+        return _utility_cpp.getLaneletLength2d(args[0])
+    if len(args) == 1 and isinstance(args[0], list):
+        return _utility_cpp.getLaneletLength2d(args[0])
+    raise TypeError(
+        "argument number does not match or 1st argument is not Lanelet or [Lanelet] type"
+    )
+
+
+def getLaneletLength3d(*args):
+    if len(args) == 1 and isinstance(args[0], lanelet2.core.Lanelet):
+        return _utility_cpp.getLaneletLength3d(args[0])
+    if len(args) == 1 and isinstance(args[0], list):
+        return _utility_cpp.getLaneletLength3d(args[0])
+    raise TypeError(
+        "argument number does not match or 1st argument is not Lanelet or [Lanelet] type"
+    )
 
 
 def getArcCoordinates(lanelet_sequence, pose: Pose):
@@ -38,21 +60,10 @@ 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_byte = _utility_cpp.getClosestCenterPose(lanelet, point_byte)
-    print(len(pose_byte))  # これは60ある
-    print(pose_byte[0].encode())  # これで b'\x00'と出た
-    byte_array = bytearray()
-    for i in range(len(pose_byte)):
-        # print(pose_byte[i])
-        print(
-            type(pose_byte[i].encode("unicode-escape").decode("unicode-escape")),
-            pose_byte[i].encode("unicode-escape").decode("unicode-escape"),
-        )
-        byte_array.append(
-            pose_byte[i].encode("unicode-escape").decode("unicode-escape")
-        )  # 0xb9で詰まる
-    print(byte_array)
-    return deserialize_message(byte_array, Pose)
+    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)
 
 
 def getLateralDistanceToCenterline(lanelet, pose: Pose):