feat: #348 use lanelet2 extension python (#356)

.driving_log_replayer.cspell.json

@@ -27,6 +27,7 @@
     "nums",
     "pydantic",
     "Kotaro",
-    "Uetake"
+    "Uetake",
+    "conlist"
   ]
 }

.vscode/settings.json

@@ -28,6 +28,7 @@
     "/opt/ros/humble/lib/python3.10/site-packages",
     "${env:HOME}/ros_ws/awf/install/autoware_auto_perception_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/autoware_auto_vehicle_msgs/local/lib/python3.10/dist-packages",
+    "${env:HOME}/ros_ws/awf/install/autoware_auto_mapping_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_planning_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/perception_eval/local/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/ros2_numpy/local/lib/python3.10/dist-packages",
@@ -37,12 +38,14 @@
     "${env:HOME}/ros_ws/awf/install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_localization_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_api_msgs/local/lib/python3.10/dist-packages",
-    "${env:HOME}/ros_ws/awf/install/tier4_perception_msgs/local/lib/python3.10/dist-packages"
+    "${env:HOME}/ros_ws/awf/install/tier4_perception_msgs/local/lib/python3.10/dist-packages",
+    "${env:HOME}/ros_ws/awf/install/lanelet2_extension_python/local/lib/python3.10/dist-packages"
   ],
   "python.analysis.extraPaths": [
     "/opt/ros/humble/lib/python3.10/site-packages",
     "${env:HOME}/ros_ws/awf/install/autoware_auto_perception_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/autoware_auto_vehicle_msgs/local/lib/python3.10/dist-packages",
+    "${env:HOME}/ros_ws/awf/install/autoware_auto_mapping_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_planning_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/perception_eval/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/ros2_numpy/local/lib/python3.10/dist-packages",
@@ -52,7 +55,8 @@
     "${env:HOME}/ros_ws/awf/install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_localization_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_api_msgs/local/lib/python3.10/dist-packages",
-    "${env:HOME}/ros_ws/awf/install/tier4_perception_msgs/local/lib/python3.10/dist-packages"
+    "${env:HOME}/ros_ws/awf/install/tier4_perception_msgs/local/lib/python3.10/dist-packages",
+    "${env:HOME}/ros_ws/awf/install/lanelet2_extension_python/local/lib/python3.10/dist-packages"
   ],
   "files.associations": {
     "cctype": "cpp",

driving_log_replayer/CMakeLists.txt

@@ -8,21 +8,6 @@ autoware_package()
 set(ROS_REQUIREMENTS "${CMAKE_CURRENT_SOURCE_DIR}/../requirements.txt")
 execute_process(COMMAND bash -c "python3 -m pip install -r ${ROS_REQUIREMENTS}")
 
-ament_auto_add_library(obstacle_segmentation_evaluator_component SHARED
-  src/obstacle_segmentation_evaluator_component.cpp
-)
-# Workaround for [email protected]
-if(TARGET lanelet2_core::lanelet2_core)
-  get_target_property(lanelet2_core_INCLUDE_DIRECTORIES lanelet2_core::lanelet2_core INTERFACE_INCLUDE_DIRECTORIES)
-  include_directories(SYSTEM
-    ${lanelet2_core_INCLUDE_DIRECTORIES}
-  )
-endif()
-rclcpp_components_register_node(obstacle_segmentation_evaluator_component
-  PLUGIN "driving_log_replayer::ObstacleSegmentationEvaluatorComponent"
-  EXECUTABLE obstacle_segmentation_evaluator_node
-)
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

driving_log_replayer/driving_log_replayer/evaluator.py

@@ -277,11 +277,16 @@ def initial_pose_cb(self, future: Future) -> None:
         # free self._initial_pose_running
         self._initial_pose_running = False
 
-    def lookup_transform(self, stamp: Stamp) -> TransformStamped:
+    def lookup_transform(
+        self,
+        stamp: Stamp,
+        from_: str = "map",
+        to: str = "base_link",
+    ) -> TransformStamped:
         try:
             return self._tf_buffer.lookup_transform(
-                "map",
-                "base_link",
+                from_,
+                to,
                 stamp,
                 Duration(seconds=0.5),
             )

driving_log_replayer/driving_log_replayer/lanelet2_util.py

@@ -0,0 +1,36 @@
+# Copyright (c) 2024 TIER IV.inc
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import Any
+
+import lanelet2
+from lanelet2.core import Lanelet
+from lanelet2_extension_python.projection import MGRSProjector
+from lanelet2_extension_python.utility import query
+from shapely.geometry import Polygon
+
+
+def road_lanelets_from_file(map_path: str) -> Any:
+    projection = MGRSProjector(lanelet2.io.Origin(0.0, 0.0))
+    lanelet_map = lanelet2.io.load(map_path, projection)
+    all_lanelets = query.laneletLayer(lanelet_map)
+    # return type lanelet2_extension_python._lanelet2_extension_python_boost_python_utility.lanelet::ConstLanelets
+    return query.roadLanelets(all_lanelets)
+
+
+def to_shapely_polygon(lanelet: Lanelet) -> Polygon:
+    points: list[float, float] = []
+    for p_2d in lanelet.polygon2d():
+        points.append([p_2d.x, p_2d.y])
+    return Polygon(points)

driving_log_replayer/driving_log_replayer/obstacle_segmentation.py

@@ -12,22 +12,32 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from copy import deepcopy
 from dataclasses import dataclass
 from pathlib import Path
 import sys
 
 from ament_index_python.packages import get_package_share_directory
+from builtin_interfaces.msg import Duration
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import PointStamped
+from geometry_msgs.msg import TransformStamped
+from geometry_msgs.msg import Vector3
 import numpy as np
 from perception_eval.common.object import DynamicObject
 from perception_eval.evaluation.sensing.sensing_frame_config import SensingFrameConfig
 from perception_eval.evaluation.sensing.sensing_frame_result import SensingFrameResult
 from perception_eval.evaluation.sensing.sensing_result import DynamicObjectWithSensingResult
 from pydantic import BaseModel
+from pydantic import conlist
+from pydantic import field_validator
 import ros2_numpy
 from rosidl_runtime_py import message_to_ordereddict
 from sensor_msgs.msg import PointCloud2
+from shapely.geometry import Polygon
 from std_msgs.msg import ColorRGBA
 from std_msgs.msg import Header
+from tf2_geometry_msgs import do_transform_point
 from typing_extensions import Literal
 from visualization_msgs.msg import Marker
 from visualization_msgs.msg import MarkerArray
@@ -49,10 +59,35 @@
 
 
 class ProposedAreaCondition(BaseModel):
-    polygon_2d: list[list[number]]
+    polygon_2d: list[conlist(number, min_length=2, max_length=2)]
     z_min: number
     z_max: number
 
+    @field_validator("polygon_2d")
+    @classmethod
+    def is_clockwise(cls, v: list[list[number]]) -> list | None:
+        v_float = []
+        if len(v) < 3:  # noqa
+            err_msg = "polygon requires 3 or more elements"
+            raise ValueError(err_msg)
+        check_clock_wise: float = 0.0
+        # convert int value to float
+        for p_2d in v:
+            v_float.append([float(p_2d[0]), float(p_2d[1])])
+        for i, _ in enumerate(v_float):
+            p1 = v_float[i]
+            p2 = v_float[(i + 1) % len(v_float)]
+            check_clock_wise += (p2[0] - p1[0]) * (p2[1] + p2[1])
+        if check_clock_wise <= 0.0:  # noqa
+            err_msg = "polygon_2d is not clockwise"
+            raise ValueError(err_msg)
+        return v_float
+
+    @field_validator("z_min", "z_max")
+    @classmethod
+    def to_float(cls, v: number) -> float:
+        return float(v)
+
 
 class BoundingBoxCondition(BaseModel):
     Start: number | None = None
@@ -249,6 +284,96 @@ def get_sensing_frame_config(
     return True, sensing_frame_config
 
 
+def get_proposed_area_list_point_stamped(
+    proposed_area: ProposedAreaCondition,
+    header: Header,
+) -> list[PointStamped]:
+    points: list[PointStamped] = []
+    for point in proposed_area.polygon_2d:
+        points.append(PointStamped(header=header, point=Point(x=point[0], y=point[1], z=0.0)))
+    return points
+
+
+def list_point_stamped_to_line_strip(
+    list_point_stamped: list[PointStamped],
+    marker_id: int,
+) -> Marker:
+    line_strip = Marker(
+        header=list_point_stamped[0].header,
+        type=Marker.LINE_STRIP,
+        action=Marker.ADD,
+        ns="intersection",
+        id=marker_id,
+        color=ColorRGBA(r=1.0, g=0.0, b=0.0, a=0.3, x=0.2, y=0.2, z=0.2),
+    )
+    for point_stamped in list_point_stamped:
+        line_strip.points.append(point_stamped.point)
+    return line_strip
+
+
+def transform_proposed_area(
+    proposed_area: ProposedAreaCondition,
+    header: Header,
+    tf: TransformStamped,
+) -> tuple[Polygon, float]:
+    proposed_area_list: list = []
+    sum_z = 0.0
+    count_point = 0
+
+    list_point_stamped = get_proposed_area_list_point_stamped(proposed_area, header)
+    for point_stamped in list_point_stamped:
+        count_point += 1
+        point_stamped_in_map = do_transform_point(point_stamped, tf)
+        proposed_area_list.append([point_stamped_in_map.point.x, point_stamped_in_map.point.y])
+        sum_z += point_stamped_in_map.point.z
+    average_z = sum_z / count_point
+    return Polygon(proposed_area_list), average_z
+
+
+def get_non_detection_area_in_base_link(
+    intersection_polygon: Polygon,
+    header: Header,
+    z_min: float,
+    z_max: float,
+    average_z: float,
+    base_link_to_map: TransformStamped,
+    marker_id: int,
+) -> tuple[Marker, list]:
+    line_strip = Marker(
+        header=header,
+        type=Marker.LINE_STRIP,
+        action=Marker.ADD,
+        ns="intersection",
+        id=marker_id,
+        color=ColorRGBA(r=1.0, g=0.0, b=0.0, a=0.3),
+        scale=Vector3(x=0.2, y=0.2, z=0.2),
+        lifetime=Duration(nanosec=200_000_000),
+    )
+    list_intersection_area = []
+    list_p_stamped_base_link: list[PointStamped] = []
+    for shapely_point in intersection_polygon.exterior.coords:
+        p_stamped_map = PointStamped(
+            header=header,
+            point=Point(x=shapely_point[0], y=shapely_point[1], z=average_z),
+        )
+        list_p_stamped_base_link.append(do_transform_point(p_stamped_map, base_link_to_map))
+    # create floor polygon
+    for p_base_link in list_p_stamped_base_link:
+        p_base_link.point.z = z_min
+        line_strip.points.append(deepcopy(p_base_link.point))
+        list_intersection_area.append(
+            [p_base_link.point.x, p_base_link.point.y, p_base_link.point.z],
+        )
+    # create roof polygon
+    for p_base_link in list_p_stamped_base_link:
+        p_base_link.point.z = z_max
+        line_strip.points.append(p_base_link.point)
+        list_intersection_area.append(
+            [p_base_link.point.x, p_base_link.point.y, p_base_link.point.z],
+        )
+    return line_strip, list_intersection_area
+
+
 @dataclass
 class Detection(EvaluationItem):
     name: str = "Detection"