Merge pull request #1121 from tier4/fix/sign-of-relative-distance

Fix/sign of relative distance
tier4 · Nov 2, 2023 · d2a5a33 · d2a5a33
2 parents c4b7daa + 6801847
commit d2a5a33
Show file tree

Hide file tree

Showing 3 changed files with 59 additions and 8 deletions.
diff --git a/docs/ReleaseNotes.md b/docs/ReleaseNotes.md
@@ -12,9 +12,10 @@ Major Changes :race_car: :red_car: :blue_car:
 
 Bug Fixes:bug:
 
-| Feature                   | Brief summary                                                                   | Category            | Pull request                                                      | Contributor                                 |
-|---------------------------|---------------------------------------------------------------------------------|---------------------|-------------------------------------------------------------------|---------------------------------------------|
-| Deleted entity management | Fix crash caused by `DeleteEntityAction` by handling deleted entity explicitly. | `traffic_simulator` | [#1096](https://github.com/tier4/scenario_simulator_v2/pull/1096) | [f0reachARR](https://github.com/f0reachARR) |
+| Feature                                  | Brief summary                                                                                                          | Category                   | Pull request                                                      | Contributor                                   |
+|------------------------------------------|------------------------------------------------------------------------------------------------------------------------|----------------------------|-------------------------------------------------------------------|-----------------------------------------------|
+| Deleted entity management                | Fix crash caused by `DeleteEntityAction` by handling deleted entity explicitly.                                        | `traffic_simulator`        | [#1096](https://github.com/tier4/scenario_simulator_v2/pull/1096) | [f0reachARR](https://github.com/f0reachARR)   |
+| OpenSCENARIO `RelativeDistanceCondition` | Restore the behavior of the existing distance measurement mode of RelativeDistanceCondition that was changed in #1095. | `openscenario_interpreter` | [#1121](https://github.com/tier4/scenario_simulator_v2/pull/1121) | [yamacir-kit](https://github.com/yamacir-kit) |
 
 Minor Tweaks :oncoming_police_car:
 

diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp
@@ -62,7 +62,7 @@ auto RelativeDistanceCondition::distance<
   if (
     global().entities->at(triggering_entity).as<ScenarioObject>().is_added and
     global().entities->at(entity_ref).as<ScenarioObject>().is_added) {
-    return makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x;
+    return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x);
   } else {
     return Double::nan();
   }
@@ -76,7 +76,8 @@ auto RelativeDistanceCondition::distance<
   if (
     global().entities->at(triggering_entity).as<ScenarioObject>().is_added and
     global().entities->at(entity_ref).as<ScenarioObject>().is_added) {
-    return makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x;
+    return std::abs(
+      makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x);
   } else {
     return Double::nan();
   }
@@ -90,7 +91,7 @@ auto RelativeDistanceCondition::distance<
   if (
     global().entities->at(triggering_entity).as<ScenarioObject>().is_added and
     global().entities->at(entity_ref).as<ScenarioObject>().is_added) {
-    return makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y;
+    return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y);
   } else {
     return Double::nan();
   }
@@ -104,7 +105,8 @@ auto RelativeDistanceCondition::distance<
   if (
     global().entities->at(triggering_entity).as<ScenarioObject>().is_added and
     global().entities->at(entity_ref).as<ScenarioObject>().is_added) {
-    return makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y;
+    return std::abs(
+      makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y);
   } else {
     return Double::nan();
   }
@@ -150,6 +152,18 @@ auto RelativeDistanceCondition::distance<
   if (
     global().entities->at(entity_ref).as<ScenarioObject>().is_added and
     global().entities->at(triggering_entity).as<ScenarioObject>().is_added) {
+    /*
+       For historical reasons, signed distances are returned when
+       coordinateSystem == lane and relativeDistanceType ==
+       longitudinal/lateral. The sign has been mainly used to determine the
+       front/back and left/right positional relationship (a negative value is
+       returned if the target entity is behind or to the right).
+
+       This behavior violates the OpenSCENARIO standard. In the future, after
+       DistanceCondition and RelativeDistanceCondition of TIER IV's
+       OpenSCENARIO Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this
+       behavior will be enabled only when routingAlgorithm == undefined.
+    */
     return static_cast<traffic_simulator::LaneletPose>(
              makeNativeRelativeLanePosition(triggering_entity, entity_ref))
       .offset;
@@ -166,6 +180,18 @@ auto RelativeDistanceCondition::distance<
   if (
     global().entities->at(entity_ref).as<ScenarioObject>().is_added and
     global().entities->at(triggering_entity).as<ScenarioObject>().is_added) {
+    /*
+       For historical reasons, signed distances are returned when
+       coordinateSystem == lane and relativeDistanceType ==
+       longitudinal/lateral. The sign has been mainly used to determine the
+       front/back and left/right positional relationship (a negative value is
+       returned if the target entity is behind or to the right).
+
+       This behavior violates the OpenSCENARIO standard. In the future, after
+       DistanceCondition and RelativeDistanceCondition of TIER IV's
+       OpenSCENARIO Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this
+       behavior will be enabled only when routingAlgorithm == undefined.
+    */
     return static_cast<traffic_simulator::LaneletPose>(
              makeNativeBoundingBoxRelativeLanePosition(triggering_entity, entity_ref))
       .offset;
@@ -182,6 +208,18 @@ auto RelativeDistanceCondition::distance<
   if (
     global().entities->at(triggering_entity).as<ScenarioObject>().is_added and
     global().entities->at(entity_ref).as<ScenarioObject>().is_added) {
+    /*
+       For historical reasons, signed distances are returned when
+       coordinateSystem == lane and relativeDistanceType ==
+       longitudinal/lateral. The sign has been mainly used to determine the
+       front/back and left/right positional relationship (a negative value is
+       returned if the target entity is behind or to the right).
+
+       This behavior violates the OpenSCENARIO standard. In the future, after
+       DistanceCondition and RelativeDistanceCondition of TIER IV's
+       OpenSCENARIO Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this
+       behavior will be enabled only when routingAlgorithm == undefined.
+    */
     return static_cast<traffic_simulator::LaneletPose>(
              makeNativeRelativeLanePosition(triggering_entity, entity_ref))
       .s;
@@ -198,6 +236,18 @@ auto RelativeDistanceCondition::distance<
   if (
     global().entities->at(triggering_entity).as<ScenarioObject>().is_added and
     global().entities->at(entity_ref).as<ScenarioObject>().is_added) {
+    /*
+       For historical reasons, signed distances are returned when
+       coordinateSystem == lane and relativeDistanceType ==
+       longitudinal/lateral. The sign has been mainly used to determine the
+       front/back and left/right positional relationship (a negative value is
+       returned if the target entity is behind or to the right).
+
+       This behavior violates the OpenSCENARIO standard. In the future, after
+       DistanceCondition and RelativeDistanceCondition of TIER IV's
+       OpenSCENARIO Interpreter support OpenSCENARIO 1.2 RoutingAlgorithm, this
+       behavior will be enabled only when routingAlgorithm == undefined.
+    */
     return static_cast<traffic_simulator::LaneletPose>(
              makeNativeBoundingBoxRelativeLanePosition(triggering_entity, entity_ref))
       .s;

diff --git a/...runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml b/...runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml
@@ -129,7 +129,7 @@ OpenSCENARIO:
                                         freespace: true # True: distance is measured between closest bounding box points. False: reference point distance is used.
                                         relativeDistanceType: lateral
                                         rule: equalTo
-                                        value: -0.1700000000055297
+                                        value: 0.1700000000055297
                                 - name: ""
                                   delay: 0
                                   conditionEdge: none