From 9a150553edf2545f2ea76d7f803d48cf735bdd89 Mon Sep 17 00:00:00 2001 From: robomic Date: Tue, 1 Oct 2024 11:21:43 +0200 Subject: [PATCH] simplify lambda --- .../src/hdmap_utils/hdmap_utils.cpp | 38 ++--- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 58 +++++++ .../test/src/utils/test_distance.cpp | 157 +++++++++--------- 3 files changed, 149 insertions(+), 104 deletions(-) diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 53006769f3a..2337d548d34 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1547,27 +1547,19 @@ auto HdMapUtils::getLongitudinalDistance( if (route.empty()) { return std::nullopt; } - double distance = 0.0; - - auto with_lane_change = [this]( - const bool allow_lane_change, const lanelet::Id current_lanelet, - const lanelet::Id next_lanelet) -> bool { - if (allow_lane_change) { - auto next_lanelet_ids = getNextLaneletIds(current_lanelet); - auto next_lanelet_itr = std::find_if( - next_lanelet_ids.begin(), next_lanelet_ids.end(), - [next_lanelet](const lanelet::Id & id) { return id == next_lanelet; }); - return next_lanelet_itr == next_lanelet_ids.end(); - } else { - return false; - } + + const auto with_lane_change = + [this](const lanelet::Id current_lanelet, const lanelet::Id next_lanelet) -> bool { + const auto next_lanelet_ids = getNextLaneletIds(current_lanelet); + return std::none_of( + next_lanelet_ids.begin(), next_lanelet_ids.end(), + [next_lanelet](const lanelet::Id id) { return id == next_lanelet; }); }; - /// @note in this for loop, some cases are marked by @note command. each case is explained in the document. /// @sa https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/DistanceCalculation/ + double accumulated_distance = 0.0; for (std::size_t i = 0UL; i < route.size() - 1UL; ++i) { - if (with_lane_change(allow_lane_change, route[i], route[i + 1UL])) { - /// @note "the lanelet before the lane change" case + if (with_lane_change(route[i], route[i + 1UL])) { const auto curr_lanelet_spline = getCenterPointsSpline(route[i]); const auto next_lanelet_spline = getCenterPointsSpline(route[i + 1UL]); const double mid_length = @@ -1583,21 +1575,21 @@ auto HdMapUtils::getLongitudinalDistance( next_lanelet_spline->getSValue(curr_lanelet_spline->getPose(mid_length), 10.0); if (curr_start_matching_s.has_value()) { - distance += curr_start_matching_s.value(); + accumulated_distance += curr_start_matching_s.value(); } else if (next_start_matching_s.has_value()) { - distance -= next_start_matching_s.value(); + accumulated_distance -= next_start_matching_s.value(); } else if (curr_middle_matching_s.has_value()) { - distance += curr_middle_matching_s.value() - mid_length; + accumulated_distance += curr_middle_matching_s.value() - mid_length; } else if (next_middle_matching_s.has_value()) { - distance -= next_middle_matching_s.value() - mid_length; + accumulated_distance -= next_middle_matching_s.value() - mid_length; } else { return std::nullopt; } } else { - distance += getLaneletLength(route[i]); + accumulated_distance += getLaneletLength(route[i]); } } - return distance - from.s + to.s; + return accumulated_distance - from.s + to.s; } auto HdMapUtils::toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin 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 26b5efaa132..88498d6cf81 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 @@ -125,6 +125,22 @@ class HdMapUtilsTest_KashiwanohaMap : public testing::Test hdmap_utils::HdMapUtils hdmap_utils; }; +class HdMapUtilsTest_IntersectionMap : public testing::Test +{ +protected: + HdMapUtilsTest_IntersectionMap() + : hdmap_utils( + 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)) + { + } + + hdmap_utils::HdMapUtils hdmap_utils; +}; /** * @note Test basic functionality. @@ -2055,6 +2071,48 @@ TEST_F(HdMapUtilsTest_KashiwanohaMap, getLongitudinalDistance_PullRequest1348) 54.18867466433655977198213804513216018676757812500000)); } +/** + * @note Test for the corner case described in https://github.com/tier4/scenario_simulator_v2/issues/1364 + */ +TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) +{ + { + const auto pose_from = traffic_simulator::helper::constructLaneletPose(563UL, 5.0); + const auto pose_to = traffic_simulator::helper::constructLaneletPose(659UL, 5.0); + + const auto result = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 157.0, 1.0)); + } + { + const auto pose_from = traffic_simulator::helper::constructLaneletPose(563UL, 5.0); + const auto pose_to = traffic_simulator::helper::constructLaneletPose(658UL, 5.0); + + const auto result = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 161.0, 1.0)); + } + { + const auto pose_from = traffic_simulator::helper::constructLaneletPose(563UL, 5.0); + const auto pose_to = traffic_simulator::helper::constructLaneletPose(657UL, 5.0); + + const auto result = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 161.0, 1.0)); + } + { + const auto pose_from = traffic_simulator::helper::constructLaneletPose(643UL, 5.0); + const auto pose_to = traffic_simulator::helper::constructLaneletPose(666UL, 5.0); + + const auto result = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 250.0, 1.0)); + } + { + const auto pose_from = traffic_simulator::helper::constructLaneletPose(643UL, 5.0); + const auto pose_to = traffic_simulator::helper::constructLaneletPose(665UL, 5.0); + + const auto result = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 253.0, 1.0)); + } +} + /** * @note Test basic functionality. * Test obtaining stop line ids correctness with a route that has no stop lines. diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index ff914199275..8a2e20fdd12 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -31,10 +31,10 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); } -class distanceTest_FourTrackHighway : public testing::Test +class distanceTest_FourTrackHighwayMap : public testing::Test { protected: - distanceTest_FourTrackHighway() + distanceTest_FourTrackHighwayMap() : hdmap_utils_ptr(std::make_shared( ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/four_track_highway/lanelet2_map.osm", @@ -63,10 +63,10 @@ class distanceTest_StandardMap : public testing::Test std::shared_ptr hdmap_utils_ptr; }; -class distanceTest_Intersection : public testing::Test +class distanceTest_IntersectionMap : public testing::Test { protected: - distanceTest_Intersection() + distanceTest_IntersectionMap() : hdmap_utils_ptr(std::make_shared( ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/intersection/lanelet2_map.osm", @@ -84,7 +84,7 @@ class distanceTest_Intersection : public testing::Test * in which it is impossible to calculate the distance, e.g. not connected lanelets * and with allow_lane_change = false. */ -TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_noChange) +TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_noChange) { const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( 3002184L, 0.0, 0.0, hdmap_utils_ptr); @@ -107,7 +107,7 @@ TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_noChange) * in which it is possible to calculate the distance * and with allow_lane_change = false. */ -TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_noChange) +TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_noChange) { const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( 3002184L, 0.0, 0.0, hdmap_utils_ptr); @@ -132,7 +132,7 @@ TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_noChange) * in which it is impossible to calculate the distance, e.g. not connected lanelets * and with allow_lane_change = true. */ -TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_change) +TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_change) { const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); @@ -155,7 +155,7 @@ TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_change) * in which it is possible to calculate the distance * and with allow_lane_change = true. */ -TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_change) +TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_change) { const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( 3002184L, 0.0, 0.0, hdmap_utils_ptr); @@ -181,7 +181,7 @@ TEST_F(distanceTest_FourTrackHighway, 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_F(distanceTest_FourTrackHighway, lateralDistance_impossible_matching) +TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_matching) { const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); @@ -198,7 +198,7 @@ TEST_F(distanceTest_FourTrackHighway, 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_F(distanceTest_FourTrackHighway, lateralDistance_possible_matching) +TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_matching) { const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( 3002184L, 0.0, 0.0, hdmap_utils_ptr); @@ -218,7 +218,7 @@ TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_matching) * include_opposite_direction = false, allow_lane_change = false * in an impossible scenario, e.g. no path. */ -TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_noChange_false) +TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOpposite_noChange_false) { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( @@ -261,7 +261,7 @@ TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noCh * include_opposite_direction = false, allow_lane_change = false * in an impossible scenario, e.g. no path. */ -TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_adjacent_noOpposite_noChange_false) +TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposite_noChange_false) { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( @@ -282,7 +282,7 @@ TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_adjacent_noOpposite_n * include_opposite_direction = false, allow_lane_change = false * in a scenario that meets those criteria. */ -TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_adjacent_noOpposite_noChange) +TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposite_noChange) { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( @@ -304,7 +304,7 @@ TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_adjacent_noOpposite_n * include_opposite_direction = false, allow_lane_change = true * in an impossible scenario, e.g. no path. */ -TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_change_false) +TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOpposite_change_false) { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( @@ -337,7 +337,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) +TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOpposite_change_case0) { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( @@ -367,12 +367,67 @@ TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite } } +/** + * @note Test for the corner case described in https://github.com/tier4/scenario_simulator_v2/issues/1364 + */ +TEST_F(distanceTest_IntersectionMap, 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_NO_THROW(EXPECT_NEAR(result.value(), 118.0, 1.0)); + } + { + 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_NO_THROW(EXPECT_NEAR(result.value(), 195.0, 1.0)); + } + { + 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_NO_THROW(EXPECT_NEAR(result.value(), 257.0, 1.0)); + } + { + 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_NO_THROW(EXPECT_NEAR(result.value(), 228.0, 1.0)); + } +} + /** * @note Test calculation correctness with include_adjacent_lanelet = true, * include_opposite_direction = false, allow_lane_change = true * in an impossible scenario, e.g. no path. */ -TEST_F(distanceTest_Intersection, longitudinalDistance_adjacent_noOpposite_change_false) +TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_change_false) { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( @@ -405,7 +460,7 @@ TEST_F(distanceTest_Intersection, longitudinalDistance_adjacent_noOpposite_chang * include_opposite_direction = false, allow_lane_change = true * in a scenario that meets those criteria. */ -TEST_F(distanceTest_Intersection, longitudinalDistance_adjacent_noOpposite_change) +TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_change) { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( @@ -593,7 +648,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_emptyVector) /** * @note Test calculation correctness with lanelet::Id. */ -TEST_F(distanceTest_Intersection, distanceToRightLaneBound_single) +TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_single) { constexpr lanelet::Id lanelet_id = 660L; constexpr double tolerance = 0.1; @@ -659,7 +714,7 @@ TEST_F(distanceTest_Intersection, distanceToRightLaneBound_single) * @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) +TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_multipleVector) { const auto lanelet_ids = lanelet::Ids{660L, 663L, 684L, 654L, 686L}; const auto pose = makePose(86642.05, 44902.61, 60.0); @@ -681,7 +736,7 @@ TEST_F(distanceTest_Intersection, distanceToRightLaneBound_multipleVector) * @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) +TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_singleVector) { constexpr lanelet::Id lanelet_id = 654L; const auto pose = makePose(86702.79, 44929.05, 150.0); @@ -697,7 +752,7 @@ TEST_F(distanceTest_Intersection, distanceToRightLaneBound_singleVector) /** * @note Test function behavior with an empty vector. */ -TEST_F(distanceTest_Intersection, distanceToRightLaneBound_emptyVector) +TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_emptyVector) { const auto pose = makePose(3825.87, 73773.08, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); @@ -706,63 +761,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_NO_THROW(EXPECT_NEAR(result.value(), 118.0, 1.0)); - } - { - 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_NO_THROW(EXPECT_NEAR(result.value(), 195.0, 1.0)); - } - { - 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_NO_THROW(EXPECT_NEAR(result.value(), 257.0, 1.0)); - } - { - 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_NO_THROW(EXPECT_NEAR(result.value(), 228.0, 1.0)); - } -} \ No newline at end of file