fix(openscenario_interpreter, traffic_simulator): fix after merge

tier4 · Oct 14, 2024 · 3868f03 · 3868f03
1 parent 6fe1268
commit 3868f03
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 151 deletions.
diff --git a/...ario_interpreter/include/openscenario_interpreter/syntax/relative_clearance_condition.hpp b/...ario_interpreter/include/openscenario_interpreter/syntax/relative_clearance_condition.hpp
@@ -41,8 +41,8 @@ inline namespace syntax
    </xsd:complexType>
 */
 struct RelativeClearanceCondition : private Scope,
-                                    private SimulatorCore::ConditionEvaluation,
-                                    private SimulatorCore::NonStandardOperation
+                                    private SimulatorCore::DistanceConditionEvaluation,
+                                    private SimulatorCore::ConditionEvaluation
 {
   /*
      Longitudinal distance behind reference point of the entity to be checked along lane centerline of the current lane of the triggering entity. Orientation of entity towards lane determines backward direction. Velocity of entity is irrelevant. Unit: [m]. Range: [0..inf[. Default if omitted: 0

diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_clearance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_clearance_condition.cpp
@@ -94,8 +94,8 @@ auto RelativeClearanceCondition::evaluate() -> Object
       } else {
         int relative_lateral_lane;
         try {
-          relative_lateral_lane = evaluateLateralRelativeLanes(
-            triggering_entity, target_entity, RoutingAlgorithm::shortest);
+          relative_lateral_lane =
+            lateralRelativeLanes(triggering_entity, target_entity, RoutingAlgorithm::shortest);
         } catch (const std::exception &) {
           // occurring errors means that the target entity is not in the specified range,
           // under the assumption that relative lane range is defined in routable range .
@@ -113,15 +113,11 @@ auto RelativeClearanceCondition::evaluate() -> Object
     auto is_in_longitudinal_range = [&]() {
       auto relative_longitudinal = [&]() {
         if (free_space) {
-          return static_cast<traffic_simulator::LaneletPose>(
-                   makeNativeBoundingBoxRelativeLanePosition(
-                     triggering_entity, target_entity, RoutingAlgorithm::shortest))
-            .s;
+          return longitudinalLaneBoundingBoxDistance(
+            triggering_entity, target_entity, RoutingAlgorithm::shortest);
         } else {
-          return static_cast<traffic_simulator::LaneletPose>(
-                   makeNativeRelativeLanePosition(
-                     triggering_entity, target_entity, RoutingAlgorithm::shortest))
-            .s;
+          return longitudinalLaneDistance(
+            triggering_entity, target_entity, RoutingAlgorithm::shortest);
         }
       }();
 

diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
@@ -256,6 +256,10 @@ class API
     const geometry_msgs::msg::Pose & from_map_pose, const std::string & to_entity_name)
     -> std::optional<geometry_msgs::msg::Pose>;
 
+  auto countLaneChanges(
+    const std::string & from_entity_name, const std::string & to_entity_name,
+    const bool allow_lane_change) const -> std::optional<std::pair<int, int>>;
+
   auto boundingBoxRelativePose(
     const std::string & from_entity_name, const geometry_msgs::msg::Pose & to_map_pose)
     -> std::optional<geometry_msgs::msg::Pose>;

diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp
@@ -455,6 +455,21 @@ auto API::relativePose(
   return pose::relativePose(from_map_pose, to_entity->getMapPose());
 }
 
+auto API::countLaneChanges(
+  const std::string & from_entity_name, const std::string & to_entity_name,
+  const bool allow_lane_change) const -> std::optional<std::pair<int, int>>
+{
+  if (from_entity_name != to_entity_name) {
+    const auto from_entity = getEntity(from_entity_name);
+    const auto to_entity = getEntity(to_entity_name);
+    return traffic_simulator::distance::countLaneChanges(
+      from_entity->getCanonicalizedLaneletPose().value(),
+      to_entity->getCanonicalizedLaneletPose().value(), allow_lane_change, getHdmapUtils());
+  } else {
+    return std::nullopt;
+  }
+}
+
 auto API::laneletDistance(
   const std::string & from_entity_name, const std::string & to_entity_name,
   const bool allow_lane_change) -> LaneletDistance

diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp
@@ -73,17 +73,6 @@ TEST(pose, quietNaNLaneletPose)
   EXPECT_TRUE(std::isnan(pose.rpy.z));
 }
 
-/**
- * @note Test canonicalization with a default constructed LaneletPose.
- */
-TEST_F(PoseTest, canonicalize_default)
-{
-  const auto pose =
-    traffic_simulator::pose::canonicalize(traffic_simulator_msgs::msg::LaneletPose(), hdmap_utils);
-
-  EXPECT_FALSE(pose.has_value());
-}
-
 /**
  * @note Test canonicalization with an invalid LaneletPose.
  */
@@ -413,134 +402,6 @@ TEST_F(PoseTest, boundingBoxRelativePose_intersect)
   EXPECT_FALSE(relative.has_value());
 }
 
-/**
- * @note Test s-value calculation correctness when longitudinal distance between the poses can not be calculated.
- */
-TEST_F(PoseTest, relativeLaneletPose_s_invalid)
-{
-  const auto from =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils);
-  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);
-
-  EXPECT_TRUE(std::isnan(relative.s));
-}
-
-/**
- * @note Test s-value calculation correctness when longitudinal distance between the poses can be calculated.
- */
-TEST_F(PoseTest, relativeLaneletPose_s_valid)
-{
-  const auto from =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils);
-  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);
-
-  EXPECT_NEAR(relative.s, 107.74, 0.001);
-}
-
-/**
- * @note Test offset-value calculation correctness when lateral distance between the poses can not be calculated.
- */
-TEST_F(PoseTest, relativeLaneletPose_offset_invalid)
-{
-  const auto from =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils);
-  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);
-
-  EXPECT_TRUE(std::isnan(relative.offset));
-}
-
-/**
- * @note Test offset-value calculation correctness when lateral distance between the poses can be calculated.
- */
-TEST_F(PoseTest, relativeLaneletPose_offset_valid)
-{
-  const auto from =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils);
-  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);
-
-  EXPECT_EQ(relative.offset, 1.0);
-}
-
-/**
- * @note Test s-value calculation correctness when longitudinal distance between the poses can not be calculated.
- */
-TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid)
-{
-  const auto from =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils);
-  const auto to =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils);
-  const auto bounding_box = makeBoundingBox();
-
-  const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose(
-    from, bounding_box, to, bounding_box, false, hdmap_utils);
-
-  EXPECT_TRUE(std::isnan(relative.s));
-}
-
-/**
- * @note Test s-value calculation correctness when longitudinal distance between the poses can be calculated.
- */
-TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid)
-{
-  const auto from =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils);
-  const auto to =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils);
-  const auto bounding_box = makeBoundingBox();
-
-  const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose(
-    from, bounding_box, to, bounding_box, false, hdmap_utils);
-
-  EXPECT_NEAR(relative.s, 103.74, 0.01);
-}
-
-/**
- * @note Test offset-value calculation correctness when lateral distance between the poses can not be calculated.
- */
-TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid)
-{
-  const auto from =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils);
-  const auto to =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils);
-  const auto bounding_box = makeBoundingBox();
-
-  const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose(
-    from, bounding_box, to, bounding_box, false, hdmap_utils);
-
-  EXPECT_TRUE(std::isnan(relative.s));
-}
-
-/**
- * @note Test offset-value calculation correctness when lateral distance between the poses can be calculated.
- */
-TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid)
-{
-  const auto from =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, -1.0, hdmap_utils);
-  const auto to =
-    traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils);
-  const auto bounding_box = makeBoundingBox();
-
-  const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose(
-    from, bounding_box, to, bounding_box, false, hdmap_utils);
-
-  EXPECT_EQ(relative.offset, 0.0);
-}
-
 /**
  * @note Test calculation correctness when the pose lies within the lanelet.
  */