From b747a1a1cc0c26f4186f5fef64e4cab2748c97fe Mon Sep 17 00:00:00 2001 From: abco20 Date: Mon, 17 Feb 2025 17:00:42 +0900 Subject: [PATCH 1/4] Add toPolygon function --- .../traffic_simulator/lanelet_wrapper/lanelet_map.hpp | 3 +++ .../src/lanelet_wrapper/lanelet_map.cpp | 11 +++++++++++ 2 files changed, 14 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp index ebda8439df7..3ac23aa2b97 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp @@ -89,6 +89,9 @@ auto previousLaneletIds( auto previousLaneletIds( const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +// Polygons +auto toPolygon(const lanelet::ConstLineString3d & line_string) -> std::vector; } // namespace lanelet_map } // namespace lanelet_wrapper } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 7a8d673cd5f..5a1d3474358 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -235,6 +235,17 @@ auto previousLaneletIds( } return lanelet::Ids(previous_lanelet_ids_set.begin(), previous_lanelet_ids_set.end()); } + +// Polygons +auto toPolygon(const lanelet::ConstLineString3d & line_string) -> std::vector +{ + std::vector points; + points.reserve(line_string.size()); + for (const auto & point : line_string) { + points.push_back(geometry_msgs::build().x(point.x()).y(point.y()).z(point.z())); + } + return points; +} } // namespace lanelet_map } // namespace lanelet_wrapper } // namespace traffic_simulator From 984f92899cc0d134d858fc90b4b3808cfdfa1a48 Mon Sep 17 00:00:00 2001 From: abco20 Date: Thu, 20 Feb 2025 15:29:17 +0900 Subject: [PATCH 2/4] move `getLeftBound` and `getRightBound` methods --- .../traffic_simulator/hdmap_utils/hdmap_utils.hpp | 4 ---- .../lanelet_wrapper/lanelet_map.hpp | 5 +++++ .../src/hdmap_utils/hdmap_utils.cpp | 12 ------------ .../src/lanelet_wrapper/lanelet_map.cpp | 11 +++++++++++ simulation/traffic_simulator/src/utils/distance.cpp | 6 ++++-- 5 files changed, 20 insertions(+), 18 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 199b01890da..f724fc08a67 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -179,8 +179,6 @@ class HdMapUtils const traffic_simulator::RoutingConfiguration & routing_configuration = traffic_simulator::RoutingConfiguration()) const -> std::optional; - auto getLeftBound(const lanelet::Id) const -> std::vector; - auto getLongitudinalDistance( const traffic_simulator_msgs::msg::LaneletPose & from_pose, const traffic_simulator_msgs::msg::LaneletPose & to_pose, @@ -200,8 +198,6 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - auto getRightBound(const lanelet::Id) const -> std::vector; - auto getRightOfWayLaneletIds(const lanelet::Ids &) const -> std::unordered_map; diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp index 3ac23aa2b97..f317a25db4f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp @@ -90,6 +90,11 @@ auto previousLaneletIds( const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; +// Bounds +auto leftBound(const lanelet::Id lanelet_id) -> std::vector; + +auto rightBound(const lanelet::Id lanelet_id) -> std::vector; + // Polygons auto toPolygon(const lanelet::ConstLineString3d & line_string) -> std::vector; } // namespace lanelet_map diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 8a4c5043465..71b43dce14a 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -740,18 +740,6 @@ auto HdMapUtils::getTrafficLightBulbPosition( return std::nullopt; } -auto HdMapUtils::getLeftBound(const lanelet::Id lanelet_id) const - -> std::vector -{ - return toPolygon(lanelet_map_ptr_->laneletLayer.get(lanelet_id).leftBound()); -} - -auto HdMapUtils::getRightBound(const lanelet::Id lanelet_id) const - -> std::vector -{ - return toPolygon(lanelet_map_ptr_->laneletLayer.get(lanelet_id).rightBound()); -} - auto HdMapUtils::getLaneChangeTrajectory( const traffic_simulator_msgs::msg::LaneletPose & from_pose, const traffic_simulator::lane_change::Parameter & lane_change_parameter) const diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 5a1d3474358..f73d9c16db3 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -236,6 +236,17 @@ auto previousLaneletIds( return lanelet::Ids(previous_lanelet_ids_set.begin(), previous_lanelet_ids_set.end()); } +// Bounds +auto leftBound(const lanelet::Id lanelet_id) -> std::vector +{ + return toPolygon(LaneletWrapper::map()->laneletLayer.get(lanelet_id).leftBound()); +} + +auto rightBound(const lanelet::Id lanelet_id) -> std::vector +{ + return toPolygon(LaneletWrapper::map()->laneletLayer.get(lanelet_id).rightBound()); +} + // Polygons auto toPolygon(const lanelet::ConstLineString3d & line_string) -> std::vector { diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 880a9cbfa5e..1985a7d415f 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -197,12 +198,13 @@ auto boundingBoxLaneLongitudinalDistance( return std::nullopt; } +// Bounds auto distanceToLeftLaneBound( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, const std::shared_ptr & hdmap_utils_ptr) -> double { - if (const auto bound = hdmap_utils_ptr->getLeftBound(lanelet_id); bound.empty()) { + if (const auto bound = lanelet_wrapper::lanelet_map::leftBound(lanelet_id); bound.empty()) { THROW_SEMANTIC_ERROR( "Failed to calculate left bounds of lanelet_id : ", lanelet_id, " please check lanelet map."); } else if (const auto polygon = @@ -235,7 +237,7 @@ auto distanceToRightLaneBound( const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, const std::shared_ptr & hdmap_utils_ptr) -> double { - if (const auto bound = hdmap_utils_ptr->getRightBound(lanelet_id); bound.empty()) { + if (const auto & bound = lanelet_wrapper::lanelet_map::rightBound(lanelet_id); bound.empty()) { THROW_SEMANTIC_ERROR( "Failed to calculate right bounds of lanelet_id : ", lanelet_id, " please check lanelet map."); From 860c419d3eeb744a0a226e9e09bcee30f0e4e298 Mon Sep 17 00:00:00 2001 From: abco20 Date: Thu, 20 Feb 2025 15:18:27 +0900 Subject: [PATCH 3/4] remove hdmap_utils from `distanceToLeftLaneBound` and `distanceToRightLaneBound` --- .../traffic_simulator/utils/distance.hpp | 16 +-- .../traffic_simulator/src/utils/distance.cpp | 52 ++++----- .../test/src/utils/test_distance.cpp | 100 +++++++++--------- 3 files changed, 84 insertions(+), 84 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 5a43dd030dc..ab0f2b01085 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -84,23 +84,23 @@ auto distanceToLaneBound( auto distanceToLeftLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double; auto distanceToLeftLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> double; auto distanceToRightLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double; auto distanceToRightLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> double; // Other objects auto distanceToCrosswalk( diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 1985a7d415f..f1053f4c1e1 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -201,8 +201,8 @@ auto boundingBoxLaneLongitudinalDistance( // Bounds auto distanceToLeftLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double { if (const auto bound = lanelet_wrapper::lanelet_map::leftBound(lanelet_id); bound.empty()) { THROW_SEMANTIC_ERROR( @@ -218,24 +218,26 @@ auto distanceToLeftLaneBound( auto distanceToLeftLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> 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) { - return distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr); - }); - return *std::min_element(distances.begin(), distances.end()); + double min_distance = std::numeric_limits::max(); + for (const auto & lanelet_id : lanelet_ids) { + const auto distance = distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id); + if (distance < min_distance) { + min_distance = distance; + } + } + return min_distance; } auto distanceToRightLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double { if (const auto & bound = lanelet_wrapper::lanelet_map::rightBound(lanelet_id); bound.empty()) { THROW_SEMANTIC_ERROR( @@ -252,18 +254,20 @@ auto distanceToRightLaneBound( auto distanceToRightLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> 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 distanceToRightLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr); - }); - return *std::min_element(distances.begin(), distances.end()); + double min_distance = std::numeric_limits::max(); + for (const auto & lanelet_id : lanelet_ids) { + const double distance = distanceToRightLaneBound(map_pose, bounding_box, lanelet_id); + if (distance < min_distance) { + min_distance = distance; + } + } + return min_distance; } auto distanceToLaneBound( @@ -272,8 +276,8 @@ auto distanceToLaneBound( const std::shared_ptr & hdmap_utils_ptr) -> double { return std::min( - distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr), - distanceToRightLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr)); + distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id), + distanceToRightLaneBound(map_pose, bounding_box, lanelet_id)); } auto distanceToLaneBound( @@ -282,8 +286,8 @@ auto distanceToLaneBound( const std::shared_ptr & hdmap_utils_ptr) -> double { return std::min( - distanceToLeftLaneBound(map_pose, bounding_box, lanelet_ids, hdmap_utils_ptr), - distanceToRightLaneBound(map_pose, bounding_box, lanelet_ids, hdmap_utils_ptr)); + distanceToLeftLaneBound(map_pose, bounding_box, lanelet_ids), + distanceToRightLaneBound(map_pose, bounding_box, lanelet_ids)); } auto distanceToCrosswalk( diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index 6eaa2e00170..d2cc01834b7 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -593,57 +593,57 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) { const auto pose = makePose(3818.33, 73726.18, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 0.5, tolerance); } { const auto pose = makePose(3816.89, 73723.09, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 2.6, tolerance); } { const auto pose = makePose(3813.42, 73721.11, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 2.7, tolerance); } { const auto pose = makePose(3813.42, 73721.11, 0.0, 120.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 1.3, tolerance); } { const auto pose = makePose(3810.99, 73721.40, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 1.4, tolerance); } { const auto pose = makePose(3810.99, 73721.40, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 2.4, tolerance); } { const auto pose = makePose(3680.81, 73757.27, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, 34684L, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, 34684L); EXPECT_NEAR(result, 5.1, tolerance); } { const auto pose = makePose(3692.79, 73753.00, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, 34684L, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, 34684L); EXPECT_NEAR(result, 7.2, tolerance); } } @@ -661,11 +661,10 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_multipleVector) lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), [](const double lhs, const double rhs) { return std::min(lhs, rhs); }, [&pose, &bounding_box, this](const lanelet::Id lanelet_id) { - return traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + return traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); }); - const double result_distance = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_ids, hdmap_utils_ptr); + const double result_distance = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_ids); EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); EXPECT_NEAR(result_distance, 1.4, 0.1); } @@ -679,10 +678,10 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) constexpr lanelet::Id lanelet_id = 34426L; const auto pose = makePose(3693.34, 73738.37, 0.0, 300.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double actual_distance = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); - const double result_distance = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, {lanelet_id}, hdmap_utils_ptr); + const double actual_distance = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); + const double result_distance = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, {lanelet_id}); EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); EXPECT_NEAR(result_distance, 1.8, 0.1); } @@ -695,8 +694,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_emptyVector) const auto pose = makePose(3825.87, 73773.08, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); EXPECT_THROW( - traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet::Ids{}), common::SemanticError); } @@ -710,57 +708,57 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_single) { const auto pose = makePose(86651.84, 44941.47, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 4.1, tolerance); } { const auto pose = makePose(86653.05, 44946.74, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 0.6, tolerance); } { const auto pose = makePose(86651.47, 44941.07, 0.0, 120.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 4.3, tolerance); } { const auto pose = makePose(86651.47, 44941.07, 0.0, 210.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 3.1, tolerance); } { const auto pose = makePose(86644.10, 44951.86, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 2.0, tolerance); } { const auto pose = makePose(86644.10, 44951.86, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 1.1, tolerance); } { const auto pose = makePose(86644.11, 44941.21, 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); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 11.2, tolerance); } { const auto pose = makePose(86656.83, 44946.96, 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); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 2.6, tolerance); } } @@ -778,11 +776,10 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_multipleVector) 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); + return traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); }); - const double result_distance = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_ids, hdmap_utils_ptr); + const double result_distance = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_ids); EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); EXPECT_NEAR(result_distance, 2.7, 0.1); } @@ -796,10 +793,10 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_singleVector) constexpr lanelet::Id lanelet_id = 654L; const auto pose = makePose(86702.79, 44929.05, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double actual_distance = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); - const double result_distance = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, {lanelet_id}, hdmap_utils_ptr); + const double actual_distance = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); + const double result_distance = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, {lanelet_id}); EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); EXPECT_NEAR(result_distance, 2.4, 0.1); } @@ -812,7 +809,6 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_emptyVector) const auto pose = makePose(3825.87, 73773.08, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); EXPECT_THROW( - traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet::Ids{}), common::SemanticError); } From d65168abf1c5cb837f92de7c50340cf47279958b Mon Sep 17 00:00:00 2001 From: abco20 Date: Thu, 20 Feb 2025 15:19:16 +0900 Subject: [PATCH 4/4] remove hdmap_utils from `distanceToLaneBound` --- .../src/measurement/get_distance_to_lane_bound.cpp | 3 +-- .../include/traffic_simulator/utils/distance.hpp | 8 ++++---- simulation/traffic_simulator/src/utils/distance.cpp | 8 ++++---- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp index d9404e163d8..04f25cacb68 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp @@ -46,8 +46,7 @@ class GetDistanceToLaneBoundScenario : public cpp_mock_scenarios::CppScenarioNod } auto & ego_entity = api_.getEntity("ego"); const auto distance = traffic_simulator::distance::distanceToLaneBound( - ego_entity.getMapPose(), ego_entity.getBoundingBox(), ego_entity.getRouteLanelets(), - api_.getHdmapUtils()); + ego_entity.getMapPose(), ego_entity.getBoundingBox(), ego_entity.getRouteLanelets()); // LCOV_EXCL_START if (distance <= 0.4 && distance >= 0.52) { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index ab0f2b01085..e5480b04f61 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -74,13 +74,13 @@ auto boundingBoxLaneLongitudinalDistance( // Bounds auto distanceToLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double; auto distanceToLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> double; auto distanceToLeftLaneBound( const geometry_msgs::msg::Pose & map_pose, diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index f1053f4c1e1..83cbabbdb84 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -272,8 +272,8 @@ auto distanceToRightLaneBound( auto distanceToLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double { return std::min( distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id), @@ -282,8 +282,8 @@ auto distanceToLaneBound( auto distanceToLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> double { return std::min( distanceToLeftLaneBound(map_pose, bounding_box, lanelet_ids),