From 751f68b7297614a330511becab4e1a0f64f98427 Mon Sep 17 00:00:00 2001 From: abco20 Date: Thu, 27 Feb 2025 15:24:38 +0900 Subject: [PATCH] move `getDistanceToStopLine` method --- .../behavior_tree_plugin/action_node.hpp | 3 - .../behavior_tree_plugin/src/action_node.cpp | 7 -- .../follow_front_entity_action.cpp | 3 +- .../follow_lane_action.cpp | 3 +- .../stop_at_crossing_entity_action.cpp | 3 +- .../stop_at_stop_line_action.cpp | 3 +- simulation/traffic_simulator/CMakeLists.txt | 1 + .../hdmap_utils/hdmap_utils.hpp | 8 --- .../lanelet_wrapper/distance.hpp | 42 +++++++++++ .../lanelet_wrapper/lanelet_wrapper.hpp | 2 + .../traffic_simulator/utils/distance.hpp | 11 +-- .../src/hdmap_utils/hdmap_utils.cpp | 62 ---------------- .../src/lanelet_wrapper/distance.cpp | 72 +++++++++++++++++++ .../traffic_simulator/src/utils/distance.cpp | 14 ---- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 66 ++++++++--------- 15 files changed, 162 insertions(+), 138 deletions(-) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 58c4199af50..7257c7b6ab5 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -55,9 +55,6 @@ class ActionNode : public BT::ActionNodeBase -> double; auto getDistanceToFrontEntity(const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional; - auto getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const std::vector & waypoints) const -> std::optional; auto getDistanceToTrafficLightStopLine( const lanelet::Ids & route_lanelets, const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional; diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 80a685d79da..e66e45342ef 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -224,13 +224,6 @@ auto ActionNode::getDistanceToTrafficLightStopLine( return std::nullopt; } -auto ActionNode::getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const std::vector & waypoints) const -> std::optional -{ - return hdmap_utils->getDistanceToStopLine(route_lanelets, waypoints); -} - auto ActionNode::getDistanceToFrontEntity( const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp index 71e8a78cdca..784e1c77b08 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp @@ -90,7 +90,8 @@ BT::NodeStatus FollowFrontEntityAction::tick() if (trajectory == nullptr) { return BT::NodeStatus::FAILURE; } - auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); + auto distance_to_stopline = + traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory); auto distance_to_conflicting_entity = getDistanceToConflictingEntity(route_lanelets, *trajectory); const auto front_entity_name = getFrontEntityName(*trajectory); if (!front_entity_name) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp index 624f59cb07e..dda89abe673 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp @@ -108,7 +108,8 @@ BT::NodeStatus FollowLaneAction::tick() return BT::NodeStatus::FAILURE; } } - auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); + auto distance_to_stopline = + traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory); auto distance_to_conflicting_entity = getDistanceToConflictingEntity(route_lanelets, *trajectory); if (distance_to_stopline) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp index cdc751475e4..a84212501a8 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp @@ -112,7 +112,8 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() return BT::NodeStatus::FAILURE; } distance_to_stop_target_ = getDistanceToConflictingEntity(route_lanelets, *trajectory); - auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); + auto distance_to_stopline = + traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory); const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); if (!distance_to_stop_target_) { in_stop_sequence_ = false; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp index 74cc2ee182e..baa8e750cec 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp @@ -108,7 +108,8 @@ BT::NodeStatus StopAtStopLineAction::tick() if (trajectory == nullptr) { return BT::NodeStatus::FAILURE; } - distance_to_stopline_ = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); + distance_to_stopline_ = + traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory); const auto distance_to_stop_target = getDistanceToConflictingEntity(route_lanelets, *trajectory); const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); if (!distance_to_stopline_) { diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index ffdf947bbad..29edc7153e0 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -48,6 +48,7 @@ ament_auto_add_library(traffic_simulator SHARED src/helper/helper.cpp src/job/job.cpp src/job/job_list.cpp + src/lanelet_wrapper/distance.cpp src/lanelet_wrapper/lanelet_loader.cpp src/lanelet_wrapper/lanelet_map.cpp src/lanelet_wrapper/lanelet_wrapper.cpp 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 dba499ade63..ed965c7d71e 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 @@ -106,14 +106,6 @@ class HdMapUtils traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - auto getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional; - - auto getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const std::vector & waypoints) const -> std::optional; - auto getDistanceToTrafficLightStopLine( const lanelet::Ids & route_lanelets, const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional; diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp new file mode 100644 index 00000000000..ca2a03be617 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp @@ -0,0 +1,42 @@ + +// 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. + +#ifndef TRAFFIC_SIMULATOR__LANELET_WRAPPER_DISTANCE_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_DISTANCE_HPP_ + +#include + +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace distance +{ +// StopLine +auto distanceToStopLine(const lanelet::Ids & route_lanelets, const SplineInterface & route_spline) + -> std::optional; + +auto distanceToStopLine( + const lanelet::Ids & route_lanelets, const std::vector & route_waypoints) + -> std::optional; + +auto distanceToStopLine(const std::vector & route_waypoints, const lanelet::Id stop_line_id) + -> std::optional; +} // namespace distance +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_DISTANCE_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp index b0bfca88a94..7f8af41ae65 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -66,6 +67,7 @@ using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; using Spline = math::geometry::CatmullRomSpline; using Vector3 = geometry_msgs::msg::Vector3; +using SplineInterface = math::geometry::CatmullRomSplineInterface; class RouteCache { diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index e5480b04f61..7887c453254 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -15,8 +15,10 @@ #ifndef TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_ #define TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_ +#include #include #include +#include namespace traffic_simulator { @@ -108,10 +110,11 @@ auto distanceToCrosswalk( const lanelet::Id target_crosswalk_id, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; -auto distanceToStopLine( - const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array, - const lanelet::Id target_stop_line_id, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional; +template +auto distanceToStopLine(Ts &&... xs) +{ + return lanelet_wrapper::distance::distanceToStopLine(std::forward(xs)...); +} // spline auto distanceToSpline( diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 43b5b586798..b7a733f03a1 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1377,68 +1377,6 @@ auto HdMapUtils::getDistanceToTrafficLightStopLine( return std::nullopt; } -auto HdMapUtils::getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const std::vector & waypoints) const -> std::optional -{ - if (waypoints.empty()) { - return std::nullopt; - } - std::set collision_points; - if (waypoints.empty()) { - return std::nullopt; - } - math::geometry::CatmullRomSpline spline(waypoints); - const auto stop_lines = getStopLinesOnPath({route_lanelets}); - for (const auto & stop_line : stop_lines) { - std::vector stop_line_points; - for (const auto & point : stop_line) { - geometry_msgs::msg::Point p; - p.x = point.x(); - p.y = point.y(); - p.z = point.z(); - stop_line_points.emplace_back(p); - } - const auto collision_point = spline.getCollisionPointIn2D(stop_line_points); - if (collision_point) { - collision_points.insert(collision_point.value()); - } - } - if (collision_points.empty()) { - return std::nullopt; - } - return *collision_points.begin(); -} - -auto HdMapUtils::getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional -{ - if (spline.getLength() <= 0) { - return std::nullopt; - } - std::set collision_points; - const auto stop_lines = getStopLinesOnPath({route_lanelets}); - for (const auto & stop_line : stop_lines) { - std::vector stop_line_points; - for (const auto & point : stop_line) { - geometry_msgs::msg::Point p; - p.x = point.x(); - p.y = point.y(); - p.z = point.z(); - stop_line_points.emplace_back(p); - } - const auto collision_point = spline.getCollisionPointIn2D(stop_line_points); - if (collision_point) { - collision_points.insert(collision_point.value()); - } - } - if (collision_points.empty()) { - return std::nullopt; - } - return *collision_points.begin(); -} - auto HdMapUtils::calculateSegmentDistances(const lanelet::ConstLineString3d & line_string) const -> std::vector { diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp new file mode 100644 index 00000000000..e40f0c9c901 --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp @@ -0,0 +1,72 @@ +// 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 + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace distance +{ +// StopLine +auto distanceToStopLine(const lanelet::Ids & route_lanelets, const SplineInterface & route_spline) + -> std::optional +{ + if (route_spline.getLength() <= 0.0) { + return std::nullopt; + } else { + std::optional min_distance{std::nullopt}; + const auto & stop_lines = lanelet_wrapper::lanelet_map::stopLinesOnPath({route_lanelets}); + for (const auto & stop_line : stop_lines) { + const auto & stop_line_points = lanelet_wrapper::lanelet_map::toPolygon(stop_line); + if (const auto & collision_point = route_spline.getCollisionPointIn2D(stop_line_points)) { + if (not min_distance.has_value() or collision_point.value() < min_distance.value()) { + min_distance = collision_point; + } + } + } + return min_distance; + } +} + +auto distanceToStopLine( + const lanelet::Ids & route_lanelets, const std::vector & route_waypoints) + -> std::optional +{ + if (route_waypoints.empty()) { + return std::nullopt; + } else { + return distanceToStopLine(route_lanelets, Spline{route_waypoints}); + } +} + +auto distanceToStopLine(const std::vector & route_waypoints, const lanelet::Id stop_line_id) + -> std::optional +{ + if (route_waypoints.empty()) { + return std::nullopt; + } else { + const Spline route_spline{route_waypoints}; + return route_spline.getCollisionPointIn2D( + lanelet_wrapper::lanelet_map::stopLinePolygon(stop_line_id)); + } +} +} // namespace distance +} // namespace lanelet_wrapper +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 87f219b5015..6c3b80bb9d6 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -305,20 +305,6 @@ auto distanceToCrosswalk( } } -auto distanceToStopLine( - const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array, - const lanelet::Id target_stop_line_id, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional -{ - if (waypoints_array.waypoints.empty()) { - return std::nullopt; - } else { - const math::geometry::CatmullRomSpline spline(waypoints_array.waypoints); - const auto polygon = hdmap_utils_ptr->getStopLinePolygon(target_stop_line_id); - return spline.getCollisionPointIn2D(polygon); - } -} - auto distanceToSpline( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, 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 62c13f8d399..90ae2ec16eb 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 @@ -2696,7 +2696,7 @@ TEST_F( TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_stopLineOnSpline) { const auto start_waypoint = makePoint(3821.86, 73777.20); - const auto result_distance = hdmap_utils.getDistanceToStopLine( + const auto result_distance = traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( lanelet::Ids{34780, 34675, 34744}, math::geometry::CatmullRomSpline(std::vector{ start_waypoint, makePoint(3837.28, 73762.67), makePoint(3846.10, 73741.38)})); @@ -2715,13 +2715,12 @@ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_stopLine */ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_noStopLineOnSplineCongruent) { - EXPECT_FALSE(hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{34690, 34759, 34576}, - math::geometry::CatmullRomSpline(std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), - makePoint(3773.19, 73723.27)})) - .has_value()); + EXPECT_FALSE( + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{34690, 34759, 34576}, + math::geometry::CatmullRomSpline(std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)})) + .has_value()); } /** @@ -2734,13 +2733,12 @@ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_noStopLi TEST_F( HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_noStopLineOnSplineIncongruent) { - EXPECT_FALSE(hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{34576, 34570, 34564}, - math::geometry::CatmullRomSpline(std::vector{ - makePoint(3821.86, 73777.20), makePoint(3837.28, 73762.67), - makePoint(3846.10, 73741.38)})) - .has_value()); + EXPECT_FALSE( + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{34576, 34570, 34564}, + math::geometry::CatmullRomSpline(std::vector{ + makePoint(3821.86, 73777.20), makePoint(3837.28, 73762.67), makePoint(3846.10, 73741.38)})) + .has_value()); } /** @@ -2749,11 +2747,10 @@ TEST_F( TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_emptyVector_spline) { EXPECT_FALSE( - hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{}, math::geometry::CatmullRomSpline(std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), - makePoint(3773.19, 73723.27)})) + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{}, + math::geometry::CatmullRomSpline(std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)})) .has_value()); } @@ -2765,7 +2762,7 @@ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_emptyVec TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_stopLineOnWaypoints) { const auto start_waypoint = makePoint(3821.86, 73777.20); - const auto result_distance = hdmap_utils.getDistanceToStopLine( + const auto result_distance = traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( lanelet::Ids{34780, 34675, 34744}, std::vector{ start_waypoint, makePoint(3837.28, 73762.67), makePoint(3846.10, 73741.38)}); @@ -2786,11 +2783,10 @@ TEST_F( HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_noStopLineOnWaypointsCongruent) { EXPECT_FALSE( - hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{34690, 34759, 34576}, - std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{34690, 34759, 34576}, + std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) .has_value()); } @@ -2805,11 +2801,10 @@ TEST_F( HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_noStopLineOnWaypointsIncongruent) { EXPECT_FALSE( - hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{34576, 34570, 34564}, - std::vector{ - makePoint(3821.86, 73777.20), makePoint(3837.28, 73762.67), makePoint(3846.10, 73741.38)}) + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{34576, 34570, 34564}, + std::vector{ + makePoint(3821.86, 73777.20), makePoint(3837.28, 73762.67), makePoint(3846.10, 73741.38)}) .has_value()); } @@ -2819,11 +2814,10 @@ TEST_F( TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_emptyVector_waypoints) { EXPECT_FALSE( - hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{}, - std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{}, + std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) .has_value()); }