diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index b6e8e8a9ff8..af12722401e 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -214,6 +214,9 @@ auto distanceToLeftLaneBound( const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, const std::shared_ptr & hdmap_utils_ptr) -> double { + if (lanelet_ids.empty()) { + THROW_SEMANTIC_ERROR("Failing to calculate distanceToLeftLaneBound given an empty vector."); + } std::vector distances; std::transform( lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), [&](auto lanelet_id) { @@ -245,10 +248,13 @@ auto distanceToRightLaneBound( const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, const std::shared_ptr & hdmap_utils_ptr) -> double { + if (lanelet_ids.empty()) { + THROW_SEMANTIC_ERROR("Failing to calculate distanceToRightLaneBound for given empty vector."); + } std::vector distances; std::transform( lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), [&](auto lanelet_id) { - return distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr); + return distanceToRightLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr); }); return *std::min_element(distances.begin(), distances.end()); } diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index 2cd692fafff..08f9f50199c 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -1,9 +1,9 @@ -add_subdirectory(src/traffic_lights) -add_subdirectory(src/helper) -add_subdirectory(src/entity) add_subdirectory(src/behavior) +add_subdirectory(src/data_type) +add_subdirectory(src/entity) +add_subdirectory(src/hdmap_utils) +add_subdirectory(src/helper) add_subdirectory(src/job) add_subdirectory(src/simulation_clock) -add_subdirectory(src/hdmap_utils) +add_subdirectory(src/traffic_lights) add_subdirectory(src/utils) -add_subdirectory(src/data_type) diff --git a/simulation/traffic_simulator/test/map/intersection/lanelet2_map.osm b/simulation/traffic_simulator/test/map/intersection/lanelet2_map.osm new file mode 100644 index 00000000000..3a239c64159 --- /dev/null +++ b/simulation/traffic_simulator/test/map/intersection/lanelet2_map.osm @@ -0,0 +1,15432 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulation/traffic_simulator/test/map/lanelet2_map.osm b/simulation/traffic_simulator/test/map/standard_map/lanelet2_map.osm similarity index 100% rename from simulation/traffic_simulator/test/map/lanelet2_map.osm rename to simulation/traffic_simulator/test/map/standard_map/lanelet2_map.osm diff --git a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp index 93512bda08d..26b5efaa132 100644 --- a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp +++ b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp @@ -34,12 +34,13 @@ class HdMapUtilsTest_StandardMap : public testing::Test { protected: HdMapUtilsTest_StandardMap() - : hdmap_utils(hdmap_utils::HdMapUtils( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + : hdmap_utils( + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", geographic_msgs::build() .latitude(35.61836750154) .longitude(139.78066608243) - .altitude(0.0))) + .altitude(0.0)) { } @@ -49,13 +50,13 @@ class HdMapUtilsTest_WithRoadShoulderMap : public testing::Test { protected: HdMapUtilsTest_WithRoadShoulderMap() - : hdmap_utils(hdmap_utils::HdMapUtils( + : hdmap_utils( ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/with_road_shoulder/lanelet2_map.osm", geographic_msgs::build() .latitude(35.61836750154) .longitude(139.78066608243) - .altitude(0.0))) + .altitude(0.0)) { } @@ -65,13 +66,13 @@ class HdMapUtilsTest_EmptyMap : public testing::Test { protected: HdMapUtilsTest_EmptyMap() - : hdmap_utils(hdmap_utils::HdMapUtils( + : hdmap_utils( ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/empty/lanelet2_map.osm", geographic_msgs::build() .latitude(0.0) .longitude(0.0) - .altitude(0.0))) + .altitude(0.0)) { } @@ -81,13 +82,13 @@ class HdMapUtilsTest_FourTrackHighwayMap : public testing::Test { protected: HdMapUtilsTest_FourTrackHighwayMap() - : hdmap_utils(hdmap_utils::HdMapUtils( + : hdmap_utils( ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/four_track_highway/lanelet2_map.osm", geographic_msgs::build() .latitude(35.22312494055522) .longitude(138.8024583466017) - .altitude(0.0))) + .altitude(0.0)) { } @@ -97,13 +98,13 @@ class HdMapUtilsTest_CrossroadsWithStoplinesMap : public testing::Test { protected: HdMapUtilsTest_CrossroadsWithStoplinesMap() - : hdmap_utils(hdmap_utils::HdMapUtils( + : hdmap_utils( ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/crossroads_with_stoplines/lanelet2_map.osm", geographic_msgs::build() .latitude(35.23808753540768) .longitude(139.9009591876285) - .altitude(0.0))) + .altitude(0.0)) { } @@ -113,17 +114,18 @@ class HdMapUtilsTest_KashiwanohaMap : public testing::Test { protected: HdMapUtilsTest_KashiwanohaMap() - : hdmap_utils(hdmap_utils::HdMapUtils( + : hdmap_utils( ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map/lanelet2_map.osm", geographic_msgs::build() .latitude(0.0) .longitude(0.0) - .altitude(0.0))) + .altitude(0.0)) { } hdmap_utils::HdMapUtils hdmap_utils; }; + /** * @note Test basic functionality. * Test initialization correctness with a correct path to a lanelet map. @@ -132,7 +134,8 @@ TEST(HdMapUtils, Construct) { ASSERT_NO_THROW( auto hdmap_utils = hdmap_utils::HdMapUtils( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", geographic_msgs::build() .latitude(35.61836750154) .longitude(139.78066608243) diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index ba5f24b86c6..a12b5e33328 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -26,6 +26,9 @@ #include "catalogs.hpp" #include "expect_eq_macros.hpp" +constexpr auto convertDegToRad(const double deg) -> double { return deg / 180.0 * M_PI; } +constexpr auto convertRadToDeg(const double rad) -> double { return rad * 180.0 / M_PI; } + auto makePoint(const double x, const double y, const double z = 0.0) -> geometry_msgs::msg::Point { return geometry_msgs::build().x(x).y(y).z(z); @@ -38,6 +41,15 @@ auto makeBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg .dimensions(geometry_msgs::build().x(4.0).y(2.0).z(1.5)); } +auto makeCustom2DBoundingBox( + const double x, const double y, const double x_offset = 0.0, const double y_offset = 0.0) + -> traffic_simulator_msgs::msg::BoundingBox +{ + return traffic_simulator_msgs::build() + .center(geometry_msgs::build().x(x_offset).y(y_offset).z(0.0)) + .dimensions(geometry_msgs::build().x(x).y(y).z(0.0)); +} + auto makeSmallBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox { return traffic_simulator_msgs::build() @@ -51,6 +63,18 @@ auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion geometry_msgs::build().x(0.0).y(0.0).z(yaw)); } +auto makePose(const double x, const double y, const double yaw_deg) -> geometry_msgs::msg::Pose +{ + /** + * @note +x axis is 0 degrees; +y axis is 90 degrees + */ + return geometry_msgs::build() + .position(geometry_msgs::build().x(x).y(y).z(0.0)) + .orientation(math::geometry::convertEulerAngleToQuaternion( + geometry_msgs::build().x(0.0).y(0.0).z( + convertDegToRad(yaw_deg)))); +} + auto makePose( geometry_msgs::msg::Point position, geometry_msgs::msg::Quaternion orientation = geometry_msgs::msg::Quaternion()) @@ -63,7 +87,8 @@ auto makePose( auto makeHdMapUtilsSharedPointer() -> std::shared_ptr { return std::make_shared( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", geographic_msgs::build() .latitude(35.9037067912303) .longitude(139.9337945139059) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index 6ab5ba9c6f9..55ac100b94f 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -714,7 +714,8 @@ class TrafficLightTest : public testing::Test protected: TrafficLightTest() : map_manager( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", geographic_msgs::build() .latitude(35.61836750154) .longitude(139.78066608243) diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp index f5b58616f44..3593b18f50b 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp @@ -23,7 +23,8 @@ class TrafficLightManagerTest : public testing::Test protected: TrafficLightManagerTest() : manager(std::make_shared( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", geographic_msgs::build() .latitude(35.61836750154) .longitude(139.78066608243) diff --git a/simulation/traffic_simulator/test/src/utils/CMakeLists.txt b/simulation/traffic_simulator/test/src/utils/CMakeLists.txt index 8c96fcb2eaa..80d35ff1bd2 100644 --- a/simulation/traffic_simulator/test/src/utils/CMakeLists.txt +++ b/simulation/traffic_simulator/test/src/utils/CMakeLists.txt @@ -1,2 +1,6 @@ + +ament_add_gtest(test_distance test_distance.cpp) +target_link_libraries(test_distance traffic_simulator) + ament_add_gtest(test_pose test_pose.cpp) target_link_libraries(test_pose traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp new file mode 100644 index 00000000000..7d59aba49b5 --- /dev/null +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -0,0 +1,708 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// 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. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../helper_functions.hpp" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +class distanceTest_FourTrackHighway : public testing::Test +{ +protected: + distanceTest_FourTrackHighway() + : hdmap_utils_ptr(std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/four_track_highway/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.22312494055522) + .longitude(138.8024583466017) + .altitude(0.0))) + { + } + std::shared_ptr hdmap_utils_ptr; +}; + +class distanceTest_StandardMap : public testing::Test +{ +protected: + distanceTest_StandardMap() + : hdmap_utils_ptr(std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0))) + { + } + std::shared_ptr hdmap_utils_ptr; +}; + +class distanceTest_Intersection : public testing::Test +{ +protected: + distanceTest_Intersection() + : hdmap_utils_ptr(std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/intersection/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.64200728302) + .longitude(139.74821144562) + .altitude(0.0))) + { + } + std::shared_ptr hdmap_utils_ptr; +}; + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is impossible to calculate the distance, e.g. not connected lanelets + * and with allow_lane_change = false. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_noChange) +{ + const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + { + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, std::numeric_limits::infinity(), false, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, false, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is possible to calculate the distance + * and with allow_lane_change = false. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_noChange) +{ + const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(201L, 0.0, 0.0, hdmap_utils_ptr); + { + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, std::numeric_limits::infinity(), false, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 0.0, std::numeric_limits::epsilon()); + } + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, false, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 0.0, std::numeric_limits::epsilon()); + } +} + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is impossible to calculate the distance, e.g. not connected lanelets + * and with allow_lane_change = true. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_change) +{ + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + { + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, std::numeric_limits::infinity(), true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is possible to calculate the distance + * and with allow_lane_change = true. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_change) +{ + const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + constexpr double approx_distance = -3.0; + constexpr double tolerance = 0.5; + { + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, std::numeric_limits::infinity(), true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), approx_distance, tolerance); + } + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), approx_distance, tolerance); + } +} + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is possible to calculate the distance, but matching_distance is too small. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_matching) +{ + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, 2.0, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is possible to calculate the distance and matching_distance is large. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_matching) +{ + const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, 3.0, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), -3.0, 0.5); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = false, + * include_opposite_direction = false, allow_lane_change = false + * in an impossible scenario, e.g. no path. + */ +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_noChange_false) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81595.44, 50006.09, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81584.48, 50084.76, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, false, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = false, + * include_opposite_direction = false, allow_lane_change = false + * in a scenario that meets those criteria. + */ +TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noChange) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(3800.05, 73715.77, 30.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(3841.26, 73748.80, 110.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, false, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 60.0, 1.0); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = false, allow_lane_change = false + * in an impossible scenario, e.g. no path. + */ +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_adjacent_noOpposite_noChange_false) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81585.79, 50042.62, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81588.34, 50083.23, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, false, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = false, allow_lane_change = false + * in a scenario that meets those criteria. + */ +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_adjacent_noOpposite_noChange) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81599.02, 50065.76, 280.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81599.61, 50045.16, 280.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, false, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 20.0, 1.0); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = false, + * include_opposite_direction = false, allow_lane_change = true + * in an impossible scenario, e.g. no path. + */ +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_change_false) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81595.47, 49982.80, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81599.34, 50022.34, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81612.35, 50015.63, 280.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81612.95, 49991.30, 280.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = false, + * include_opposite_direction = false, allow_lane_change = true + * in a scenario that meets those criteria. + */ +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_change) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81592.96, 49997.94, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81570.56, 50141.75, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 145.0, 1.0); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81587.31, 50165.57, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81610.25, 49988.59, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 178.0, 1.0); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = false, allow_lane_change = true + * in an impossible scenario, e.g. no path. + */ +TEST_F(distanceTest_Intersection, longitudinalDistance_adjacent_noOpposite_change_false) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86736.13, 44969.63, 210.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86642.95, 44958.78, 340.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86732.06, 44976.58, 210.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86704.59, 44927.32, 340.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = false, allow_lane_change = true + * in a scenario that meets those criteria. + */ +TEST_F(distanceTest_Intersection, longitudinalDistance_adjacent_noOpposite_change) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86637.19, 44967.35, 340.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86648.82, 44886.19, 240.0), false, hdmap_utils_ptr); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 103.0, 1.0); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86719.94, 44957.20, 210.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86599.32, 44975.01, 180.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 131.0, 1.0); + } +} + +/** + * @note Test equality with math::geometry::getPolygonDistance + * function result on intersecting bounding boxes. + */ +TEST(distance, boundingBoxDistance_intersection) +{ + const auto pose_from = makePose(100.0, 100.0, 0.0); + const auto pose_to = makePose(120.0, 100.0, 90.0); + const auto bounding_box_from = makeCustom2DBoundingBox(30.0, 1.0); + const auto bounding_box_to = makeCustom2DBoundingBox(1.0, 30.0); + + const auto result_distance = traffic_simulator::distance::boundingBoxDistance( + pose_from, bounding_box_from, pose_to, bounding_box_to); + const auto actual_distance = + math::geometry::getPolygonDistance(pose_from, bounding_box_from, pose_to, bounding_box_to); + EXPECT_FALSE(result_distance.has_value()); + EXPECT_FALSE(actual_distance.has_value()); +} + +/** + * @note Test equality with math::geometry::getPolygonDistance + * function result on disjoint bounding boxes. + */ +TEST(distance, boundingBoxDistance_disjoint) +{ + const auto pose_from = makePose(100.0, 100.0, 0.0); + const auto pose_to = makePose(120.0, 100.0, 90.0); + const auto bounding_box_from = makeCustom2DBoundingBox(1.0, 30.0); + const auto bounding_box_to = makeCustom2DBoundingBox(30.0, 1.0); + + const auto result_distance = traffic_simulator::distance::boundingBoxDistance( + pose_from, bounding_box_from, pose_to, bounding_box_to); + const auto actual_distance = + math::geometry::getPolygonDistance(pose_from, bounding_box_from, pose_to, bounding_box_to); + ASSERT_TRUE(result_distance.has_value()); + ASSERT_TRUE(actual_distance.has_value()); + EXPECT_NEAR( + actual_distance.value(), result_distance.value(), std::numeric_limits::epsilon()); +} + +/** + * @note Test calculation correctness with lanelet::Id. + */ +TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) +{ + constexpr lanelet::Id lanelet_id = 34741L; + constexpr double tolerance = 0.1; + { + const auto pose = makePose(3818.33, 73726.18, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 0.5, tolerance); + } + { + const auto pose = makePose(3816.89, 73723.09, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 2.6, tolerance); + } + { + const auto pose = makePose(3813.42, 73721.11, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 2.7, tolerance); + } + { + const auto pose = makePose(3813.42, 73721.11, 120.0); + const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 1.3, tolerance); + } + { + const auto pose = makePose(3810.99, 73721.40, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 1.4, tolerance); + } + { + const auto pose = makePose(3810.99, 73721.40, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 2.4, tolerance); + } + { + const auto pose = makePose(3680.81, 73757.27, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, 34684L, hdmap_utils_ptr); + EXPECT_NEAR(result, 5.1, tolerance); + } + { + const auto pose = makePose(3692.79, 73753.00, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, 34684L, hdmap_utils_ptr); + EXPECT_NEAR(result, 7.2, tolerance); + } +} + +/** + * @note Test calculation correctness with a vector containing multiple lanelets. + * Test equality with the minimum of distanceToLeftLaneBound results (lanelet::Id overload). + */ +TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_multipleVector) +{ + const auto lanelet_ids = lanelet::Ids{34603L, 34600L, 34621L, 34741L}; + const auto pose = makePose(3836.16, 73757.99, 120.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double actual_distance = std::transform_reduce( + lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), + [](const double lhs, const double rhs) { return std::min(lhs, rhs); }, + [&pose, &bounding_box, this](const lanelet::Id lanelet_id) { + return traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + }); + const double result_distance = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_ids, hdmap_utils_ptr); + EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); + EXPECT_NEAR(result_distance, 1.4, 0.1); +} + +/** + * @note Test calculation correctness with a vector containing a single lanelet. + * Test equality with the distanceToLeftLaneBound results (lanelet::Id overload). + */ +TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) +{ + constexpr lanelet::Id lanelet_id = 34426L; + const auto pose = makePose(3693.34, 73738.37, 300.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double actual_distance = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result_distance = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, {lanelet_id}, hdmap_utils_ptr); + EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); + EXPECT_NEAR(result_distance, 1.8, 0.1); +} + +/** + * @note Test function behavior with an empty vector. + */ +TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_emptyVector) +{ + const auto pose = makePose(3825.87, 73773.08, 135.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + EXPECT_THROW( + traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), + common::SemanticError); +} + +/** + * @note Test calculation correctness with lanelet::Id. + */ +TEST_F(distanceTest_Intersection, distanceToRightLaneBound_single) +{ + constexpr lanelet::Id lanelet_id = 660L; + constexpr double tolerance = 0.1; + { + const auto pose = makePose(86651.84, 44941.47, 135.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 4.1, tolerance); + } + { + const auto pose = makePose(86653.05, 44946.74, 135.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 0.6, tolerance); + } + { + const auto pose = makePose(86651.47, 44941.07, 120.0); + const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 4.3, tolerance); + } + { + const auto pose = makePose(86651.47, 44941.07, 210.0); + const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 3.1, tolerance); + } + { + const auto pose = makePose(86644.10, 44951.86, 150.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 2.0, tolerance); + } + { + const auto pose = makePose(86644.10, 44951.86, 150.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 1.1, tolerance); + } + { + const auto pose = makePose(86644.11, 44941.21, 0.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 11.2, tolerance); + } + { + const auto pose = makePose(86656.83, 44946.96, 0.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 2.6, tolerance); + } +} + +/** + * @note Test calculation correctness with a vector containing multiple lanelets. + * Test equality with the minimum of distanceToRightLaneBound results (lanelet::Id overload). + */ +TEST_F(distanceTest_Intersection, distanceToRightLaneBound_multipleVector) +{ + const auto lanelet_ids = lanelet::Ids{660L, 663L, 684L, 654L, 686L}; + const auto pose = makePose(86642.05, 44902.61, 60.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double actual_distance = std::transform_reduce( + lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), + [](const double lhs, const double rhs) { return std::min(lhs, rhs); }, + [&pose, &bounding_box, this](const lanelet::Id lanelet_id) { + return traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + }); + const double result_distance = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_ids, hdmap_utils_ptr); + EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); + EXPECT_NEAR(result_distance, 2.7, 0.1); +} + +/** + * @note Test calculation correctness with a vector containing a single lanelet. + * Test equality with the distanceToRightLaneBound result (lanelet::Id overload). + */ +TEST_F(distanceTest_Intersection, distanceToRightLaneBound_singleVector) +{ + constexpr lanelet::Id lanelet_id = 654L; + const auto pose = makePose(86702.79, 44929.05, 150.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double actual_distance = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result_distance = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, {lanelet_id}, hdmap_utils_ptr); + EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); + EXPECT_NEAR(result_distance, 2.4, 0.1); +} + +/** + * @note Test function behavior with an empty vector. + */ +TEST_F(distanceTest_Intersection, distanceToRightLaneBound_emptyVector) +{ + const auto pose = makePose(3825.87, 73773.08, 135.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + EXPECT_THROW( + traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), + common::SemanticError); +}