From 66baecfd61ddaaefe993a09602ae642f4f929594 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 29 Aug 2024 11:38:10 +0200 Subject: [PATCH 01/11] longitudinal distance tests --- .../traffic_simulator/test/CMakeLists.txt | 9 +- .../test/src/utils/CMakeLists.txt | 2 + .../test/src/utils/test_distance.cpp | 329 ++++++++++++++++++ 3 files changed, 336 insertions(+), 4 deletions(-) create mode 100644 simulation/traffic_simulator/test/src/utils/CMakeLists.txt create mode 100644 simulation/traffic_simulator/test/src/utils/test_distance.cpp diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index 713c8b40678..f60dfacda88 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -1,7 +1,8 @@ -add_subdirectory(src/traffic_lights) -add_subdirectory(src/helper) -add_subdirectory(src/entity) add_subdirectory(src/behavior) +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) diff --git a/simulation/traffic_simulator/test/src/utils/CMakeLists.txt b/simulation/traffic_simulator/test/src/utils/CMakeLists.txt new file mode 100644 index 00000000000..29fb6b608ab --- /dev/null +++ b/simulation/traffic_simulator/test/src/utils/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_distance test_distance.cpp) +target_link_libraries(test_distance 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..7fb03b977c7 --- /dev/null +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -0,0 +1,329 @@ +// 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 + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +/** + * @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(distance, lateralDistance_impossible_noChange) +{ + auto 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)); + 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(distance, lateralDistance_possible_noChange) +{ + auto 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)); + 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(distance, lateralDistance_impossible_change) +{ + auto 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)); + 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(distance, lateralDistance_possible_change) +{ + auto 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)); + 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(distance, lateralDistance_impossible_matching) +{ + auto 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)); + 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(distance, lateralDistance_possible_matching) +{ + auto 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)); + 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, 3.0, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), approx_distance, tolerance); + } +} + +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 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)))); +} + +/** + * @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(distance, longitudinalDistance_noAdjacent_noOpposite_noChange_false) +{ + auto 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)); + + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81595.44, 50006.09, 100.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81584.48, 50084.76, 100.0), false, hdmap_utils_ptr); + { + 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(distance, longitudinalDistance_noAdjacent_noOpposite_noChange) +{ + auto hdmap_utils_ptr = std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0)); + + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(3800.05, 73715.77, 30.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(3841.26, 73748.80, 110.0), false, hdmap_utils_ptr); + { + constexpr double approx_distance = 60.0; + constexpr double tolerance = 1.0; + 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(), approx_distance, tolerance); + } +} + +/** + * @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(distance, longitudinalDistance_adjacent_noOpposite_noChange_false) +{ + auto 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)); + + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81585.79, 50042.62, 100.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81588.34, 50083.23, 100.0), false, hdmap_utils_ptr); + { + 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(distance, longitudinalDistance_adjacent_noOpposite_noChange) +{ + auto 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)); + + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81599.02, 50065.76, 280.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81599.61, 50045.16, 280.0), false, hdmap_utils_ptr); + { + constexpr double approx_distance = 20.0; + constexpr double tolerance = 1.0; + 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(), approx_distance, tolerance); + } +} \ No newline at end of file From 4f7fa88e5d4ea45c44277a1de8b6feed85d73dbb Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 29 Aug 2024 11:38:10 +0200 Subject: [PATCH 02/11] longitudinal distance tests --- .../traffic_simulator/test/CMakeLists.txt | 9 +- .../test/src/utils/CMakeLists.txt | 2 + .../test/src/utils/test_distance.cpp | 329 ++++++++++++++++++ 3 files changed, 336 insertions(+), 4 deletions(-) create mode 100644 simulation/traffic_simulator/test/src/utils/CMakeLists.txt create mode 100644 simulation/traffic_simulator/test/src/utils/test_distance.cpp diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index 713c8b40678..f60dfacda88 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -1,7 +1,8 @@ -add_subdirectory(src/traffic_lights) -add_subdirectory(src/helper) -add_subdirectory(src/entity) add_subdirectory(src/behavior) +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) diff --git a/simulation/traffic_simulator/test/src/utils/CMakeLists.txt b/simulation/traffic_simulator/test/src/utils/CMakeLists.txt new file mode 100644 index 00000000000..29fb6b608ab --- /dev/null +++ b/simulation/traffic_simulator/test/src/utils/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_distance test_distance.cpp) +target_link_libraries(test_distance 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..7fb03b977c7 --- /dev/null +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -0,0 +1,329 @@ +// 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 + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +/** + * @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(distance, lateralDistance_impossible_noChange) +{ + auto 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)); + 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(distance, lateralDistance_possible_noChange) +{ + auto 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)); + 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(distance, lateralDistance_impossible_change) +{ + auto 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)); + 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(distance, lateralDistance_possible_change) +{ + auto 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)); + 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(distance, lateralDistance_impossible_matching) +{ + auto 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)); + 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(distance, lateralDistance_possible_matching) +{ + auto 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)); + 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, 3.0, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), approx_distance, tolerance); + } +} + +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 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)))); +} + +/** + * @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(distance, longitudinalDistance_noAdjacent_noOpposite_noChange_false) +{ + auto 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)); + + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81595.44, 50006.09, 100.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81584.48, 50084.76, 100.0), false, hdmap_utils_ptr); + { + 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(distance, longitudinalDistance_noAdjacent_noOpposite_noChange) +{ + auto hdmap_utils_ptr = std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0)); + + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(3800.05, 73715.77, 30.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(3841.26, 73748.80, 110.0), false, hdmap_utils_ptr); + { + constexpr double approx_distance = 60.0; + constexpr double tolerance = 1.0; + 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(), approx_distance, tolerance); + } +} + +/** + * @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(distance, longitudinalDistance_adjacent_noOpposite_noChange_false) +{ + auto 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)); + + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81585.79, 50042.62, 100.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81588.34, 50083.23, 100.0), false, hdmap_utils_ptr); + { + 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(distance, longitudinalDistance_adjacent_noOpposite_noChange) +{ + auto 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)); + + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81599.02, 50065.76, 280.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81599.61, 50045.16, 280.0), false, hdmap_utils_ptr); + { + constexpr double approx_distance = 20.0; + constexpr double tolerance = 1.0; + 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(), approx_distance, tolerance); + } +} \ No newline at end of file From 6971ebdf08a9d3380261e75259e620378fc05cc5 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 3 Sep 2024 16:14:35 +0200 Subject: [PATCH 03/11] distance tests skeleton, add intersection map and move standard map to its folder --- .../test/map/inctersection/lanelet2_map.osm | 15432 ++++++++++++++++ .../map/{ => standard_map}/lanelet2_map.osm | 0 .../test/src/hdmap_utils/test_hdmap_utils.cpp | 6 +- .../test/src/helper_functions.hpp | 3 +- .../src/traffic_lights/test_traffic_light.cpp | 3 +- .../test_traffic_light_manager.cpp | 3 +- .../test/src/utils/test_distance.cpp | 122 +- 7 files changed, 15562 insertions(+), 7 deletions(-) create mode 100644 simulation/traffic_simulator/test/map/inctersection/lanelet2_map.osm rename simulation/traffic_simulator/test/map/{ => standard_map}/lanelet2_map.osm (100%) diff --git a/simulation/traffic_simulator/test/map/inctersection/lanelet2_map.osm b/simulation/traffic_simulator/test/map/inctersection/lanelet2_map.osm new file mode 100644 index 00000000000..3a239c64159 --- /dev/null +++ b/simulation/traffic_simulator/test/map/inctersection/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..b0bd92f0569 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 @@ -35,7 +35,8 @@ 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", + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", geographic_msgs::build() .latitude(35.61836750154) .longitude(139.78066608243) @@ -132,7 +133,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..ff0cb7248cc 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -63,7 +63,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/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index 7fb03b977c7..249e556be03 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -253,7 +253,8 @@ TEST(distance, longitudinalDistance_noAdjacent_noOpposite_noChange_false) TEST(distance, longitudinalDistance_noAdjacent_noOpposite_noChange) { auto hdmap_utils_ptr = 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) @@ -326,4 +327,121 @@ TEST(distance, longitudinalDistance_adjacent_noOpposite_noChange) ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), approx_distance, tolerance); } -} \ No newline at end of file +} + +/** + * @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(distance, longitudinalDistance_noAdjacent_noOpposite_change_false) +{ + auto 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)); + + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81595.47, 49982.80, 100.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81599.34, 50022.34, 100.0), false, hdmap_utils_ptr); + 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); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81612.95, 49991.30, 280.0), false, hdmap_utils_ptr); + 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(distance, longitudinalDistance_noAdjacent_noOpposite_change) +{ + auto 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)); + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81592.96, 49997.94, 100.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81570.56, 50141.75, 100.0), false, hdmap_utils_ptr); + constexpr double approx_distance = 145.0; + constexpr double tolerance = 1.0; + 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(), approx_distance, tolerance); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81587.31, 50165.57, 100.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81610.25, 49988.59, 100.0), false, hdmap_utils_ptr); + constexpr double approx_distance = 178.0; + constexpr double tolerance = 1.0; + 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(), approx_distance, tolerance); + } +} + +/** + * @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(distance, longitudinalDistance_adjacent_noOpposite_change_false) {} + +/** + * @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(distance, longitudinalDistance_adjacent_noOpposite_change) {} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = true, allow_lane_change = false + * in an impossible scenario, e.g. no path. + */ +TEST(distance, longitudinalDistance_adjacent_opposite_noChange_false) {} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = true, allow_lane_change = false + * in a scenario that meets those criteria. + */ +TEST(distance, longitudinalDistance_adjacent_opposite_noChange) {} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = true, allow_lane_change = true + * in an impossible scenario, e.g. no path. + */ +TEST(distance, longitudinalDistance_adjacent_opposite_change_false) {} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = true, allow_lane_change = true + * in a scenario that meets those criteria. + */ +TEST(distance, longitudinalDistance_adjacent_opposite_change) {} From 851a696e7551e68e86802d70d9ad2060d8ebf9a2 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 5 Sep 2024 11:34:02 +0200 Subject: [PATCH 04/11] clean-up; test_fixture --- .../lanelet2_map.osm | 0 .../test/src/utils/test_distance.cpp | 369 ++++++++++-------- 2 files changed, 212 insertions(+), 157 deletions(-) rename simulation/traffic_simulator/test/map/{inctersection => intersection}/lanelet2_map.osm (100%) diff --git a/simulation/traffic_simulator/test/map/inctersection/lanelet2_map.osm b/simulation/traffic_simulator/test/map/intersection/lanelet2_map.osm similarity index 100% rename from simulation/traffic_simulator/test/map/inctersection/lanelet2_map.osm rename to simulation/traffic_simulator/test/map/intersection/lanelet2_map.osm diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index 249e556be03..3295392590b 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -27,20 +27,61 @@ int main(int argc, char ** 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(distance, lateralDistance_impossible_noChange) +TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_noChange) { - auto 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)); const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( 3002184L, 0.0, 0.0, hdmap_utils_ptr); const auto pose_to = @@ -62,15 +103,8 @@ TEST(distance, lateralDistance_impossible_noChange) * in which it is possible to calculate the distance * and with allow_lane_change = false. */ -TEST(distance, lateralDistance_possible_noChange) +TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_noChange) { - auto 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)); const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( 3002184L, 0.0, 0.0, hdmap_utils_ptr); const auto pose_to = @@ -94,15 +128,8 @@ TEST(distance, lateralDistance_possible_noChange) * in which it is impossible to calculate the distance, e.g. not connected lanelets * and with allow_lane_change = true. */ -TEST(distance, lateralDistance_impossible_change) +TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_change) { - auto 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)); const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( @@ -124,15 +151,8 @@ TEST(distance, lateralDistance_impossible_change) * in which it is possible to calculate the distance * and with allow_lane_change = true. */ -TEST(distance, lateralDistance_possible_change) +TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_change) { - auto 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)); const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( 3002184L, 0.0, 0.0, hdmap_utils_ptr); const auto pose_to = @@ -157,15 +177,8 @@ TEST(distance, lateralDistance_possible_change) * @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(distance, lateralDistance_impossible_matching) +TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_matching) { - auto 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)); const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( @@ -181,26 +194,18 @@ TEST(distance, lateralDistance_impossible_matching) * @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(distance, lateralDistance_possible_matching) +TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_matching) { - auto 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)); 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, 3.0, true, hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); - EXPECT_NEAR(result.value(), approx_distance, tolerance); + EXPECT_NEAR(result.value(), -3.0, 0.5); } } @@ -224,21 +229,16 @@ auto makePose(const double x, const double y, const double yaw_deg) -> geometry_ * include_opposite_direction = false, allow_lane_change = false * in an impossible scenario, e.g. no path. */ -TEST(distance, longitudinalDistance_noAdjacent_noOpposite_noChange_false) +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_noChange_false) { - auto 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)); - - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81595.44, 50006.09, 100.0), false, hdmap_utils_ptr); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81584.48, 50084.76, 100.0), false, hdmap_utils_ptr); { + 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()); @@ -250,27 +250,20 @@ TEST(distance, longitudinalDistance_noAdjacent_noOpposite_noChange_false) * include_opposite_direction = false, allow_lane_change = false * in a scenario that meets those criteria. */ -TEST(distance, longitudinalDistance_noAdjacent_noOpposite_noChange) +TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noChange) { - auto 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)); - - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3800.05, 73715.77, 30.0), false, hdmap_utils_ptr); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3841.26, 73748.80, 110.0), false, hdmap_utils_ptr); { - constexpr double approx_distance = 60.0; - constexpr double tolerance = 1.0; + 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(), approx_distance, tolerance); + EXPECT_NEAR(result.value(), 60.0, 1.0); } } @@ -279,21 +272,16 @@ TEST(distance, longitudinalDistance_noAdjacent_noOpposite_noChange) * include_opposite_direction = false, allow_lane_change = false * in an impossible scenario, e.g. no path. */ -TEST(distance, longitudinalDistance_adjacent_noOpposite_noChange_false) +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_adjacent_noOpposite_noChange_false) { - auto 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)); - - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81585.79, 50042.62, 100.0), false, hdmap_utils_ptr); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81588.34, 50083.23, 100.0), false, hdmap_utils_ptr); { + 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()); @@ -305,27 +293,20 @@ TEST(distance, longitudinalDistance_adjacent_noOpposite_noChange_false) * include_opposite_direction = false, allow_lane_change = false * in a scenario that meets those criteria. */ -TEST(distance, longitudinalDistance_adjacent_noOpposite_noChange) +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_adjacent_noOpposite_noChange) { - auto 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)); - - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.02, 50065.76, 280.0), false, hdmap_utils_ptr); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.61, 50045.16, 280.0), false, hdmap_utils_ptr); { - constexpr double approx_distance = 20.0; - constexpr double tolerance = 1.0; + 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(), approx_distance, tolerance); + EXPECT_NEAR(result.value(), 20.0, 1.0); } } @@ -334,21 +315,16 @@ TEST(distance, longitudinalDistance_adjacent_noOpposite_noChange) * include_opposite_direction = false, allow_lane_change = true * in an impossible scenario, e.g. no path. */ -TEST(distance, longitudinalDistance_noAdjacent_noOpposite_change_false) +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_change_false) { - auto 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)); - { 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()); @@ -356,8 +332,11 @@ TEST(distance, longitudinalDistance_noAdjacent_noOpposite_change_false) { 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()); @@ -369,79 +348,155 @@ TEST(distance, longitudinalDistance_noAdjacent_noOpposite_change_false) * include_opposite_direction = false, allow_lane_change = true * in a scenario that meets those criteria. */ -TEST(distance, longitudinalDistance_noAdjacent_noOpposite_change) +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_change_case0) { - auto 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)); { 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); - constexpr double approx_distance = 145.0; - constexpr double tolerance = 1.0; + 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(), approx_distance, tolerance); + 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); - constexpr double approx_distance = 178.0; - constexpr double tolerance = 1.0; + 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(), approx_distance, tolerance); + 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(distance, longitudinalDistance_adjacent_noOpposite_change_false) {} - -/** - * @note Test calculation correctness with include_adjacent_lanelet = true, + * @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(distance, longitudinalDistance_adjacent_noOpposite_change) {} +TEST_F(distanceTest_Intersection, longitudinalDistance_noAdjacent_noOpposite_change_case1) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86627.71, 44972.06, 340.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); -/** - * @note Test calculation correctness with include_adjacent_lanelet = true, - * include_opposite_direction = true, allow_lane_change = false - * in an impossible scenario, e.g. no path. - */ -TEST(distance, longitudinalDistance_adjacent_opposite_noChange_false) {} + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + EXPECT_TRUE(result.has_value()); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86555.38, 45000.88, 340.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); -/** - * @note Test calculation correctness with include_adjacent_lanelet = true, - * include_opposite_direction = true, allow_lane_change = false - * in a scenario that meets those criteria. - */ -TEST(distance, longitudinalDistance_adjacent_opposite_noChange) {} + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + EXPECT_TRUE(result.has_value()); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86553.48, 44990.56, 150.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_TRUE(result.has_value()); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86787.85, 44998.44, 210.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86648.95, 44884.04, 240.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_TRUE(result.has_value()); + } +} /** * @note Test calculation correctness with include_adjacent_lanelet = true, - * include_opposite_direction = true, allow_lane_change = true + * include_opposite_direction = false, allow_lane_change = true * in an impossible scenario, e.g. no path. */ -TEST(distance, longitudinalDistance_adjacent_opposite_change_false) {} +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 = true, allow_lane_change = true + * include_opposite_direction = false, allow_lane_change = true * in a scenario that meets those criteria. */ -TEST(distance, longitudinalDistance_adjacent_opposite_change) {} +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); + } +} From 059a7c1758afd6ef95217a868bfc69da4d10ef11 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 5 Sep 2024 11:36:46 +0200 Subject: [PATCH 05/11] remove redundant constructor call --- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) 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 b0bd92f0569..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,13 +34,13 @@ class HdMapUtilsTest_StandardMap : public testing::Test { protected: HdMapUtilsTest_StandardMap() - : hdmap_utils(hdmap_utils::HdMapUtils( + : 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)) { } @@ -50,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)) { } @@ -66,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)) { } @@ -82,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)) { } @@ -98,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)) { } @@ -114,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. From 1f6cf08cdb30bc070857221374c7cd1c30e5e06e Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 5 Sep 2024 16:27:07 +0200 Subject: [PATCH 06/11] distance.cpp bug fixes, distanceToRightLaneBound tests --- .../traffic_simulator/src/utils/distance.cpp | 8 +- .../test/src/utils/test_distance.cpp | 167 ++++++++++++++++++ 2 files changed, 174 insertions(+), 1 deletion(-) 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/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index 3295392590b..31cfb0614c2 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -15,7 +15,9 @@ #include #include +#include #include +#include #include #include #include @@ -500,3 +502,168 @@ TEST_F(distanceTest_Intersection, longitudinalDistance_adjacent_noOpposite_chang EXPECT_NEAR(result.value(), 131.0, 1.0); } } + +auto makeBoundingBox( + 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)); +} + +/** + * @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 = makeBoundingBox(30.0, 1.0); + const auto bounding_box_to = makeBoundingBox(1.0, 30.0); + + const auto result = traffic_simulator::distance::boundingBoxDistance( + pose_from, bounding_box_from, pose_to, bounding_box_to); + const auto actual = + math::geometry::getPolygonDistance(pose_from, bounding_box_from, pose_to, bounding_box_to); + EXPECT_FALSE(result.has_value()); + EXPECT_FALSE(actual.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 = makeBoundingBox(1.0, 30.0); + const auto bounding_box_to = makeBoundingBox(30.0, 1.0); + + const auto result = traffic_simulator::distance::boundingBoxDistance( + pose_from, bounding_box_from, pose_to, bounding_box_to); + const auto actual = + math::geometry::getPolygonDistance(pose_from, bounding_box_from, pose_to, bounding_box_to); + ASSERT_TRUE(result.has_value()); + ASSERT_TRUE(actual.has_value()); + EXPECT_NEAR(actual.value(), result.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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); + const auto 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); + const auto 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 = makeBoundingBox(3.0, 0.1, 0.0, 0.0); + const auto 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 = makeBoundingBox(3.0, 0.1, 0.0, 0.0); + const auto 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 = makeBoundingBox(0.1, 0.1, 1.0, 0.0); + const auto 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 = makeBoundingBox(0.1, 0.1, 0.0, -1.0); + const auto 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); + const auto 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); + const auto 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); + const auto 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](lanelet::Id lanelet_id) { + return traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + }); + const auto 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); + const auto actual_distance = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const auto 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, distanceToRightLaneBound_emptyVector) +{ + const auto pose = makePose(3825.87, 73773.08, 135.0); + const auto bounding_box = makeBoundingBox(0.1, 0.1, 0.0, 0.0); + EXPECT_THROW( + traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), + common::SemanticError); +} From 545dec6da89910d5c36fbea323adb0b92c7dd06e Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 6 Sep 2024 10:18:41 +0200 Subject: [PATCH 07/11] distanceToRightLaneBound tests, cleanup --- .../test/src/utils/test_distance.cpp | 164 +++++++++++++++--- 1 file changed, 141 insertions(+), 23 deletions(-) diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index 31cfb0614c2..2a05446490b 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -523,12 +523,12 @@ TEST(distance, boundingBoxDistance_intersection) const auto bounding_box_from = makeBoundingBox(30.0, 1.0); const auto bounding_box_to = makeBoundingBox(1.0, 30.0); - const auto result = traffic_simulator::distance::boundingBoxDistance( + const auto result_distance = traffic_simulator::distance::boundingBoxDistance( pose_from, bounding_box_from, pose_to, bounding_box_to); - const auto actual = + const auto actual_distance = math::geometry::getPolygonDistance(pose_from, bounding_box_from, pose_to, bounding_box_to); - EXPECT_FALSE(result.has_value()); - EXPECT_FALSE(actual.has_value()); + EXPECT_FALSE(result_distance.has_value()); + EXPECT_FALSE(actual_distance.has_value()); } /** @@ -542,13 +542,14 @@ TEST(distance, boundingBoxDistance_disjoint) const auto bounding_box_from = makeBoundingBox(1.0, 30.0); const auto bounding_box_to = makeBoundingBox(30.0, 1.0); - const auto result = traffic_simulator::distance::boundingBoxDistance( + const auto result_distance = traffic_simulator::distance::boundingBoxDistance( pose_from, bounding_box_from, pose_to, bounding_box_to); - const auto actual = + const auto actual_distance = math::geometry::getPolygonDistance(pose_from, bounding_box_from, pose_to, bounding_box_to); - ASSERT_TRUE(result.has_value()); - ASSERT_TRUE(actual.has_value()); - EXPECT_NEAR(actual.value(), result.value(), std::numeric_limits::epsilon()); + ASSERT_TRUE(result_distance.has_value()); + ASSERT_TRUE(actual_distance.has_value()); + EXPECT_NEAR( + actual_distance.value(), result_distance.value(), std::numeric_limits::epsilon()); } /** @@ -561,56 +562,56 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) { const auto pose = makePose(3818.33, 73726.18, 30.0); const auto bounding_box = makeBoundingBox(0.1, 0.1, 0.0, 0.0); - const auto result = traffic_simulator::distance::distanceToLeftLaneBound( + 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); - const auto result = traffic_simulator::distance::distanceToLeftLaneBound( + 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 = makeBoundingBox(3.0, 0.1, 0.0, 0.0); - const auto result = traffic_simulator::distance::distanceToLeftLaneBound( + 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 = makeBoundingBox(3.0, 0.1, 0.0, 0.0); - const auto result = traffic_simulator::distance::distanceToLeftLaneBound( + 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 = makeBoundingBox(0.1, 0.1, 1.0, 0.0); - const auto result = traffic_simulator::distance::distanceToLeftLaneBound( + 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 = makeBoundingBox(0.1, 0.1, 0.0, -1.0); - const auto result = traffic_simulator::distance::distanceToLeftLaneBound( + 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); - const auto result = traffic_simulator::distance::distanceToLeftLaneBound( + 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); - const auto result = traffic_simulator::distance::distanceToLeftLaneBound( + const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, 34684L, hdmap_utils_ptr); EXPECT_NEAR(result, 7.2, tolerance); } @@ -625,14 +626,14 @@ 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); - const auto actual_distance = std::transform_reduce( + 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](lanelet::Id lanelet_id) { + [&pose, &bounding_box, this](const lanelet::Id lanelet_id) { return traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); }); - const auto result_distance = traffic_simulator::distance::distanceToLeftLaneBound( + 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); @@ -647,9 +648,9 @@ 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); - const auto actual_distance = traffic_simulator::distance::distanceToLeftLaneBound( + const double actual_distance = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); - const auto result_distance = traffic_simulator::distance::distanceToLeftLaneBound( + 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); @@ -658,7 +659,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) /** * @note Test function behavior with an empty vector. */ -TEST_F(distanceTest_StandardMap, distanceToRightLaneBound_emptyVector) +TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_emptyVector) { const auto pose = makePose(3825.87, 73773.08, 135.0); const auto bounding_box = makeBoundingBox(0.1, 0.1, 0.0, 0.0); @@ -667,3 +668,120 @@ TEST_F(distanceTest_StandardMap, distanceToRightLaneBound_emptyVector) 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 = makeBoundingBox(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 = makeBoundingBox(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 = makeBoundingBox(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 = makeBoundingBox(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 = makeBoundingBox(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 = makeBoundingBox(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 = makeBoundingBox(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 = makeBoundingBox(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 = makeBoundingBox(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 = makeBoundingBox(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 = makeBoundingBox(0.1, 0.1, 0.0, 0.0); + EXPECT_THROW( + traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), + common::SemanticError); +} From 724a87c7ee16ad3d26693ad2968ba30380e5507f Mon Sep 17 00:00:00 2001 From: robomic Date: Fri, 6 Sep 2024 10:19:50 +0200 Subject: [PATCH 08/11] remove the failing case --- .../test/src/utils/test_distance.cpp | 59 +------------------ 1 file changed, 1 insertion(+), 58 deletions(-) diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index 2a05446490b..f312ce3184e 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -350,7 +350,7 @@ TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite * include_opposite_direction = false, allow_lane_change = true * in a scenario that meets those criteria. */ -TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_change_case0) +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_change) { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( @@ -380,63 +380,6 @@ TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite } } -/** - * @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_Intersection, longitudinalDistance_noAdjacent_noOpposite_change_case1) -{ - { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86627.71, 44972.06, 340.0), false, hdmap_utils_ptr); - ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.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_TRUE(result.has_value()); - } - { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86555.38, 45000.88, 340.0), false, hdmap_utils_ptr); - ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.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_TRUE(result.has_value()); - } - { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); - ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86553.48, 44990.56, 150.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_TRUE(result.has_value()); - } - { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86787.85, 44998.44, 210.0), false, hdmap_utils_ptr); - ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86648.95, 44884.04, 240.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_TRUE(result.has_value()); - } -} - /** * @note Test calculation correctness with include_adjacent_lanelet = true, * include_opposite_direction = false, allow_lane_change = true From cc971b451b16a4cda7e5eb5c147963a786815e80 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Sep 2024 11:54:39 +0200 Subject: [PATCH 09/11] BUG EXAMPLE --- .../src/hdmap_utils/hdmap_utils.cpp | 4 ++ .../test/src/utils/test_distance.cpp | 60 +++++++++++++++++++ 2 files changed, 64 insertions(+) diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 821a2fd0acd..5ff4c58009d 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1547,6 +1547,10 @@ auto HdMapUtils::getLongitudinalDistance( if (route.empty()) { return std::nullopt; } + for (const auto & id : route) { + printf("%ld ", id); + } + printf("\n"); double distance = 0; auto with_lane_change = [this]( diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index f312ce3184e..80aa5ba10b1 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -728,3 +728,63 @@ TEST_F(distanceTest_Intersection, distanceToRightLaneBound_emptyVector) pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), common::SemanticError); } + +TEST(distance, longitudinalDistance_noAdjacent_noOpposite_change_case1) +{ + std::shared_ptr 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)); + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86627.71, 44972.06, 340.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86647.23, 44882.51, 240.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_TRUE(result.has_value()); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86555.38, 45000.88, 340.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86647.23, 44882.51, 240.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_TRUE(result.has_value()); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86553.48, 44990.56, 150.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_TRUE(result.has_value()); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86579.91, 44979.00, 150.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_TRUE(result.has_value()); + } +} From 145246797f2d853b1df19402d9890275520b7741 Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Sep 2024 11:57:12 +0200 Subject: [PATCH 10/11] rm bug example --- .../src/hdmap_utils/hdmap_utils.cpp | 4 -- .../test/src/utils/test_distance.cpp | 60 ------------------- 2 files changed, 64 deletions(-) diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 5ff4c58009d..821a2fd0acd 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1547,10 +1547,6 @@ auto HdMapUtils::getLongitudinalDistance( if (route.empty()) { return std::nullopt; } - for (const auto & id : route) { - printf("%ld ", id); - } - printf("\n"); double distance = 0; auto with_lane_change = [this]( diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index 80aa5ba10b1..f312ce3184e 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -728,63 +728,3 @@ TEST_F(distanceTest_Intersection, distanceToRightLaneBound_emptyVector) pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), common::SemanticError); } - -TEST(distance, longitudinalDistance_noAdjacent_noOpposite_change_case1) -{ - std::shared_ptr 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)); - { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86627.71, 44972.06, 340.0), false, hdmap_utils_ptr); - ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.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_TRUE(result.has_value()); - } - { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86555.38, 45000.88, 340.0), false, hdmap_utils_ptr); - ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.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_TRUE(result.has_value()); - } - { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); - ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86553.48, 44990.56, 150.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_TRUE(result.has_value()); - } - { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); - ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86579.91, 44979.00, 150.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_TRUE(result.has_value()); - } -} From ddd827a2186e7620843fde3751e9dd04e50fca2c Mon Sep 17 00:00:00 2001 From: robomic Date: Mon, 9 Sep 2024 12:07:21 +0200 Subject: [PATCH 11/11] move helper functions to the helper file --- .../test/src/helper_functions.hpp | 24 ++++++ .../test/src/utils/test_distance.cpp | 78 +++++++------------ 2 files changed, 52 insertions(+), 50 deletions(-) diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index ff0cb7248cc..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()) diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index f312ce3184e..7d59aba49b5 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -23,6 +23,8 @@ #include #include +#include "../helper_functions.hpp" + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); @@ -211,21 +213,6 @@ TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_matching) } } -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 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)))); -} - /** * @note Test calculation correctness with include_adjacent_lanelet = false, * include_opposite_direction = false, allow_lane_change = false @@ -446,15 +433,6 @@ TEST_F(distanceTest_Intersection, longitudinalDistance_adjacent_noOpposite_chang } } -auto makeBoundingBox( - 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)); -} - /** * @note Test equality with math::geometry::getPolygonDistance * function result on intersecting bounding boxes. @@ -463,8 +441,8 @@ 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 = makeBoundingBox(30.0, 1.0); - const auto bounding_box_to = makeBoundingBox(1.0, 30.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); @@ -482,8 +460,8 @@ 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 = makeBoundingBox(1.0, 30.0); - const auto bounding_box_to = makeBoundingBox(30.0, 1.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); @@ -504,56 +482,56 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) constexpr double tolerance = 0.1; { const auto pose = makePose(3818.33, 73726.18, 30.0); - const auto bounding_box = makeBoundingBox(0.1, 0.1, 0.0, 0.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 = makeBoundingBox(0.1, 0.1, 0.0, 0.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 = makeBoundingBox(3.0, 0.1, 0.0, 0.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 = makeBoundingBox(3.0, 0.1, 0.0, 0.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 = makeBoundingBox(0.1, 0.1, 1.0, 0.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 = makeBoundingBox(0.1, 0.1, 0.0, -1.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 = makeBoundingBox(0.1, 0.1, 0.0, 0.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 = makeBoundingBox(0.1, 0.1, 0.0, 0.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); @@ -568,7 +546,7 @@ 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.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); }, @@ -590,7 +568,7 @@ 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.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( @@ -605,7 +583,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_emptyVector) { const auto pose = makePose(3825.87, 73773.08, 135.0); - const auto bounding_box = makeBoundingBox(0.1, 0.1, 0.0, 0.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), @@ -621,56 +599,56 @@ TEST_F(distanceTest_Intersection, distanceToRightLaneBound_single) constexpr double tolerance = 0.1; { const auto pose = makePose(86651.84, 44941.47, 135.0); - const auto bounding_box = makeBoundingBox(0.1, 0.1, 0.0, 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, 4.1, tolerance); } { const auto pose = makePose(86653.05, 44946.74, 135.0); - const auto bounding_box = makeBoundingBox(0.1, 0.1, 0.0, 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, 0.6, tolerance); } { const auto pose = makePose(86651.47, 44941.07, 120.0); - const auto bounding_box = makeBoundingBox(3.0, 0.1, 0.0, 0.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 = makeBoundingBox(3.0, 0.1, 0.0, 0.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 = makeBoundingBox(0.1, 0.1, 1.0, 0.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 = makeBoundingBox(0.1, 0.1, 0.0, -1.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 = makeBoundingBox(0.1, 0.1, 0.0, 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 = makeBoundingBox(0.1, 0.1, 0.0, 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); @@ -685,7 +663,7 @@ 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.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); }, @@ -707,7 +685,7 @@ 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 = makeBoundingBox(0.1, 0.1, 0.0, 0.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( @@ -722,7 +700,7 @@ TEST_F(distanceTest_Intersection, distanceToRightLaneBound_singleVector) TEST_F(distanceTest_Intersection, distanceToRightLaneBound_emptyVector) { const auto pose = makePose(3825.87, 73773.08, 135.0); - const auto bounding_box = makeBoundingBox(0.1, 0.1, 0.0, 0.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),