diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp index 9b6c59895ee..eed467535ce 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp @@ -50,7 +50,8 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar to_entity && to_entity->laneMatchingSucceed()) { return traffic_simulator::distance::lateralDistance( from_entity->getCanonicalizedLaneletPose().value(), - to_entity->getCanonicalizedLaneletPose().value(), false, api_.getHdmapUtils()); + to_entity->getCanonicalizedLaneletPose().value(), + traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils()); } } return std::nullopt; @@ -66,8 +67,8 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar auto to_entity_lanelet_pose = to_entity->getCanonicalizedLaneletPose(matching_distance); if (from_entity_lanelet_pose && to_entity_lanelet_pose) { return traffic_simulator::distance::lateralDistance( - from_entity_lanelet_pose.value(), to_entity_lanelet_pose.value(), false, - api_.getHdmapUtils()); + from_entity_lanelet_pose.value(), to_entity_lanelet_pose.value(), + traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils()); } } } @@ -84,8 +85,8 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar to_entity && to_entity->laneMatchingSucceed()) { return traffic_simulator::distance::longitudinalDistance( from_entity->getCanonicalizedLaneletPose().value(), - to_entity->getCanonicalizedLaneletPose().value(), false, true, false, - api_.getHdmapUtils()); + to_entity->getCanonicalizedLaneletPose().value(), false, true, + traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils()); } } return std::nullopt; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 393b4a11420..1891391ca86 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -176,9 +176,11 @@ class SimulatorCore const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) -> traffic_simulator::LaneletPose { - const bool allow_lane_change = (routing_algorithm == RoutingAlgorithm::value_type::shortest); + traffic_simulator::RoutingConfiguration routing_configuration; + routing_configuration.allow_lane_change = + (routing_algorithm == RoutingAlgorithm::value_type::shortest); return traffic_simulator::pose::relativeLaneletPose( - from_lanelet_pose, to_lanelet_pose, allow_lane_change, core->getHdmapUtils()); + from_lanelet_pose, to_lanelet_pose, routing_configuration, core->getHdmapUtils()); } static auto makeNativeBoundingBoxRelativeLanePosition( @@ -221,10 +223,12 @@ class SimulatorCore const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) -> traffic_simulator::LaneletPose { - const bool allow_lane_change = (routing_algorithm == RoutingAlgorithm::value_type::shortest); + traffic_simulator::RoutingConfiguration routing_configuration; + routing_configuration.allow_lane_change = + (routing_algorithm == RoutingAlgorithm::value_type::shortest); return traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from_lanelet_pose, from_bounding_box, to_lanelet_pose, to_bounding_box, allow_lane_change, - core->getHdmapUtils()); + from_lanelet_pose, from_bounding_box, to_lanelet_pose, to_bounding_box, + routing_configuration, core->getHdmapUtils()); } static auto makeNativeBoundingBoxRelativeWorldPosition( @@ -261,14 +265,15 @@ class SimulatorCore const std::string & from_entity_name, const std::string & to_entity_name, const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) -> int { + traffic_simulator::RoutingConfiguration routing_configuration; + routing_configuration.allow_lane_change = + (routing_algorithm == RoutingAlgorithm::value_type::shortest); if (const auto from_entity = core->getEntity(from_entity_name)) { if (const auto to_entity = core->getEntity(to_entity_name)) { - const bool allow_lane_change = - (routing_algorithm == RoutingAlgorithm::value_type::shortest); if ( auto lane_changes = traffic_simulator::distance::countLaneChanges( from_entity->getCanonicalizedLaneletPose().value(), - to_entity->getCanonicalizedLaneletPose().value(), allow_lane_change, + to_entity->getCanonicalizedLaneletPose().value(), routing_configuration, core->getHdmapUtils())) { return lane_changes.value().first - lane_changes.value().second; } diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp index a1d9d83b7bf..34d75b4e1f3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp @@ -41,7 +41,8 @@ class CanonicalizedLaneletPose auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; } auto getAlternativeLaneletPoseBaseOnShortestRouteFrom( LaneletPose from, const std::shared_ptr & hdmap_utils, - bool allow_lane_change = false) const -> std::optional; + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) const -> std::optional; static auto setConsiderPoseByRoadSlope(bool consider_pose_by_road_slope) -> void { consider_pose_by_road_slope_ = consider_pose_by_road_slope; diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_configuration.hpp new file mode 100644 index 00000000000..b67e630a4d7 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_configuration.hpp @@ -0,0 +1,44 @@ +// 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__DATA_TYPE__ROUTING_CONFIGURATIONS_HPP_ +#define TRAFFIC_SIMULATOR__DATA_TYPE__ROUTING_CONFIGURATIONS_HPP_ + +#include +#include + +namespace traffic_simulator +{ +struct RoutingConfiguration +{ + bool allow_lane_change = false; + traffic_simulator::RoutingGraphType routing_graph_type = + traffic_simulator::RoutingGraphType::VEHICLE; + + bool operator==(const RoutingConfiguration & routing_configuration) const + { + return allow_lane_change == routing_configuration.allow_lane_change && + routing_graph_type == routing_configuration.routing_graph_type; + } + + friend std::ostream & operator<<(std::ostream & os, const RoutingConfiguration & rc) + { + os << "{ allow_lane_change: " << (rc.allow_lane_change ? "true" : "false") + << ", routing_graph_type: " << to_string(rc.routing_graph_type) << " }"; + return os; + } +}; +} // namespace traffic_simulator + +#endif // TRAFFIC_SIMULATOR__DATA_TYPE__ROUTING_CONFIGURATIONS_HPP_ 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 78c975bbb44..9e6de237138 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 @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -65,7 +66,10 @@ class HdMapUtils public: explicit HdMapUtils(const boost::filesystem::path &, const geographic_msgs::msg::GeoPoint &); - auto canChangeLane(const lanelet::Id from, const lanelet::Id to) const -> bool; + auto canChangeLane( + const lanelet::Id from, const lanelet::Id to, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> bool; auto canonicalizeLaneletPose(const traffic_simulator_msgs::msg::LaneletPose &) const -> std::tuple< @@ -82,8 +86,9 @@ class HdMapUtils auto countLaneChanges( const traffic_simulator_msgs::msg::LaneletPose & from, - const traffic_simulator_msgs::msg::LaneletPose & to, bool allow_lane_change) const - -> std::optional>; + const traffic_simulator_msgs::msg::LaneletPose & to, + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) const -> std::optional>; auto filterLaneletIds(const lanelet::Ids &, const char subtype[]) const -> lanelet::Ids; @@ -93,8 +98,9 @@ class HdMapUtils -> std::vector; auto getAlongLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & from, const double along) const - -> traffic_simulator_msgs::msg::LaneletPose; + const traffic_simulator_msgs::msg::LaneletPose & from, const double along, + const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) + const -> traffic_simulator_msgs::msg::LaneletPose; auto getCenterPoints(const lanelet::Ids &) const -> std::vector; @@ -113,7 +119,9 @@ class HdMapUtils auto getConflictingCrosswalkIds(const lanelet::Ids &) const -> lanelet::Ids; - auto getConflictingLaneIds(const lanelet::Ids &) const -> lanelet::Ids; + auto getConflictingLaneIds( + const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; auto getDistanceToStopLine( const lanelet::Ids & route_lanelets, @@ -141,11 +149,14 @@ class HdMapUtils auto getFollowingLanelets( const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon = 100, - const bool include_current_lanelet_id = true) const -> lanelet::Ids; + const bool include_current_lanelet_id = true, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; auto getFollowingLanelets( - const lanelet::Id, const double distance = 100, const bool include_self = true) const - -> lanelet::Ids; + const lanelet::Id, const double distance = 100, const bool include_self = true, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; auto getHeight(const traffic_simulator_msgs::msg::LaneletPose &) const -> double; @@ -162,12 +173,14 @@ class HdMapUtils -> std::optional>; auto getLaneChangeableLaneletId( - const lanelet::Id, const traffic_simulator::lane_change::Direction) const - -> std::optional; + const lanelet::Id, const traffic_simulator::lane_change::Direction, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> std::optional; auto getLaneChangeableLaneletId( - const lanelet::Id, const traffic_simulator::lane_change::Direction, - const std::uint8_t shift) const -> std::optional; + const lanelet::Id, const traffic_simulator::lane_change::Direction, const std::uint8_t shift, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> std::optional; auto getLaneletIds() const -> lanelet::Ids; @@ -179,8 +192,9 @@ class HdMapUtils auto getLateralDistance( const traffic_simulator_msgs::msg::LaneletPose & from, - const traffic_simulator_msgs::msg::LaneletPose & to, bool allow_lane_change = false) const - -> std::optional; + const traffic_simulator_msgs::msg::LaneletPose & to, + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) const -> std::optional; auto getLeftBound(const lanelet::Id) const -> std::vector; @@ -191,7 +205,8 @@ class HdMapUtils auto getLongitudinalDistance( const traffic_simulator_msgs::msg::LaneletPose & from_pose, const traffic_simulator_msgs::msg::LaneletPose & to_pose, - const bool allow_lane_change = false) const -> std::optional; + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) const -> std::optional; auto getNearbyLaneletIds( const geometry_msgs::msg::Point &, const double distance_threshold, @@ -241,8 +256,10 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; - auto getPreviousLanelets(const lanelet::Id, const double backward_horizon = 100) const - -> lanelet::Ids; + auto getPreviousLanelets( + const lanelet::Id, const double backward_horizon = 100, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; auto getRightBound(const lanelet::Id) const -> std::vector; @@ -256,11 +273,13 @@ class HdMapUtils auto getRightOfWayLaneletIds(const lanelet::Id) const -> lanelet::Ids; auto getRoute( - const lanelet::Id from, const lanelet::Id to, bool allow_lane_change = false, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; + const lanelet::Id from, const lanelet::Id to, + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) const -> lanelet::Ids; - auto getSpeedLimit(const lanelet::Ids &) const -> double; + auto getSpeedLimit( + const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> double; auto getStopLineIds() const -> lanelet::Ids; @@ -299,10 +318,18 @@ class HdMapUtils auto isTrafficLightRegulatoryElement(const lanelet::Id) const -> bool; +private: + /// @note This value was determined experimentally by @hakuturu583 and not theoretically. + /// @sa https://github.com/tier4/scenario_simulator_v2/commit/4c8e9f496b061b00bec799159d59c33f2ba46b3a + constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; + +public: auto matchToLane( const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, const bool include_crosswalk, const double matching_distance = 1.0, - const double reduction_ratio = 0.8) const -> std::optional; + const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> std::optional; auto toLaneletPose( const geometry_msgs::msg::Pose &, const bool include_crosswalk, @@ -316,13 +343,15 @@ class HdMapUtils auto toLaneletPose( const geometry_msgs::msg::Point &, const traffic_simulator_msgs::msg::BoundingBox &, - const bool include_crosswalk, const double matching_distance = 1.0) const - -> std::optional; + const bool include_crosswalk, const double matching_distance = 1.0, + const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) + const -> std::optional; auto toLaneletPose( const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, - const bool include_crosswalk, const double matching_distance = 1.0) const - -> std::optional; + const bool include_crosswalk, const double matching_distance = 1.0, + const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) + const -> std::optional; auto toLaneletPose( const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 1.0) const @@ -330,8 +359,9 @@ class HdMapUtils auto toLaneletPoses( const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 5.0, - const bool include_opposite_direction = true) const - -> std::vector; + const bool include_opposite_direction = true, + const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) + const -> std::vector; auto toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin; @@ -372,8 +402,9 @@ class HdMapUtils [[nodiscard]] auto getRoute( const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, - lanelet::LaneletMapPtr lanelet_map_ptr, const bool allow_lane_change, - const traffic_simulator::RoutingGraphType type) -> lanelet::Ids; + lanelet::LaneletMapPtr lanelet_map_ptr, + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) -> lanelet::Ids; private: [[nodiscard]] RouteCache & route_cache(const traffic_simulator::RoutingGraphType type); diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 80b1a287e88..7919a140491 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -25,24 +25,26 @@ inline namespace distance // Lateral auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional; + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> std::optional; auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - double matching_distance, bool allow_lane_change, + double matching_distance, const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; // Lateral (unit: lanes) auto countLaneChanges( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> std::optional>; // Longitudinal auto longitudinalDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool include_adjacent_lanelet, bool include_opposite_direction, bool allow_lane_change, + bool include_adjacent_lanelet, bool include_opposite_direction, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; // BoundingBox @@ -56,7 +58,8 @@ auto boundingBoxLaneLateralDistance( const CanonicalizedLaneletPose & from, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, - const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool allow_lane_change, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; auto boundingBoxLaneLongitudinalDistance( @@ -64,7 +67,8 @@ auto boundingBoxLaneLongitudinalDistance( const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool include_adjacent_lanelet, - bool include_opposite_direction, bool allow_lane_change, + bool include_opposite_direction, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; // Bounds diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 37428420fde..9dc3b8053f8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -80,14 +80,15 @@ auto boundingBoxRelativePose( // Relative LaneletPose auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) - -> LaneletPose; + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; auto boundingBoxRelativeLaneletPose( const CanonicalizedLaneletPose & from, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, - const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const bool allow_lane_change, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; // Others diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp index d9b37d19eb7..b4dfb4dc0d8 100644 --- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp +++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp @@ -111,17 +111,18 @@ auto CanonicalizedLaneletPose::canonicalize( auto CanonicalizedLaneletPose::getAlternativeLaneletPoseBaseOnShortestRouteFrom( LaneletPose from, const std::shared_ptr & hdmap_utils, - bool allow_lane_change) const -> std::optional + const traffic_simulator::RoutingConfiguration & routing_configuration) const + -> std::optional { if (lanelet_poses_.empty()) { return std::nullopt; } lanelet::Ids shortest_route = - hdmap_utils->getRoute(from.lanelet_id, lanelet_poses_[0].lanelet_id); + hdmap_utils->getRoute(from.lanelet_id, lanelet_poses_[0].lanelet_id, routing_configuration); LaneletPose alternative_lanelet_pose = lanelet_poses_[0]; for (const auto & laneletPose : lanelet_poses_) { const auto route = - hdmap_utils->getRoute(from.lanelet_id, laneletPose.lanelet_id, allow_lane_change); + hdmap_utils->getRoute(from.lanelet_id, laneletPose.lanelet_id, routing_configuration); if (shortest_route.size() > route.size()) { shortest_route = route; alternative_lanelet_pose = laneletPose; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 5537f99c48e..4a283710e8d 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -714,8 +715,12 @@ auto EntityBase::requestSynchronize( "If so please contact the developer since there might be an undiscovered bug."); } + RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto entity_distance = longitudinalDistance( - entity_lanelet_pose.value(), entity_target, true, true, true, hdmap_utils_ptr_); + entity_lanelet_pose.value(), entity_target, true, true, + lane_changeable_routing_configuration, hdmap_utils_ptr_); if (!entity_distance.has_value()) { THROW_SEMANTIC_ERROR( "Failed to get distance between entity and target lanelet pose. Check if the entity has " @@ -730,7 +735,7 @@ auto EntityBase::requestSynchronize( const auto target_entity_distance = longitudinalDistance( CanonicalizedLaneletPose(target_entity_lanelet_pose, hdmap_utils_ptr_), target_sync_pose, - true, true, true, hdmap_utils_ptr_); + true, true, lane_changeable_routing_configuration, hdmap_utils_ptr_); if (!target_entity_distance.has_value() || target_entity_distance.value() < 0.0) { RCLCPP_WARN_ONCE( rclcpp::get_logger("traffic_simulator"), diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 4a66bbe6eb7..e61e479cb19 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -205,10 +205,11 @@ auto HdMapUtils::canonicalizeLaneletPose( auto HdMapUtils::countLaneChanges( const traffic_simulator_msgs::msg::LaneletPose & from, - const traffic_simulator_msgs::msg::LaneletPose & to, bool allow_lane_change) const + const traffic_simulator_msgs::msg::LaneletPose & to, + const traffic_simulator::RoutingConfiguration & routing_configuration) const -> std::optional> { - const auto route = getRoute(from.lanelet_id, to.lanelet_id, allow_lane_change); + const auto route = getRoute(from.lanelet_id, to.lanelet_id, routing_configuration); if (route.empty()) { return std::nullopt; } else { @@ -217,13 +218,13 @@ auto HdMapUtils::countLaneChanges( const auto & previous = route[i - 1]; const auto & current = route[i]; - if (auto followings = getNextLaneletIds(previous); + if (auto followings = getNextLaneletIds(previous, routing_configuration.routing_graph_type); std::find(followings.begin(), followings.end(), current) == followings.end()) { - if (auto lefts = getLeftLaneletIds(previous, traffic_simulator::RoutingGraphType::VEHICLE); + if (auto lefts = getLeftLaneletIds(previous, routing_configuration.routing_graph_type); std::find(lefts.begin(), lefts.end(), current) != lefts.end()) { lane_changes.first++; } else if (auto rights = - getRightLaneletIds(previous, traffic_simulator::RoutingGraphType::VEHICLE); + getRightLaneletIds(previous, routing_configuration.routing_graph_type); std::find(rights.begin(), rights.end(), current) != rights.end()) { lane_changes.second++; } @@ -386,13 +387,15 @@ auto HdMapUtils::getCollisionPointInLaneCoordinate( return std::nullopt; } -auto HdMapUtils::getConflictingLaneIds(const lanelet::Ids & lanelet_ids) const -> lanelet::Ids +auto HdMapUtils::getConflictingLaneIds( + const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const + -> lanelet::Ids { lanelet::Ids ids; for (const auto & lanelet_id : lanelet_ids) { const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - const auto conflicting_lanelets = lanelet::utils::getConflictingLanelets( - routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE), lanelet); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graphs_->routing_graph(type), lanelet); for (const auto & conflicting_lanelet : conflicting_lanelets) { ids.emplace_back(conflicting_lanelet.id()); } @@ -521,8 +524,8 @@ auto HdMapUtils::toPoint2d(const geometry_msgs::msg::Point & point) const -> lan auto HdMapUtils::matchToLane( const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double matching_distance, const double reduction_ratio) const - -> std::optional + const bool include_crosswalk, const double matching_distance, const double reduction_ratio, + const traffic_simulator::RoutingGraphType type) const -> std::optional { lanelet::matching::Object2d obj; obj.pose.translation() = toPoint2d(pose.position); @@ -542,7 +545,7 @@ auto HdMapUtils::matchToLane( lanelet::matching::getDeterministicMatches(*lanelet_map_ptr_, obj, matching_distance); if (!include_crosswalk) { matches = lanelet::matching::removeNonRuleCompliantMatches( - matches, routing_graphs_->traffic_rule(traffic_simulator::RoutingGraphType::VEHICLE)); + matches, routing_graphs_->traffic_rule(type)); } if (matches.empty()) { return std::nullopt; @@ -626,21 +629,24 @@ auto HdMapUtils::toLaneletPose( auto HdMapUtils::toLaneletPose( const geometry_msgs::msg::Point & position, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double matching_distance) const + const bool include_crosswalk, const double matching_distance, + const traffic_simulator::RoutingGraphType type) const -> std::optional { return toLaneletPose( geometry_msgs::build().position(position).orientation( geometry_msgs::build().x(0).y(0).z(0).w(1)), - bbox, include_crosswalk, matching_distance); + bbox, include_crosswalk, matching_distance, type); } auto HdMapUtils::toLaneletPose( const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double matching_distance) const + const bool include_crosswalk, const double matching_distance, + const traffic_simulator::RoutingGraphType type) const -> std::optional { - const auto lanelet_id = matchToLane(pose, bbox, include_crosswalk, matching_distance); + const auto lanelet_id = matchToLane( + pose, bbox, include_crosswalk, matching_distance, DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, type); if (!lanelet_id) { return toLaneletPose(pose, include_crosswalk, matching_distance); } @@ -648,36 +654,35 @@ auto HdMapUtils::toLaneletPose( if (pose_in_target_lanelet) { return pose_in_target_lanelet; } - const auto previous = getPreviousLaneletIds(lanelet_id.value()); + const auto previous = getPreviousLaneletIds(lanelet_id.value(), type); for (const auto id : previous) { const auto pose_in_previous = toLaneletPose(pose, id, matching_distance); if (pose_in_previous) { return pose_in_previous; } } - const auto next = getNextLaneletIds(lanelet_id.value()); + const auto next = getNextLaneletIds(lanelet_id.value(), type); for (const auto id : previous) { const auto pose_in_next = toLaneletPose(pose, id, matching_distance); if (pose_in_next) { return pose_in_next; } } - return toLaneletPose(pose, include_crosswalk); + return toLaneletPose(pose, include_crosswalk, matching_distance); } auto HdMapUtils::toLaneletPoses( const geometry_msgs::msg::Pose & pose, const lanelet::Id lanelet_id, - const double matching_distance, const bool include_opposite_direction) const + const double matching_distance, const bool include_opposite_direction, + const traffic_simulator::RoutingGraphType type) const -> std::vector { std::vector ret; std::vector lanelet_ids = {lanelet_id}; - lanelet_ids += getLeftLaneletIds( - lanelet_id, traffic_simulator::RoutingGraphType::VEHICLE, include_opposite_direction); - lanelet_ids += getRightLaneletIds( - lanelet_id, traffic_simulator::RoutingGraphType::VEHICLE, include_opposite_direction); - lanelet_ids += getPreviousLaneletIds(lanelet_ids); - lanelet_ids += getNextLaneletIds(lanelet_ids); + lanelet_ids += getLeftLaneletIds(lanelet_id, type, include_opposite_direction); + lanelet_ids += getRightLaneletIds(lanelet_id, type, include_opposite_direction); + lanelet_ids += getPreviousLaneletIds(lanelet_ids, type); + lanelet_ids += getNextLaneletIds(lanelet_ids, type); for (const auto & id : sortAndUnique(lanelet_ids)) { if (const auto & lanelet_pose = toLaneletPose(pose, id, matching_distance)) { ret.emplace_back(lanelet_pose.value()); @@ -718,7 +723,8 @@ auto HdMapUtils::getClosestLaneletId( } } -auto HdMapUtils::getSpeedLimit(const lanelet::Ids & lanelet_ids) const -> double +auto HdMapUtils::getSpeedLimit( + const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const -> double { std::vector limits; if (lanelet_ids.empty()) { @@ -726,8 +732,7 @@ auto HdMapUtils::getSpeedLimit(const lanelet::Ids & lanelet_ids) const -> double } for (auto itr = lanelet_ids.begin(); itr != lanelet_ids.end(); itr++) { const auto lanelet = lanelet_map_ptr_->laneletLayer.get(*itr); - const auto limit = routing_graphs_->traffic_rule(traffic_simulator::RoutingGraphType::VEHICLE) - ->speedLimit(lanelet); + const auto limit = routing_graphs_->traffic_rule(type)->speedLimit(lanelet); limits.push_back(lanelet::units::KmHQuantity(limit.speedLimit).value() / 3.6); } return *std::min_element(limits.begin(), limits.end()); @@ -735,15 +740,16 @@ auto HdMapUtils::getSpeedLimit(const lanelet::Ids & lanelet_ids) const -> double auto HdMapUtils::getLaneChangeableLaneletId( const lanelet::Id lanelet_id, const traffic_simulator::lane_change::Direction direction, - const std::uint8_t shift) const -> std::optional + const std::uint8_t shift, const traffic_simulator::RoutingGraphType type) const + -> std::optional { if (shift == 0) { return getLaneChangeableLaneletId( - lanelet_id, traffic_simulator::lane_change::Direction::STRAIGHT); + lanelet_id, traffic_simulator::lane_change::Direction::STRAIGHT, type); } else { auto reference_id = lanelet_id; for (uint8_t i = 0; i < shift; i++) { - auto id = getLaneChangeableLaneletId(reference_id, direction); + auto id = getLaneChangeableLaneletId(reference_id, direction, type); if (!id) { return std::nullopt; } else { @@ -758,8 +764,8 @@ auto HdMapUtils::getLaneChangeableLaneletId( } auto HdMapUtils::getLaneChangeableLaneletId( - const lanelet::Id lanelet_id, const traffic_simulator::lane_change::Direction direction) const - -> std::optional + const lanelet::Id lanelet_id, const traffic_simulator::lane_change::Direction direction, + const traffic_simulator::RoutingGraphType type) const -> std::optional { const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); std::optional target = std::nullopt; @@ -768,19 +774,13 @@ auto HdMapUtils::getLaneChangeableLaneletId( target = lanelet.id(); break; case traffic_simulator::lane_change::Direction::LEFT: - if (routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) - ->left(lanelet)) { - target = routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) - ->left(lanelet) - ->id(); + if (routing_graphs_->routing_graph(type)->left(lanelet)) { + target = routing_graphs_->routing_graph(type)->left(lanelet)->id(); } break; case traffic_simulator::lane_change::Direction::RIGHT: - if (routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) - ->right(lanelet)) { - target = routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) - ->right(lanelet) - ->id(); + if (routing_graphs_->routing_graph(type)->right(lanelet)) { + target = routing_graphs_->routing_graph(type)->right(lanelet)->id(); } break; } @@ -788,18 +788,20 @@ auto HdMapUtils::getLaneChangeableLaneletId( } auto HdMapUtils::getPreviousLanelets( - const lanelet::Id current_lanelet_id, const double backward_horizon) const -> lanelet::Ids + const lanelet::Id current_lanelet_id, const double backward_horizon, + const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids { lanelet::Ids previous_lanelets_ids; double total_distance = 0.0; previous_lanelets_ids.push_back(current_lanelet_id); while (total_distance < backward_horizon) { const auto & reference_lanelet_id = previous_lanelets_ids.back(); - if (const auto straight_lanelet_ids = getPreviousLaneletIds(reference_lanelet_id, "straight"); + if (const auto straight_lanelet_ids = + getPreviousLaneletIds(reference_lanelet_id, "straight", type); not straight_lanelet_ids.empty()) { total_distance = total_distance + getLaneletLength(straight_lanelet_ids[0]); previous_lanelets_ids.push_back(straight_lanelet_ids[0]); - } else if (auto non_straight_lanelet_ids = getPreviousLaneletIds(reference_lanelet_id); + } else if (auto non_straight_lanelet_ids = getPreviousLaneletIds(reference_lanelet_id, type); not non_straight_lanelet_ids.empty()) { total_distance = total_distance + getLaneletLength(non_straight_lanelet_ids[0]); previous_lanelets_ids.push_back(non_straight_lanelet_ids[0]); @@ -817,11 +819,12 @@ auto HdMapUtils::isInRoute(const lanelet::Id lanelet_id, const lanelet::Ids & ro auto HdMapUtils::getFollowingLanelets( const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon, - const bool include_current_lanelet_id) const -> lanelet::Ids + const bool include_current_lanelet_id, const traffic_simulator::RoutingGraphType type) const + -> lanelet::Ids { const auto is_following_lanelet = - [this](const auto & current_lanelet, const auto & candidate_lanelet) { - const auto next_ids = getNextLaneletIds(current_lanelet); + [this, type](const auto & current_lanelet, const auto & candidate_lanelet) { + const auto next_ids = getNextLaneletIds(current_lanelet, type); return std::find(next_ids.cbegin(), next_ids.cend(), candidate_lanelet) != next_ids.cend(); }; @@ -858,7 +861,7 @@ auto HdMapUtils::getFollowingLanelets( THROW_SEMANTIC_ERROR("lanelet id does not match"); } else if (total_distance <= horizon) { const auto remaining_lanelets = - getFollowingLanelets(route.back(), horizon - total_distance, false); + getFollowingLanelets(route.back(), horizon - total_distance, false, type); following_lanelets_ids.insert( following_lanelets_ids.end(), remaining_lanelets.begin(), remaining_lanelets.end()); } @@ -867,8 +870,8 @@ auto HdMapUtils::getFollowingLanelets( } auto HdMapUtils::getFollowingLanelets( - const lanelet::Id lanelet_id, const double distance, const bool include_self) const - -> lanelet::Ids + const lanelet::Id lanelet_id, const double distance, const bool include_self, + const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids { lanelet::Ids ret; double total_distance = 0.0; @@ -877,13 +880,13 @@ auto HdMapUtils::getFollowingLanelets( } lanelet::Id end_lanelet_id = lanelet_id; while (total_distance < distance) { - if (const auto straight_ids = getNextLaneletIds(end_lanelet_id, "straight"); + if (const auto straight_ids = getNextLaneletIds(end_lanelet_id, "straight", type); !straight_ids.empty()) { total_distance = total_distance + getLaneletLength(straight_ids[0]); ret.push_back(straight_ids[0]); end_lanelet_id = straight_ids[0]; continue; - } else if (const auto ids = getNextLaneletIds(end_lanelet_id); ids.size() != 0) { + } else if (const auto ids = getNextLaneletIds(end_lanelet_id, type); ids.size() != 0) { total_distance = total_distance + getLaneletLength(ids[0]); ret.push_back(ids[0]); end_lanelet_id = ids[0]; @@ -896,11 +899,11 @@ auto HdMapUtils::getFollowingLanelets( } auto HdMapUtils::getRoute( - const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, bool allow_lane_change, - const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, + const traffic_simulator::RoutingConfiguration & routing_configuration) const -> lanelet::Ids { return routing_graphs_->getRoute( - from_lanelet_id, to_lanelet_id, lanelet_map_ptr_, allow_lane_change, type); + from_lanelet_id, to_lanelet_id, lanelet_map_ptr_, routing_configuration); } auto HdMapUtils::getCenterPointsSpline(const lanelet::Id lanelet_id) const @@ -1125,16 +1128,16 @@ auto HdMapUtils::getTrafficLightBulbPosition( } auto HdMapUtils::getAlongLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & from_pose, const double along) const - -> traffic_simulator_msgs::msg::LaneletPose + const traffic_simulator_msgs::msg::LaneletPose & from_pose, const double along, + const traffic_simulator::RoutingGraphType type) const -> traffic_simulator_msgs::msg::LaneletPose { traffic_simulator_msgs::msg::LaneletPose along_pose = from_pose; along_pose.s = along_pose.s + along; if (along_pose.s >= 0) { while (along_pose.s >= getLaneletLength(along_pose.lanelet_id)) { - auto next_ids = getNextLaneletIds(along_pose.lanelet_id, "straight"); + auto next_ids = getNextLaneletIds(along_pose.lanelet_id, "straight", type); if (next_ids.empty()) { - next_ids = getNextLaneletIds(along_pose.lanelet_id); + next_ids = getNextLaneletIds(along_pose.lanelet_id, type); if (next_ids.empty()) { THROW_SEMANTIC_ERROR( "failed to calculate along pose (id,s) = (", from_pose.lanelet_id, ",", @@ -1146,9 +1149,9 @@ auto HdMapUtils::getAlongLaneletPose( } } else { while (along_pose.s < 0) { - auto previous_ids = getPreviousLaneletIds(along_pose.lanelet_id, "straight"); + auto previous_ids = getPreviousLaneletIds(along_pose.lanelet_id, "straight", type); if (previous_ids.empty()) { - previous_ids = getPreviousLaneletIds(along_pose.lanelet_id); + previous_ids = getPreviousLaneletIds(along_pose.lanelet_id, type); if (previous_ids.empty()) { THROW_SEMANTIC_ERROR( "failed to calculate along pose (id,s) = (", from_pose.lanelet_id, ",", @@ -1401,27 +1404,28 @@ auto HdMapUtils::getTangentVector(const lanelet::Id lanelet_id, const double s) } auto HdMapUtils::canChangeLane( - const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id) const -> bool + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, + const traffic_simulator::RoutingGraphType type) const -> bool { const auto from_lanelet = lanelet_map_ptr_->laneletLayer.get(from_lanelet_id); const auto to_lanelet = lanelet_map_ptr_->laneletLayer.get(to_lanelet_id); - return routing_graphs_->traffic_rule(traffic_simulator::RoutingGraphType::VEHICLE) - ->canChangeLane(from_lanelet, to_lanelet); + return routing_graphs_->traffic_rule(type)->canChangeLane(from_lanelet, to_lanelet); } auto HdMapUtils::getLateralDistance( const traffic_simulator_msgs::msg::LaneletPose & from, - const traffic_simulator_msgs::msg::LaneletPose & to, bool allow_lane_change) const + const traffic_simulator_msgs::msg::LaneletPose & to, + const traffic_simulator::RoutingConfiguration & routing_configuration) const -> std::optional { - const auto route = getRoute(from.lanelet_id, to.lanelet_id, allow_lane_change); + const auto route = getRoute(from.lanelet_id, to.lanelet_id, routing_configuration); if (route.empty()) { return std::nullopt; } - if (allow_lane_change) { + if (routing_configuration.allow_lane_change) { double lateral_distance_by_lane_change = 0.0; for (unsigned int i = 0; i < route.size() - 1; i++) { - auto next_lanelet_ids = getNextLaneletIds(route[i]); + auto next_lanelet_ids = getNextLaneletIds(route[i], routing_configuration.routing_graph_type); if (auto next_lanelet = std::find_if( next_lanelet_ids.begin(), next_lanelet_ids.end(), [&route, i](const lanelet::Id & id) { return id == route[i + 1]; }); @@ -1457,18 +1461,21 @@ auto HdMapUtils::getLateralDistance( auto HdMapUtils::getLongitudinalDistance( const traffic_simulator_msgs::msg::LaneletPose & from_pose, const traffic_simulator_msgs::msg::LaneletPose & to_pose, - const bool allow_lane_change /*= false*/) const -> std::optional + const traffic_simulator::RoutingConfiguration & routing_configuration) const + -> std::optional { - const auto is_lane_change_required = - [this](const lanelet::Id current_lanelet, const lanelet::Id next_lanelet) -> bool { - const auto next_lanelet_ids = getNextLaneletIds(current_lanelet); + const auto is_lane_change_required = [this, routing_configuration]( + const lanelet::Id current_lanelet, + const lanelet::Id next_lanelet) -> bool { + const auto next_lanelet_ids = + getNextLaneletIds(current_lanelet, routing_configuration.routing_graph_type); return std::none_of( next_lanelet_ids.cbegin(), next_lanelet_ids.cend(), [next_lanelet](const lanelet::Id id) { return id == next_lanelet; }); }; /// @sa https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/DistanceCalculation/ - if (const auto route = getRoute(from_pose.lanelet_id, to_pose.lanelet_id, allow_lane_change); + if (const auto route = getRoute(from_pose.lanelet_id, to_pose.lanelet_id, routing_configuration); not route.empty() || from_pose.lanelet_id == to_pose.lanelet_id) { /// @note horizontal bar must intersect with the lanelet spline, so the distance was set arbitrarily to 10 meters. static constexpr double matching_distance = 10.0; @@ -2222,31 +2229,32 @@ RouteCache & HdMapUtils::RoutingGraphs::route_cache(const traffic_simulator::Rou auto HdMapUtils::RoutingGraphs::getRoute( const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, - lanelet::LaneletMapPtr lanelet_map_ptr, bool allow_lane_change, - const traffic_simulator::RoutingGraphType type) -> lanelet::Ids + lanelet::LaneletMapPtr lanelet_map_ptr, + const traffic_simulator::RoutingConfiguration & routing_configuration) -> lanelet::Ids { - auto & cache = route_cache(type); - if (cache.exists(from_lanelet_id, to_lanelet_id, allow_lane_change)) { - return cache.getRoute(from_lanelet_id, to_lanelet_id, allow_lane_change); + auto & cache = route_cache(routing_configuration.routing_graph_type); + if (cache.exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) { + return cache.getRoute(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change); } lanelet::Ids ids; const auto lanelet = lanelet_map_ptr->laneletLayer.get(from_lanelet_id); const auto to_lanelet = lanelet_map_ptr->laneletLayer.get(to_lanelet_id); lanelet::Optional route = - routing_graph(type)->getRoute(lanelet, to_lanelet, 0, allow_lane_change); + routing_graph(routing_configuration.routing_graph_type) + ->getRoute(lanelet, to_lanelet, 0, routing_configuration.allow_lane_change); if (!route) { - cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); + cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); return ids; } lanelet::routing::LaneletPath shortest_path = route->shortestPath(); if (shortest_path.empty()) { - cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); + cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); return ids; } for (auto lane_itr = shortest_path.begin(); lane_itr != shortest_path.end(); lane_itr++) { ids.push_back(lane_itr->id()); } - cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); + cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); return ids; } diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index a8d9b6175f3..98bac102b88 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -24,22 +24,22 @@ inline namespace distance { auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> std::optional { return hdmap_utils_ptr->getLateralDistance( - static_cast(from), static_cast(to), allow_lane_change); + static_cast(from), static_cast(to), routing_configuration); } auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - double matching_distance, bool allow_lane_change, + double matching_distance, const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional { if ( std::abs(static_cast(from).offset) <= matching_distance && std::abs(static_cast(to).offset) <= matching_distance) { - return lateralDistance(from, to, allow_lane_change, hdmap_utils_ptr); + return lateralDistance(from, to, routing_configuration, hdmap_utils_ptr); } else { return std::nullopt; } @@ -47,17 +47,19 @@ auto lateralDistance( auto countLaneChanges( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> std::optional> { return hdmap_utils_ptr->countLaneChanges( - static_cast(from), static_cast(to), allow_lane_change); + static_cast(from), static_cast(to), routing_configuration); } /// @sa https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md auto longitudinalDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool include_adjacent_lanelet, bool include_opposite_direction, bool allow_lane_change, + bool include_adjacent_lanelet, bool include_opposite_direction, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional { if (!include_adjacent_lanelet) { @@ -65,16 +67,16 @@ auto longitudinalDistance( if (to.hasAlternativeLaneletPose()) { if ( const auto to_canonicalized_opt = to.getAlternativeLaneletPoseBaseOnShortestRouteFrom( - static_cast(from), hdmap_utils_ptr, allow_lane_change)) { + static_cast(from), hdmap_utils_ptr, routing_configuration)) { to_canonicalized = to_canonicalized_opt.value(); } } const auto forward_distance = hdmap_utils_ptr->getLongitudinalDistance( - static_cast(from), to_canonicalized, allow_lane_change); + static_cast(from), to_canonicalized, routing_configuration); const auto backward_distance = hdmap_utils_ptr->getLongitudinalDistance( - to_canonicalized, static_cast(from), allow_lane_change); + to_canonicalized, static_cast(from), routing_configuration); if (forward_distance && backward_distance) { return forward_distance.value() > backward_distance.value() ? -backward_distance.value() @@ -97,12 +99,12 @@ auto longitudinalDistance( auto from_poses = hdmap_utils_ptr->toLaneletPoses( static_cast(from), static_cast(from).lanelet_id, - matching_distance, include_opposite_direction); + matching_distance, include_opposite_direction, routing_configuration.routing_graph_type); from_poses.emplace_back(from); auto to_poses = hdmap_utils_ptr->toLaneletPoses( static_cast(to), static_cast(to).lanelet_id, - matching_distance, include_opposite_direction); + matching_distance, include_opposite_direction, routing_configuration.routing_graph_type); to_poses.emplace_back(to); std::vector distances = {}; @@ -112,7 +114,7 @@ auto longitudinalDistance( const auto distance = longitudinalDistance( CanonicalizedLaneletPose(from_pose, hdmap_utils_ptr), CanonicalizedLaneletPose(to_pose, hdmap_utils_ptr), false, include_opposite_direction, - allow_lane_change, hdmap_utils_ptr)) { + routing_configuration, hdmap_utils_ptr)) { distances.emplace_back(distance.value()); } } @@ -141,10 +143,12 @@ auto boundingBoxLaneLateralDistance( const CanonicalizedLaneletPose & from, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, - const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool allow_lane_change, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional { - if (const auto lateral_distance = lateralDistance(from, to, allow_lane_change, hdmap_utils_ptr); + if (const auto lateral_distance = + lateralDistance(from, to, routing_configuration, hdmap_utils_ptr); lateral_distance) { const auto from_bounding_box_distances = math::geometry::getDistancesFromCenterToEdge(from_bounding_box); @@ -168,11 +172,12 @@ auto boundingBoxLaneLongitudinalDistance( const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool include_adjacent_lanelet, - bool include_opposite_direction, bool allow_lane_change, + bool include_opposite_direction, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional { if (const auto longitudinal_distance = longitudinalDistance( - from, to, include_adjacent_lanelet, include_opposite_direction, allow_lane_change, + from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils_ptr); longitudinal_distance) { const auto from_bounding_box_distances = diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 36cf69ca25a..b354ea0ea0f 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -177,8 +177,8 @@ auto boundingBoxRelativePose( auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) - -> LaneletPose + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose { constexpr bool include_adjacent_lanelet{false}; constexpr bool include_opposite_direction{true}; @@ -188,11 +188,13 @@ auto relativeLaneletPose( // it is not possible to calculate one of them - it happens that one is sufficient if ( const auto longitudinal_distance = longitudinalDistance( - from, to, include_adjacent_lanelet, include_opposite_direction, allow_lane_change, + from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils_ptr)) { position.s = longitudinal_distance.value(); } - if (const auto lateral_distance = lateralDistance(from, to, allow_lane_change, hdmap_utils_ptr)) { + if ( + const auto lateral_distance = + lateralDistance(from, to, routing_configuration, hdmap_utils_ptr)) { position.offset = lateral_distance.value(); } return position; @@ -202,7 +204,8 @@ auto boundingBoxRelativeLaneletPose( const CanonicalizedLaneletPose & from, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, - const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const bool allow_lane_change, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose { constexpr bool include_adjacent_lanelet{false}; @@ -214,12 +217,12 @@ auto boundingBoxRelativeLaneletPose( if ( const auto longitudinal_bounding_box_distance = boundingBoxLaneLongitudinalDistance( from, from_bounding_box, to, to_bounding_box, include_adjacent_lanelet, - include_opposite_direction, allow_lane_change, hdmap_utils_ptr)) { + include_opposite_direction, routing_configuration, hdmap_utils_ptr)) { position.s = longitudinal_bounding_box_distance.value(); } if ( const auto lateral_bounding_box_distance = boundingBoxLaneLateralDistance( - from, from_bounding_box, to, to_bounding_box, allow_lane_change, hdmap_utils_ptr)) { + from, from_bounding_box, to, to_bounding_box, routing_configuration, hdmap_utils_ptr)) { position.offset = lateral_bounding_box_distance.value(); } return position; @@ -231,7 +234,7 @@ auto isInLanelet( { constexpr bool include_adjacent_lanelet{false}; constexpr bool include_opposite_direction{false}; - constexpr bool allow_lane_change{false}; + constexpr RoutingConfiguration routing_configuration; if (isSameLaneletId(canonicalized_lanelet_pose, lanelet_id)) { return true; @@ -240,7 +243,7 @@ auto isInLanelet( helper::constructCanonicalizedLaneletPose(lanelet_id, 0.0, 0.0, hdmap_utils_ptr); if (const auto distance_to_start_lanelet_pose = longitudinalDistance( start_lanelet_pose, canonicalized_lanelet_pose, include_adjacent_lanelet, - include_opposite_direction, allow_lane_change, hdmap_utils_ptr); + include_opposite_direction, routing_configuration, hdmap_utils_ptr); distance_to_start_lanelet_pose and std::abs(distance_to_start_lanelet_pose.value()) <= tolerance) { return true; @@ -250,7 +253,7 @@ auto isInLanelet( lanelet_id, hdmap_utils_ptr->getLaneletLength(lanelet_id), 0.0, hdmap_utils_ptr); if (const auto distance_to_end_lanelet_pose = longitudinalDistance( canonicalized_lanelet_pose, end_lanelet_pose, include_adjacent_lanelet, - include_opposite_direction, allow_lane_change, hdmap_utils_ptr); + include_opposite_direction, routing_configuration, hdmap_utils_ptr); distance_to_end_lanelet_pose and std::abs(distance_to_end_lanelet_pose.value()) <= tolerance) { return true; 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 904eaf66b0f..f161f368129 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 @@ -487,37 +487,47 @@ TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAll) TEST_F(HdMapUtilsTest_FourTrackHighwayMap, CountLaneChangesAlongRoute) { using traffic_simulator::helper::constructLaneletPose; + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(3002175, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(3002175, 0), + lane_changeable_routing_configuration), std::make_pair(1, 0)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(3002182, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(3002182, 0), + lane_changeable_routing_configuration), std::make_pair(1, 0)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(199, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(199, 0), + lane_changeable_routing_configuration), std::make_pair(1, 0)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(3002176, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(3002176, 0), + lane_changeable_routing_configuration), std::make_pair(0, 0)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(200, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(200, 0), + lane_changeable_routing_configuration), std::make_pair(0, 0)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(201, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(201, 0), + lane_changeable_routing_configuration), std::make_pair(0, 1)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(202, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(202, 0), + lane_changeable_routing_configuration), std::make_pair(0, 2)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(206, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(206, 0), + lane_changeable_routing_configuration), std::make_pair(0, 2)); } @@ -1724,7 +1734,7 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_parallelLanesCanNo EXPECT_FALSE(hdmap_utils .getLateralDistance( traffic_simulator::helper::constructLaneletPose(3002185, 0.0, 0.5), - traffic_simulator::helper::constructLaneletPose(3002184, 10.0, 0.2), false) + traffic_simulator::helper::constructLaneletPose(3002184, 10.0, 0.2)) .has_value()); } @@ -1738,7 +1748,10 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_parallelLanesCanCh const auto from = traffic_simulator::helper::constructLaneletPose(3002185, 0.0, 0.5); const auto to = traffic_simulator::helper::constructLaneletPose(3002184, 10.0, 0.2); - const auto result = hdmap_utils.getLateralDistance(from, to, true); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto result = + hdmap_utils.getLateralDistance(from, to, lane_changeable_routing_configuration); EXPECT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 2.80373 / 2.0 + 3.03463 / 2.0 + to.offset - from.offset, 1e-3); @@ -1753,10 +1766,13 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_parallelLanesCanCh */ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_notConnected) { + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; EXPECT_FALSE(hdmap_utils .getLateralDistance( traffic_simulator::helper::constructLaneletPose(3002185, 0.0, 0.5), - traffic_simulator::helper::constructLaneletPose(3002166, 10.0, 0.2), true) + traffic_simulator::helper::constructLaneletPose(3002166, 10.0, 0.2), + lane_changeable_routing_configuration) .has_value()); } @@ -1766,8 +1782,10 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_notConnected) */ TEST_F(HdMapUtilsTest_StandardMap, getRoute_correct) { + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; EXPECT_EQ( - hdmap_utils.getRoute(34579, 34630, true), + hdmap_utils.getRoute(34579, 34630, lane_changeable_routing_configuration), (lanelet::Ids{34579, 34774, 120659, 120660, 34468, 34438, 34408, 34624, 34630})); } @@ -1780,11 +1798,12 @@ TEST_F(HdMapUtilsTest_StandardMap, getRoute_correctCache) { const lanelet::Id from_id = 34579; const lanelet::Id to_id = 34630; - const bool allow_lane_change = true; + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; EXPECT_EQ( - hdmap_utils.getRoute(from_id, to_id, allow_lane_change), - hdmap_utils.getRoute(from_id, to_id, allow_lane_change)); + hdmap_utils.getRoute(from_id, to_id, lane_changeable_routing_configuration), + hdmap_utils.getRoute(from_id, to_id, lane_changeable_routing_configuration)); } /** @@ -1794,7 +1813,11 @@ TEST_F(HdMapUtilsTest_StandardMap, getRoute_correctCache) */ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getRoute_impossibleRouting) { - EXPECT_EQ(hdmap_utils.getRoute(199, 196, true).size(), static_cast(0)); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + EXPECT_EQ( + hdmap_utils.getRoute(199, 196, lane_changeable_routing_configuration).size(), + static_cast(0)); } /** @@ -1807,7 +1830,8 @@ TEST_F(HdMapUtilsTest_StandardMap, getRoute_circular) const lanelet::Id from_and_to_id = 120659; EXPECT_EQ( - hdmap_utils.getRoute(from_and_to_id, from_and_to_id, false), lanelet::Ids{from_and_to_id}); + hdmap_utils.getRoute(from_and_to_id, from_and_to_id, traffic_simulator::RoutingConfiguration()), + lanelet::Ids{from_and_to_id}); } /** @@ -2019,8 +2043,8 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLanelet) ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - const auto result_distance = - hdmap_utils.getLongitudinalDistance(pose_from.value(), pose_to.value(), false); + const auto result_distance = hdmap_utils.getLongitudinalDistance( + pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()); ASSERT_TRUE(result_distance.has_value()); EXPECT_NEAR(result_distance.value(), 27.0, 1.0); @@ -2040,8 +2064,8 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLaneletBehind) ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - const auto longitudinal_distance = - hdmap_utils.getLongitudinalDistance(pose_from.value(), pose_to.value(), false); + const auto longitudinal_distance = hdmap_utils.getLongitudinalDistance( + pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()); EXPECT_FALSE(longitudinal_distance.has_value()); } @@ -2059,8 +2083,8 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_differentLanelet) ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - const auto result_distance = - hdmap_utils.getLongitudinalDistance(pose_from.value(), pose_to.value(), false); + const auto result_distance = hdmap_utils.getLongitudinalDistance( + pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()); ASSERT_TRUE(result_distance.has_value()); EXPECT_NEAR(result_distance.value(), 86.0, 1.0); @@ -2080,8 +2104,10 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLongitudinalDistance_differentLane ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - EXPECT_FALSE( - hdmap_utils.getLongitudinalDistance(pose_from.value(), pose_to.value(), false).has_value()); + EXPECT_FALSE(hdmap_utils + .getLongitudinalDistance( + pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()) + .has_value()); } /** @@ -2092,8 +2118,11 @@ TEST_F(HdMapUtilsTest_KashiwanohaMap, getLongitudinalDistance_PullRequest1348) auto pose_from = traffic_simulator::helper::constructLaneletPose(34468, 10.0); auto pose_to = traffic_simulator::helper::constructLaneletPose(34795, 5.0); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; EXPECT_NO_THROW(EXPECT_DOUBLE_EQ( - hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true).value(), + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, lane_changeable_routing_configuration) + .value(), 54.18867466433655977198213804513216018676757812500000)); } @@ -2105,14 +2134,19 @@ TEST_F(HdMapUtilsTest_KashiwanohaMap, getLongitudinalDistance_PullRequest1348) */ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) { + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + traffic_simulator::RoutingConfiguration default_routing_configuration; { const auto pose_from = traffic_simulator::helper::constructLaneletPose(563L, 5.0); const auto pose_to = traffic_simulator::helper::constructLaneletPose(659L, 5.0); - const auto without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false); + const auto without_lane_change = + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + const auto with_lane_change = hdmap_utils.getLongitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 157.0, 1.0); } @@ -2120,10 +2154,12 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_from = traffic_simulator::helper::constructLaneletPose(563L, 5.0); const auto pose_to = traffic_simulator::helper::constructLaneletPose(658L, 5.0); - const auto without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false); + const auto without_lane_change = + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + const auto with_lane_change = hdmap_utils.getLongitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 161.0, 1.0); } @@ -2131,10 +2167,12 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_from = traffic_simulator::helper::constructLaneletPose(563L, 5.0); const auto pose_to = traffic_simulator::helper::constructLaneletPose(657L, 5.0); - const auto without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false); + const auto without_lane_change = + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + const auto with_lane_change = hdmap_utils.getLongitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 161.0, 1.0); } @@ -2142,10 +2180,12 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_from = traffic_simulator::helper::constructLaneletPose(643L, 5.0); const auto pose_to = traffic_simulator::helper::constructLaneletPose(666L, 5.0); - const auto without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false); + const auto without_lane_change = + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + const auto with_lane_change = hdmap_utils.getLongitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 250.0, 1.0); } @@ -2153,10 +2193,12 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_from = traffic_simulator::helper::constructLaneletPose(643L, 5.0); const auto pose_to = traffic_simulator::helper::constructLaneletPose(665L, 5.0); - const auto without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false); + const auto without_lane_change = + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + const auto with_lane_change = hdmap_utils.getLongitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 253.0, 1.0); } @@ -2739,11 +2781,16 @@ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLanelets) TEST_F(HdMapUtilsTest_WithRoadShoulderMap, routingWithRoadShoulder) { // default: traffic_simulator::RoutingGraphType::VEHICLE - const auto default_route = hdmap_utils.getRoute(34693, 34615, false); + const auto default_route = + hdmap_utils.getRoute(34693, 34615, traffic_simulator::RoutingConfiguration()); EXPECT_EQ(default_route.size(), 0); - const auto route_with_road_shoulder = hdmap_utils.getRoute( - 34693, 34615, false, traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER); + traffic_simulator::RoutingConfiguration routing_configuration_with_road_shoulder; + routing_configuration_with_road_shoulder.routing_graph_type = + traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER; + traffic_simulator::RoutingConfiguration default_routing_configuration; + const auto route_with_road_shoulder = + hdmap_utils.getRoute(34693, 34615, routing_configuration_with_road_shoulder); EXPECT_EQ(route_with_road_shoulder.size(), 4); EXPECT_EQ(route_with_road_shoulder[0], 34693); EXPECT_EQ(route_with_road_shoulder[1], 34696); // road shoulder diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index 8a2e20fdd12..ebe1558fb98 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -92,12 +92,13 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_noChange) 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); + pose_from, pose_to, std::numeric_limits::infinity(), + traffic_simulator::RoutingConfiguration(), hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } { - const auto result = - traffic_simulator::distance::lateralDistance(pose_from, pose_to, false, hdmap_utils_ptr); + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, traffic_simulator::RoutingConfiguration(), hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -115,13 +116,14 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_noChange) 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); + pose_from, pose_to, std::numeric_limits::infinity(), + traffic_simulator::RoutingConfiguration(), 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); + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, traffic_simulator::RoutingConfiguration(), hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 0.0, std::numeric_limits::epsilon()); } @@ -139,13 +141,18 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_change) const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( 3002184L, 0.0, 0.0, hdmap_utils_ptr); { + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, std::numeric_limits::infinity(), true, hdmap_utils_ptr); + pose_from, pose_to, std::numeric_limits::infinity(), + lane_changeable_routing_configuration, hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } { - const auto result = - traffic_simulator::distance::lateralDistance(pose_from, pose_to, true, hdmap_utils_ptr); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, lane_changeable_routing_configuration, hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -164,14 +171,19 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_change) constexpr double approx_distance = -3.0; constexpr double tolerance = 0.5; { + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, std::numeric_limits::infinity(), true, hdmap_utils_ptr); + pose_from, pose_to, std::numeric_limits::infinity(), + lane_changeable_routing_configuration, 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); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, lane_changeable_routing_configuration, hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), approx_distance, tolerance); } @@ -188,8 +200,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_matching) 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); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, 2.0, lane_changeable_routing_configuration, hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -206,8 +220,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_matching) traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); { - const auto result = - traffic_simulator::distance::lateralDistance(pose_from, pose_to, 3.0, true, hdmap_utils_ptr); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, 3.0, lane_changeable_routing_configuration, hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), -3.0, 0.5); } @@ -229,7 +245,8 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos 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); + pose_from.value(), pose_to.value(), false, false, traffic_simulator::RoutingConfiguration(), + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -250,7 +267,8 @@ TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noCh 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); + pose_from.value(), pose_to.value(), false, false, traffic_simulator::RoutingConfiguration(), + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 60.0, 1.0); } @@ -272,7 +290,8 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit 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); + pose_from.value(), pose_to.value(), true, false, traffic_simulator::RoutingConfiguration(), + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -293,7 +312,8 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit 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); + pose_from.value(), pose_to.value(), true, false, traffic_simulator::RoutingConfiguration(), + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 20.0, 1.0); } @@ -314,8 +334,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos makePose(81599.34, 50022.34, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } { @@ -326,8 +349,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos makePose(81612.95, 49991.30, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -347,8 +373,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos makePose(81570.56, 50141.75, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 145.0, 1.0); } @@ -360,8 +389,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos makePose(81610.25, 49988.59, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 178.0, 1.0); } @@ -380,8 +412,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 118.0, 1.0)); } { @@ -392,8 +427,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 195.0, 1.0)); } { @@ -404,8 +442,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ makePose(86553.48, 44990.56, 150.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 257.0, 1.0)); } { @@ -416,8 +457,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ makePose(86579.91, 44979.00, 150.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 228.0, 1.0)); } } @@ -437,8 +481,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch makePose(86642.95, 44958.78, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } { @@ -449,8 +496,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch makePose(86704.59, 44927.32, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -468,8 +518,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( makePose(86648.82, 44886.19, 240.0), false, hdmap_utils_ptr); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 103.0, 1.0); } @@ -481,8 +534,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch makePose(86599.32, 44975.01, 180.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 131.0, 1.0); } diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp index a7132a31c62..78c4c107294 100644 --- a/simulation/traffic_simulator/test/src/utils/test_pose.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_pose.cpp @@ -423,7 +423,8 @@ TEST_F(PoseTest, relativeLaneletPose_s_invalid) const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); - const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + const auto relative = traffic_simulator::pose::relativeLaneletPose( + from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_TRUE(std::isnan(relative.s)); } @@ -438,7 +439,8 @@ TEST_F(PoseTest, relativeLaneletPose_s_valid) const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils); - const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + const auto relative = traffic_simulator::pose::relativeLaneletPose( + from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_NEAR(relative.s, 107.74, 0.001); } @@ -453,7 +455,8 @@ TEST_F(PoseTest, relativeLaneletPose_offset_invalid) const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); - const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + const auto relative = traffic_simulator::pose::relativeLaneletPose( + from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_TRUE(std::isnan(relative.offset)); } @@ -468,7 +471,10 @@ TEST_F(PoseTest, relativeLaneletPose_offset_valid) const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils); - const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, true, hdmap_utils); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto relative = traffic_simulator::pose::relativeLaneletPose( + from, to, lane_changeable_routing_configuration, hdmap_utils); EXPECT_EQ(relative.offset, 1.0); } @@ -485,7 +491,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, false, hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_TRUE(std::isnan(relative.s)); } @@ -502,7 +508,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, false, hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_NEAR(relative.s, 103.74, 0.01); } @@ -519,7 +525,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, false, hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_TRUE(std::isnan(relative.s)); } @@ -536,7 +542,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, false, hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_EQ(relative.offset, 0.0); }