Merge pull request #1458 from tier4/refactor/add_routing_graph_argument

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;

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;
           }

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::HdMapUtils> & hdmap_utils,
-    bool allow_lane_change = false) const -> std::optional<LaneletPose>;
+    const traffic_simulator::RoutingConfiguration & routing_configuration =
+      traffic_simulator::RoutingConfiguration()) const -> std::optional<LaneletPose>;
   static auto setConsiderPoseByRoadSlope(bool consider_pose_by_road_slope) -> void
   {
     consider_pose_by_road_slope_ = consider_pose_by_road_slope;

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 <iostream>
+#include <traffic_simulator/data_type/routing_graph_type.hpp>
+
+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_

simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp

@@ -45,6 +45,7 @@
 #include <string>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <traffic_simulator/data_type/lane_change.hpp>
+#include <traffic_simulator/data_type/routing_configuration.hpp>
 #include <traffic_simulator/data_type/routing_graph_type.hpp>
 #include <traffic_simulator/hdmap_utils/cache.hpp>
 #include <traffic_simulator/hdmap_utils/traffic_rules.hpp>
@@ -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<std::pair<int, int>>;
+    const traffic_simulator_msgs::msg::LaneletPose & to,
+    const traffic_simulator::RoutingConfiguration & routing_configuration =
+      traffic_simulator::RoutingConfiguration()) const -> std::optional<std::pair<int, int>>;
 
   auto filterLaneletIds(const lanelet::Ids &, const char subtype[]) const -> lanelet::Ids;
 
@@ -93,8 +98,9 @@ class HdMapUtils
     -> std::vector<traffic_simulator_msgs::msg::LaneletPose>;
 
   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<geometry_msgs::msg::Point>;
 
@@ -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<std::pair<math::geometry::HermiteCurve, double>>;
 
   auto getLaneChangeableLaneletId(
-    const lanelet::Id, const traffic_simulator::lane_change::Direction) const
-    -> std::optional<lanelet::Id>;
+    const lanelet::Id, const traffic_simulator::lane_change::Direction,
+    const traffic_simulator::RoutingGraphType type =
+      traffic_simulator::RoutingGraphType::VEHICLE) const -> std::optional<lanelet::Id>;
 
   auto getLaneChangeableLaneletId(
-    const lanelet::Id, const traffic_simulator::lane_change::Direction,
-    const std::uint8_t shift) const -> std::optional<lanelet::Id>;
+    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<lanelet::Id>;
 
   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<double>;
+    const traffic_simulator_msgs::msg::LaneletPose & to,
+    const traffic_simulator::RoutingConfiguration & routing_configuration =
+      traffic_simulator::RoutingConfiguration()) const -> std::optional<double>;
 
   auto getLeftBound(const lanelet::Id) const -> std::vector<geometry_msgs::msg::Point>;
 
@@ -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<double>;
+    const traffic_simulator::RoutingConfiguration & routing_configuration =
+      traffic_simulator::RoutingConfiguration()) const -> std::optional<double>;
 
   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<geometry_msgs::msg::Point>;
 
@@ -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<lanelet::Id>;
+    const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO,
+    const traffic_simulator::RoutingGraphType type =
+      traffic_simulator::RoutingGraphType::VEHICLE) const -> std::optional<lanelet::Id>;
 
   auto toLaneletPose(
     const geometry_msgs::msg::Pose &, const bool include_crosswalk,
@@ -316,22 +343,25 @@ 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<traffic_simulator_msgs::msg::LaneletPose>;
+    const bool include_crosswalk, const double matching_distance = 1.0,
+    const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE)
+    const -> std::optional<traffic_simulator_msgs::msg::LaneletPose>;
 
   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<traffic_simulator_msgs::msg::LaneletPose>;
+    const bool include_crosswalk, const double matching_distance = 1.0,
+    const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE)
+    const -> std::optional<traffic_simulator_msgs::msg::LaneletPose>;
 
   auto toLaneletPose(
     const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 1.0) const
     -> std::optional<traffic_simulator_msgs::msg::LaneletPose>;
 
   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<traffic_simulator_msgs::msg::LaneletPose>;
+    const bool include_opposite_direction = true,
+    const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE)
+    const -> std::vector<traffic_simulator_msgs::msg::LaneletPose>;
 
   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);

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::HdMapUtils> & hdmap_utils_ptr)
-  -> std::optional<double>;
+  const traffic_simulator::RoutingConfiguration & routing_configuration,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
 
 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::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
 
 // Lateral (unit: lanes)
 auto countLaneChanges(
   const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to,
-  bool allow_lane_change, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  const traffic_simulator::RoutingConfiguration & routing_configuration,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
   -> std::optional<std::pair<int, int>>;
 
 // 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::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
 
 // BoundingBox
@@ -56,15 +58,17 @@ 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::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
 
 auto boundingBoxLaneLongitudinalDistance(
   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 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::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
 
 // Bounds