diff --git a/CMakeLists.txt b/CMakeLists.txt
index 33632fc5..a463e684 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,7 +19,7 @@ add_subdirectory(lanelet2_core)
 add_subdirectory(lanelet2_io)
 add_subdirectory(lanelet2_projection)
 add_subdirectory(lanelet2_traffic_rules)
-#add_subdirectory(lanelet2_routing)
+add_subdirectory(lanelet2_routing)
 #add_subdirectory(lanelet2_validation)
 #add_subdirectory(lanelet2_examples)
 #add_subdirectory(lanelet2_python)
diff --git a/lanelet2_io/CMakeLists.txt b/lanelet2_io/CMakeLists.txt
index 02650a52..2e6d0440 100644
--- a/lanelet2_io/CMakeLists.txt
+++ b/lanelet2_io/CMakeLists.txt
@@ -56,7 +56,7 @@ find_package(Boost REQUIRED)
 find_package(pugixml REQUIRED)
 add_library(${PROJECT_NAME} SHARED ${PROJECT_SOURCE_FILES_SRC})
 target_include_directories(${PROJECT_NAME} PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
-target_link_libraries(${PROJECT_NAME} PUBLIC lanelet2_core Boost::boost Boost::filesystem Boost::serialization pugixml::pugixml)
+target_link_libraries(${PROJECT_NAME} PUBLIC lanelet2_core Boost::filesystem Boost::serialization pugixml::pugixml)
 
 #############
 ## Install ##
diff --git a/lanelet2_io/test/TestSetup.h b/lanelet2_io/test/TestSetup.h
index 1f70c50f..85da790b 100644
--- a/lanelet2_io/test/TestSetup.h
+++ b/lanelet2_io/test/TestSetup.h
@@ -124,10 +124,9 @@ inline Area setUpArea(int& num, const std::string& type = AttributeValueString::
 class Tempfile {
  public:
   explicit Tempfile(std::string name) {
-    boost::filesystem::path dir = boost::filesystem::temp_directory_path() / "lanelet2_io_test.XXXXXX";
-    if (!boost::filesystem::exists(dir)) {
-      bool created = boost::filesystem::create_directory(dir);
-      if (!created) {
+    auto dir = fs::temp_directory_path() / "lanelet2_io_test.XXXXXX";
+    if (!exists(dir)) {
+      if (!create_directory(dir)) {
         throw lanelet::LaneletError("Failed to crate temporary directory");
       }
     }
diff --git a/lanelet2_routing/CMakeLists.txt b/lanelet2_routing/CMakeLists.txt
index a1f08032..2351111b 100644
--- a/lanelet2_routing/CMakeLists.txt
+++ b/lanelet2_routing/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(MRT_PKG_VERSION 4.0.0)
+#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_routing)
@@ -6,64 +6,87 @@ project(lanelet2_routing)
 ###################
 ## Find packages ##
 ###################
-find_package(mrt_cmake_modules REQUIRED)
-include(UseMrtStdCompilerFlags)
-include(GatherDeps)
+#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()
+#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()
+#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")
+#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")
+file(GLOB PROJECT_SOURCE_FILES_SRC "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp")
 
 ###########
 ## Build ##
 ###########
 # Declare a cpp library
-mrt_add_library(${PROJECT_NAME}
-    INCLUDES ${PROJECT_HEADER_FILES_INC} ${PROJECT_SOURCE_FILES_INC}
-    SOURCES ${PROJECT_SOURCE_FILES_SRC}
-    )
+#mrt_add_library(${PROJECT_NAME}
+#    INCLUDES ${PROJECT_HEADER_FILES_INC} ${PROJECT_SOURCE_FILES_INC}
+#    SOURCES ${PROJECT_SOURCE_FILES_SRC}
+#    )
+find_package(Boost REQUIRED)
+add_library(${PROJECT_NAME} SHARED ${PROJECT_SOURCE_FILES_SRC})
+target_include_directories(${PROJECT_NAME} PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
+target_link_libraries(${PROJECT_NAME} PUBLIC lanelet2_core lanelet2_traffic_rules Boost::filesystem)
 
 #############
 ## 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})
+#mrt_install(PROGRAMS scripts FILES res data ${PROJECT_INSTALL_FILES})
+install(TARGETS ${PROJECT_NAME}
+        ARCHIVE DESTINATION ${PROJECT_INSTALL_FILES}
+        LIBRARY DESTINATION ${PROJECT_INSTALL_FILES}
+        RUNTIME DESTINATION ${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()
+#if (CATKIN_ENABLE_TESTING)
+#    mrt_add_tests(test)
+#    mrt_add_nosetests(test)
+#endif()
+if (LANELET2_ENABLE_TESTING)
+    include(GoogleTest)
+    find_package(GTest REQUIRED)
+    file(GLOB PROJECT_SOURCE_FILES_TEST "${CMAKE_CURRENT_SOURCE_DIR}/test/*.cpp")
+    add_executable("${PROJECT_NAME}_test" ${PROJECT_SOURCE_FILES_TEST})
+    target_link_libraries("${PROJECT_NAME}_test" PRIVATE GTest::gtest_main ${PROJECT_NAME})
+
+    add_custom_command(TARGET "${PROJECT_NAME}_test" POST_BUILD
+            COMMAND ${CMAKE_COMMAND} -E copy_if_different "$<TARGET_FILE:lanelet2_core>" "$<TARGET_FILE_DIR:${PROJECT_NAME}_test>"
+            COMMAND ${CMAKE_COMMAND} -E copy_if_different "$<TARGET_FILE:lanelet2_traffic_rules>" "$<TARGET_FILE_DIR:${PROJECT_NAME}_test>")
+
+    gtest_discover_tests("${PROJECT_NAME}_test")
+    add_dependencies("${PROJECT_NAME}_test" lanelet2_maps)
+endif ()
\ No newline at end of file
diff --git a/lanelet2_routing/include/lanelet2_routing/internal/Graph.h b/lanelet2_routing/include/lanelet2_routing/internal/Graph.h
index 3bdc80a3..97460670 100644
--- a/lanelet2_routing/include/lanelet2_routing/internal/Graph.h
+++ b/lanelet2_routing/include/lanelet2_routing/internal/Graph.h
@@ -99,7 +99,8 @@ struct EdgeCostFilter {
       pmIds_;  ///< Property map to the routing cost IDs of the edges
 };
 
-using LaneletOrAreaToVertex = std::unordered_map<ConstLaneletOrArea, std::uint32_t>;
+// using LaneletOrAreaToVertex = std::unordered_map<ConstLaneletOrArea, std::uint32_t>;
+using LaneletOrAreaToVertex = std::unordered_map<ConstLaneletOrArea, size_t>;
 using FilteredGraphDesc = std::pair<size_t, RelationType>;
 
 /// @brief Manages the actual routing graph and provieds different views on the edges (lazily computed)
diff --git a/lanelet2_routing/src/RouteBuilder.cpp b/lanelet2_routing/src/RouteBuilder.cpp
index 9eee0943..75700473 100644
--- a/lanelet2_routing/src/RouteBuilder.cpp
+++ b/lanelet2_routing/src/RouteBuilder.cpp
@@ -234,12 +234,18 @@ class PathsOutOfRouteFinder {
     auto isAdjacentToRoute = [&](LaneletVertexId v) {
       auto inEdges = boost::in_edges(v, g);
       auto outEdges = boost::out_edges(v, g);
-      constexpr auto Adjacent =
-          RelationType::Left | RelationType::Right | RelationType::AdjacentLeft | RelationType::AdjacentRight;
-      return std::any_of(inEdges.first, inEdges.second,
-                         [&](auto e) { return hasRelation<Adjacent>(g, e) && has(*llts_, boost::source(e, g)); }) ||
-             std::any_of(outEdges.first, outEdges.second,
-                         [&](auto e) { return hasRelation<Adjacent>(g, e) && has(*llts_, boost::target(e, g)); });
+      return std::any_of(inEdges.first, inEdges.second,  //
+                         [&](auto e) {
+                           constexpr auto Adjacent = RelationType::Left | RelationType::Right |
+                                                     RelationType::AdjacentLeft | RelationType::AdjacentRight;
+                           return hasRelation<Adjacent>(g, e) && has(*llts_, boost::source(e, g));
+                         }) ||
+             std::any_of(outEdges.first, outEdges.second,  //
+                         [&](auto e) {
+                           constexpr auto Adjacent = RelationType::Left | RelationType::Right |
+                                                     RelationType::AdjacentLeft | RelationType::AdjacentRight;
+                           return hasRelation<Adjacent>(g, e) && has(*llts_, boost::target(e, g));
+                         });
     };
     return std::all_of(path.begin(), path.end(), isAdjacentToRoute);
   }
diff --git a/lanelet2_routing/src/RoutingGraph.cpp b/lanelet2_routing/src/RoutingGraph.cpp
index 057db0bf..8f586f32 100644
--- a/lanelet2_routing/src/RoutingGraph.cpp
+++ b/lanelet2_routing/src/RoutingGraph.cpp
@@ -334,7 +334,7 @@ Optional<PathT> shortestPathImpl(const PrimT& from, const PrimT& to, RoutingCost
     return {};
   }
   auto filteredGraph =
-      withLaneChanges
+      withLaneChanges  //
           ? withAreas ? graph.withAreasAndLaneChanges(routingCostId) : graph.withLaneChanges(routingCostId)
           : withAreas ? graph.withAreasWithoutLaneChanges(routingCostId) : graph.withoutLaneChanges(routingCostId);
   DijkstraStyleSearch<FilteredRoutingGraph> search(filteredGraph);
diff --git a/lanelet2_routing/test/test_routing_visualization.cpp b/lanelet2_routing/test/test_routing_visualization.cpp
index 91609ecd..b918f207 100644
--- a/lanelet2_routing/test/test_routing_visualization.cpp
+++ b/lanelet2_routing/test/test_routing_visualization.cpp
@@ -14,12 +14,13 @@ namespace fs = boost::filesystem;
 class Tempfile {
  public:
   Tempfile() {
-    char path[] = {"/tmp/lanelet2_unittest.XXXXXX"};
-    auto* res = mkdtemp(path);
-    if (res == nullptr) {
-      throw lanelet::LaneletError("Failed to crate temporary directory");
+    auto dir = fs::temp_directory_path() / "lanelet2_unittest.XXXXXX";
+    if (!exists(dir)) {
+      if (!create_directory(dir)) {
+        throw lanelet::LaneletError("Failed to crate temporary directory");
+      }
     }
-    path_ = path;
+    path_ = dir.string();
   }
   Tempfile(const Tempfile&) = delete;
   Tempfile(Tempfile&&) = delete;