New module: lanelet2_ml_converter (#329)

fzi-forschungszentrum-informatik · Feb 1, 2024 · 4d20128 · 4d20128
1 parent 4311fa7
commit 4d20128
Show file tree

Hide file tree

Showing 36 changed files with 4,323 additions and 44 deletions.
diff --git a/.github/lint.bash b/.github/lint.bash
@@ -25,7 +25,7 @@ export AMENT_PREFIX_PATH=${LANELET2_ROOT}
 CMAKE_ROOT=${LANELET2_ROOT}/CMakeLists.txt
 echo "cmake_minimum_required(VERSION 3.12)" > ${CMAKE_ROOT}
 echo "project(lanelet2)" >> ${CMAKE_ROOT}
-export LANELET2_PACKAGES_TOPOLOGICAL="lanelet2_core lanelet2_io lanelet2_projection lanelet2_traffic_rules lanelet2_routing lanelet2_maps lanelet2_validation lanelet2_matching lanelet2_python lanelet2_examples"
+export LANELET2_PACKAGES_TOPOLOGICAL="lanelet2_core lanelet2_io lanelet2_projection lanelet2_traffic_rules lanelet2_routing lanelet2_maps lanelet2_validation lanelet2_matching lanelet2_ml_converter lanelet2_python lanelet2_examples"
 for pkg in $LANELET2_PACKAGES_TOPOLOGICAL; do
   echo "add_subdirectory($pkg)" >> ${CMAKE_ROOT};
   echo "set(${pkg}_LIBRARIES $pkg)" >> ${CMAKE_ROOT};

diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -0,0 +1,9 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/opt/mrtros/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/mrtros/lib/python3/dist-packages"
+    ],
+    "cmake.sourceDirectory": "/home/immel/workspaces/av2_link_pred_ws/src"
+}
diff --git a/Dockerfile b/Dockerfile
@@ -16,27 +16,27 @@ SHELL ["/bin/bash", "-c"]
 # basics
 RUN set -ex; \
     if [ "${ROS_DISTRO}" = "melodic" ] || [ "${ROS_DISTRO}" = "kinetic" ]; \
-        then export PY_VERSION=python; \
-        else export PY_VERSION=python3; \
+    then export PY_VERSION=python; \
+    else export PY_VERSION=python3; \
     fi; \
     if [ "$DEV" -ne "0" ]; then \
-        export DEV_PACKAGES="clang-format-11 clang-tidy-11 clang-11 i${PY_VERSION} nano lcov"; \
+    export DEV_PACKAGES="clang-format-11 clang-tidy-11 clang-11 i${PY_VERSION} nano lcov"; \
     fi; \
     apt-get update && \
     DEBIAN_FRONTEND=noninteractive apt-get install -y \
-        bash-completion \
-        build-essential \
-        curl \
-        git \
-        cmake \
-        keyboard-configuration \
-        locales \
-        lsb-core \
-        lib${PY_VERSION}-dev \
-        software-properties-common \
-        sudo \
-        wget \
-        ${DEV_PACKAGES} && \
+    bash-completion \
+    build-essential \
+    curl \
+    git \
+    cmake \
+    keyboard-configuration \
+    locales \
+    lsb-core \
+    lib${PY_VERSION}-dev \
+    software-properties-common \
+    sudo \
+    wget \
+    ${DEV_PACKAGES} && \
     locale-gen en_US.UTF-8 && \
     apt-get clean && \
     rm -rf /var/lib/apt/lists/*
@@ -57,30 +57,34 @@ RUN set -ex; \
 
 # dependencies for lanelet2
 RUN if [ "${ROS_DISTRO}" = "melodic" ] || [ "${ROS_DISTRO}" = "kinetic" ]; \
-        then export PY_VERSION=python; \
-        else export PY_VERSION=python3; \
+    then export PY_VERSION=python; \
+    else export PY_VERSION=python3; \
     fi; \
     apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
-        libgtest-dev \
-        libboost-all-dev \
-        libeigen3-dev \
-        libgeographic-dev \
-        libpugixml-dev \
-        libboost-python-dev \
-        ${PY_VERSION}-rospkg \
-        ros-$ROS_DISTRO-ros-environment && \
+    libgtest-dev \
+    libboost-all-dev \
+    libeigen3-dev \
+    libgeographic-dev \
+    libpugixml-dev \
+    libboost-python-dev \
+    pip \
+    ${PY_VERSION}-rospkg \
+    ros-$ROS_DISTRO-ros-environment && \
     apt-get clean && \
     rm -rf /var/lib/apt/lists/*
 
+# install python dependencies (numpy)
+RUN pip install numpy
+
 # ros version specific dependencies
 RUN set -ex; \
     if [ "${ROS_DISTRO}" = "melodic" ] || [ "${ROS_DISTRO}" = "kinetic" ]; \
-        then export PY_VERSION=python; \
-        else export PY_VERSION=python3; \
+    then export PY_VERSION=python; \
+    else export PY_VERSION=python3; \
     fi; \
     if [ "$ROS" = "ros" ]; \
-        then export ROS_DEPS="ros-$ROS_DISTRO-catkin ros-$ROS_DISTRO-rosbash ${PY_VERSION}-catkin-tools"; \
-        else export ROS_DEPS="ros-$ROS_DISTRO-ament-cmake python3-colcon-ros ros-$ROS_DISTRO-ros2cli"; \
+    then export ROS_DEPS="ros-$ROS_DISTRO-catkin ros-$ROS_DISTRO-rosbash ${PY_VERSION}-catkin-tools"; \
+    else export ROS_DEPS="ros-$ROS_DISTRO-ament-cmake python3-colcon-ros ros-$ROS_DISTRO-ros2cli"; \
     fi; \
     apt-get update && \
     DEBIAN_FRONTEND=noninteractive apt-get install -y $ROS_DEPS && \
@@ -111,8 +115,8 @@ RUN set -ex; \
     cd /home/developer/workspace && \
     mkdir -p /home/developer/workspace/src && \
     if [ "$ROS" = "ros" ]; then \
-      source /home/developer/.bashrc && \
-      catkin init; \
+    source /home/developer/.bashrc && \
+    catkin init; \
     fi; \
     git clone https://github.com/KIT-MRT/mrt_cmake_modules.git /home/developer/workspace/src/mrt_cmake_modules
 
@@ -131,21 +135,21 @@ FROM lanelet2_src AS lanelet2
 # build
 RUN set -ex; \
     if [ "$DEV" -ne "0" ]; then \
-      export CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Debug -DMRT_SANITIZER=checks -DMRT_ENABLE_COVERAGE=1"; \
+    export CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Debug -DMRT_SANITIZER=checks -DMRT_ENABLE_COVERAGE=1"; \
     else \
-      export CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Release"; \
+    export CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Release"; \
     fi; \
     if [ "$ROS" = "ros" ]; then \
-      export CONFIGURE="catkin config --cmake-args $CMAKE_ARGS"; \
-      export BUILD_CMD="catkin build --no-status"; \
+    export CONFIGURE="catkin config --cmake-args $CMAKE_ARGS"; \
+    export BUILD_CMD="catkin build --no-status"; \
     else \
-      export CONFIGURE=true; \
-      export BUILD_CMD="colcon build --symlink-install --cmake-args $CMAKE_ARGS"; \
+    export CONFIGURE=true; \
+    export BUILD_CMD="colcon build --symlink-install --cmake-args $CMAKE_ARGS"; \
     fi; \
     source /opt/ros/$ROS_DISTRO/setup.bash && \
     env && \
     $CONFIGURE && \
     $BUILD_CMD && \
     if [ "$DEV" -eq "0" ]; then \
-      rm -rf build logs; \
+    rm -rf build logs; \
     fi
diff --git a/README.md b/README.md
@@ -13,6 +13,12 @@ https://build.ros2.org/job/Hdev__lanelet2__ubuntu_jammy_amd64/lastBuild/)
 [![PyPI Downloads](https://img.shields.io/pypi/dm/lanelet2.svg?label=PyPI%20downloads)](
 https://pypi.org/project/lanelet2/)
 
+## New module :rocket: : `lanelet2_ml_converter`
+
+Convert Lanelet2 maps directly to instance labels for map perception and other map-based learning tasks! `lanelet2_ml_converter` provides local instance labels of various abstraction layers and representations accessible as numpy arrays from python. Check out the `lanelet2_ml_converter` module for more information and usage examples.
+
+**Note:** As the module is not released officially yet, to try it out you need to either build Lanelet2 with the python bindings yourself or manually install the pip wheel files from the CI artefacts (download [here](https://github.com/immel-f/Lanelet2/actions/runs/7714926040/artifacts/1206339616)).
+
 ## Overview
 
 Lanelet2 is a C++ library for handling map data in the context of automated driving. It is designed to utilize high-definition map data in order to efficiently handle the challenges posed to a vehicle in complex traffic scenarios. Flexibility and extensibility are some of the core principles to handle the upcoming challenges of future maps.
@@ -30,6 +36,7 @@ Features:
 - **Boost Geometry** support for all thinkable kinds of geometry calculations on map primitives
 - Released under the [**BSD 3-Clause license**](LICENSE)
 - Support for **ROS1, ROS2, Docker and Conan** (see instructions below)
+- Converter module to convert Lanelet2 maps into **local instance labels for machine learning tasks**.
 
 ![](lanelet2_core/doc/images/lanelet2_example_image.png)
 
@@ -181,6 +188,7 @@ Examples and common use cases in both C++ and Python can be found [here](lanelet
 * **lanelet2_routing** implements the routing graph for routing or reachable set or queries as well as collision checking
 * **lanelet2_maps** provides example maps and functionality to visualize and modify them easily in JOSM
 * **lanelet2_matching** provides functions to determine in which lanelet an object is/could be currently located
+* **lanelet2_ml_converter** converts Lanelet2 maps into local instance labels for machine learning tasks
 * **lanelet2_python** implements the python interface for lanelet2
 * **lanelet2_validation** provides checks to ensure a valid lanelet2 map
 * **lanelet2_examples** contains tutorials for working with Lanelet2 in C++ and Python

diff --git a/conanfile.py b/conanfile.py
@@ -27,7 +27,7 @@
 
 # declare dependencies
 include_directories(lanelet2_core/include lanelet2_io/include lanelet2_projection/include lanelet2_traffic_rules/include
-    lanelet2_routing/include lanelet2_validation/include)
+    lanelet2_routing/include lanelet2_validation/include lanelet2_ml_converter/include)
 add_subdirectory(lanelet2_core)
 add_subdirectory(lanelet2_io)
 add_subdirectory(lanelet2_projection)
@@ -38,15 +38,17 @@
 add_subdirectory(lanelet2_python)
 add_subdirectory(lanelet2_maps)
 add_subdirectory(lanelet2_matching)
+add_subdirectory(lanelet2_ml_converter)
 # declare dependencies
 target_link_libraries(lanelet2_io PUBLIC lanelet2_core)
 target_link_libraries(lanelet2_projection PUBLIC lanelet2_core)
 target_link_libraries(lanelet2_traffic_rules PUBLIC lanelet2_core)
 target_link_libraries(lanelet2_routing PUBLIC lanelet2_core lanelet2_traffic_rules)
 target_link_libraries(lanelet2_matching PUBLIC lanelet2_core lanelet2_traffic_rules lanelet2_io lanelet2_projection lanelet2_maps)
 target_link_libraries(lanelet2_validation PUBLIC lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection)
-target_link_libraries(lanelet2_examples_compiler_flags INTERFACE lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection lanelet2_matching)
-target_link_libraries(lanelet2_python_compiler_flags INTERFACE lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection lanelet2_matching)
+target_link_libraries(lanelet2_ml_converter PUBLIC lanelet2_core lanelet2_routing lanelet2_traffic_rules)
+target_link_libraries(lanelet2_examples_compiler_flags INTERFACE lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection lanelet2_matching lanelet2_ml_converter)
+target_link_libraries(lanelet2_python_compiler_flags INTERFACE lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection lanelet2_matching lanelet2_ml_converter)
 """
 
 def read_version():
@@ -88,7 +90,8 @@ class Lanelet2Conan(ConanFile):
         'lanelet2_projection',
         'lanelet2_traffic_rules',
         'lanelet2_routing',
-        'lanelet2_validation'
+        'lanelet2_validation',
+        'lanelet2_ml_converter'
     ]
 
     def _configure_cmake(self):

diff --git a/lanelet2_core/src/Lanelet.cpp b/lanelet2_core/src/Lanelet.cpp
@@ -210,6 +210,7 @@ std::shared_ptr<ConstLineString3d> calculateCenterline(const ConstLineString2d&
     // Determine right candidate
     std::tie(rightCandidate, rightCandidateDistance) =
         findClosestNonintersectingPoint(std::next(rightCurrent), leftCurrent, bounds, centerlinePoints.back(), false);
+
     // Choose the better one
     if (leftCandidateDistance && (!rightCandidateDistance || leftCandidateDistance <= rightCandidateDistance)) {
       assert(leftCandidate != leftBound.end());

diff --git a/lanelet2_ml_converter/.vscode/c_cpp_properties.json b/lanelet2_ml_converter/.vscode/c_cpp_properties.json
@@ -0,0 +1,36 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/mrtros/include/**",
+        "/opt/mrtsoftware/local/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/devel/include/**",
+        "/opt/mrtsoftware/release/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_core/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_examples/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_io/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_ml_converter/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_maps/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/lanelet2_maps_mrt/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_matching/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_projection/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_python/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_routing/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_traffic_rules/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_validation/include/**",
+        "/home/immel/workspaces/av2_link_pred_ws/src/ll2_map_graph_converter/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}
diff --git a/lanelet2_ml_converter/.vscode/settings.json b/lanelet2_ml_converter/.vscode/settings.json
@@ -0,0 +1,8 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/opt/mrtros/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/mrtros/lib/python3/dist-packages"
+    ]
+}
diff --git a/lanelet2_ml_converter/CHANGELOG.rst b/lanelet2_ml_converter/CHANGELOG.rst
@@ -0,0 +1,3 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package lanelet2_ml_converter
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
diff --git a/lanelet2_ml_converter/CMakeLists.txt b/lanelet2_ml_converter/CMakeLists.txt
@@ -0,0 +1,69 @@
+set(MRT_PKG_VERSION 4.0.0)
+# Modify only if you know what you are doing!
+cmake_minimum_required(VERSION 3.5.1)
+project(lanelet2_ml_converter)
+
+###################
+## Find packages ##
+###################
+find_package(mrt_cmake_modules REQUIRED)
+include(UseMrtStdCompilerFlags)
+include(GatherDeps)
+
+# You can add a custom.cmake in order to add special handling for this package. E.g. you can do:
+# list(REMOVE_ITEM DEPENDEND_PACKAGES <package name 1> <package name 2> ...)
+# To remove libs which cannot be found automatically. You can also "find_package" other, custom dependencies there.
+# You can also set PROJECT_INSTALL_FILES to install files that are not installed by default.
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake")
+    include("${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake")
+endif()
+
+find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES})
+
+mrt_parse_package_xml()
+
+########################
+## Add python modules ##
+########################
+# This adds a python module if located under src/{PROJECT_NAME)
+mrt_python_module_setup()
+
+mrt_glob_files(PROJECT_PYTHON_SOURCE_FILES_SRC "python_api/*.cpp")
+if (PROJECT_PYTHON_SOURCE_FILES_SRC)
+    # Add a cpp-python api library. Make sure there are no name collisions with python modules in this project
+    mrt_add_python_api( ${PROJECT_NAME}
+        FILES ${PROJECT_PYTHON_SOURCE_FILES_SRC}
+        )
+endif()
+
+############################
+## Read source code files ##
+############################
+mrt_glob_files_recurse(PROJECT_HEADER_FILES_INC "include/*.h" "include/*.hpp" "include/*.cuh")
+mrt_glob_files(PROJECT_SOURCE_FILES_INC "src/*.h" "src/*.hpp" "src/*.cuh")
+mrt_glob_files(PROJECT_SOURCE_FILES_SRC "src/*.cpp" "src/*.cu")
+
+###########
+## Build ##
+###########
+# Declare a cpp library
+mrt_add_library(${PROJECT_NAME}
+    INCLUDES ${PROJECT_HEADER_FILES_INC} ${PROJECT_SOURCE_FILES_INC}
+    SOURCES ${PROJECT_SOURCE_FILES_SRC}
+    )
+
+#############
+## Install ##
+#############
+# Install all targets, headers by default and scripts and other files if specified (folders or files).
+# This command also exports libraries and config files for dependent packages and this supersedes catkin_package.
+mrt_install(PROGRAMS scripts FILES res data ${PROJECT_INSTALL_FILES})
+
+#############
+## Testing ##
+#############
+# Add test targets for cpp and python tests
+if (CATKIN_ENABLE_TESTING)
+    mrt_add_tests(test)
+    mrt_add_nosetests(test)
+endif()