Skip to content

Commit

Permalink
feat: add lanelet2_routing
Browse files Browse the repository at this point in the history
  • Loading branch information
wu-vincent committed Nov 20, 2023
1 parent 7e86eb6 commit 24082b2
Show file tree
Hide file tree
Showing 8 changed files with 81 additions and 51 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion lanelet2_io/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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 ##
Expand Down
7 changes: 3 additions & 4 deletions lanelet2_io/test/TestSetup.h
Original file line number Diff line number Diff line change
Expand Up @@ -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");
}
}
Expand Down
87 changes: 55 additions & 32 deletions lanelet2_routing/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,69 +1,92 @@
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)

###################
## 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 ()
3 changes: 2 additions & 1 deletion lanelet2_routing/include/lanelet2_routing/internal/Graph.h
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
18 changes: 12 additions & 6 deletions lanelet2_routing/src/RouteBuilder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down
2 changes: 1 addition & 1 deletion lanelet2_routing/src/RoutingGraph.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
11 changes: 6 additions & 5 deletions lanelet2_routing/test/test_routing_visualization.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

0 comments on commit 24082b2

Please sign in to comment.