Feature/time to collision condition

docs/developer_guide/OpenSCENARIOSupport.md

@@ -486,8 +486,8 @@ Currently, the only way to know the result of the simulation is by viewing the s
 | TimeOfDay                                    | 1.3               |                                            |
 | TimeOfDayCondition                           | unimplemented     |                                            |
 | TimeReference                                | 1.3               |                                            |
-| TimeToCollisionCondition                     | unimplemented     |                                            |
-| TimeToCollisionConditionTarget               | unimplemented     |                                            |
+| TimeToCollisionCondition                     | 1.3.1 (partial)   | [detail](#TimeToCollisionCondition)        |
+| TimeToCollisionConditionTarget               | 1.3.1             |                                            |
 | Timing                                       | 1.3               |                                            |
 | TrafficAction                                | unimplemented     |                                            |
 | TrafficArea                                  | unimplemented     |                                            |
@@ -848,6 +848,13 @@ Currently, the only way to know the result of the simulation is by viewing the s
 - Property `freespace` is ignored.
   - The simulator behaves as if `freespace` is `false`.
 
+#### TimeToCollisionCondition
+
+- Since `TimeToCollisionCondition` is implemented using `DistanceCondition`,
+  `RelativeDistanceCondition`, `SpeedCondition`, and `RelativeSpeedCondition`,
+  if a combination of properties that is not supported by those Conditions is
+  given to `TimeToCollisionCondition`, an error will be thrown.
+
 #### TransitionDynamics
 
 - Property `followingMode` created in version 1.2 is ignored.

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -562,15 +562,9 @@ class SimulatorCore
       if (const auto observer = core->getEntity(from.name())) {
         if (const auto observed = core->getEntity(to.name())) {
           auto velocity = [](const auto & entity) -> Eigen::Vector3d {
-            auto direction = [](auto orientation) -> Eigen::Vector3d {
-              const auto euler_angle = math::geometry::convertQuaternionToEulerAngle(orientation);
-              const auto r = euler_angle.x;
-              const auto p = euler_angle.y;
-              const auto y = euler_angle.z;
-              return Eigen::Vector3d(
-                std::cos(y) * std::cos(p), std::sin(y) * std::cos(p), std::sin(p));
+            auto direction = [](const auto & q) -> Eigen::Vector3d {
+              return Eigen::Quaternion(q.w, q.x, q.y, q.z) * Eigen::Vector3d::UnitX();
             };
-
             return direction(entity->getMapPose().orientation) * entity->getCurrentTwist().linear.x;
           };
 

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp

@@ -26,7 +26,7 @@ namespace openscenario_interpreter
 inline namespace syntax
 {
 /*
-   RelativeSpeedCondition 1.3.1
+   RelativeSpeedCondition (OpenSCENARIO XML 1.3.1)
 
    The current relative speed of a triggering entity/entities to a reference
    entity is compared to a given value. The logical operator used for the

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp

@@ -0,0 +1,186 @@
+// 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 OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_HPP_
+#define OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_HPP_
+
+#include <openscenario_interpreter/scope.hpp>
+#include <openscenario_interpreter/simulator_core.hpp>
+#include <openscenario_interpreter/syntax/distance_condition.hpp>
+#include <openscenario_interpreter/syntax/relative_distance_condition.hpp>
+#include <openscenario_interpreter/syntax/relative_speed_condition.hpp>
+#include <openscenario_interpreter/syntax/speed_condition.hpp>
+#include <openscenario_interpreter/syntax/time_to_collision_condition_target.hpp>
+#include <openscenario_interpreter/utility/print.hpp>
+
+namespace openscenario_interpreter
+{
+inline namespace syntax
+{
+/*
+   TimeToCollisionCondition (OpenSCENARIO XML 1.3.1)
+
+   The currently predicted time to collision of a triggering entity/entities
+   and either a reference entity or an explicit position is compared to a given
+   time value. Time to collision is calculated as the distance divided by the
+   relative speed. Acceleration of entities is not taken into consideration.
+   For the relative speed calculation the same coordinate system and relative
+   distance type applies as for the distance calculation. If the relative speed
+   is negative, which means the entity is moving away from the position / the
+   entities are moving away from each other, then the time to collision cannot
+   be predicted and the condition evaluates to false. The logical operator for
+   comparison is defined by the rule attribute. The property "alongRoute" is
+   deprecated. If "coordinateSystem" or "relativeDistanceType" are set,
+   "alongRoute" is ignored.
+
+   <xsd:complexType name="TimeToCollisionCondition">
+     <xsd:all>
+       <xsd:element name="TimeToCollisionConditionTarget" type="TimeToCollisionConditionTarget"/>
+     </xsd:all>
+     <xsd:attribute name="alongRoute" type="Boolean">
+       <xsd:annotation>
+         <xsd:appinfo>
+           deprecated
+         </xsd:appinfo>
+       </xsd:annotation>
+     </xsd:attribute>
+     <xsd:attribute name="freespace" type="Boolean" use="required"/>
+     <xsd:attribute name="rule" type="Rule" use="required"/>
+     <xsd:attribute name="value" type="Double" use="required"/>
+     <xsd:attribute name="relativeDistanceType" type="RelativeDistanceType"/>
+     <xsd:attribute name="coordinateSystem" type="CoordinateSystem"/>
+     <xsd:attribute name="routingAlgorithm" type="RoutingAlgorithm"/>
+   </xsd:complexType>
+*/
+struct TimeToCollisionCondition : private Scope, private SimulatorCore::ConditionEvaluation
+{
+  const TimeToCollisionConditionTarget time_to_collision_condition_target;
+
+  const Boolean freespace;
+
+  const Rule rule;
+
+  const Double value;
+
+  const RelativeDistanceType relative_distance_type;
+
+  const CoordinateSystem coordinate_system;
+
+  const RoutingAlgorithm routing_algorithm;
+
+  const TriggeringEntities triggering_entities;
+
+  std::vector<std::valarray<double>> evaluations;
+
+  explicit TimeToCollisionCondition(
+    const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities)
+  : Scope(scope),
+    time_to_collision_condition_target(
+      readElement<TimeToCollisionConditionTarget>("TimeToCollisionConditionTarget", node, scope)),
+    freespace(readAttribute<Boolean>("freespace", node, scope)),
+    rule(readAttribute<Rule>("rule", node, scope)),
+    value(readAttribute<Double>("value", node, scope)),
+    relative_distance_type(
+      readAttribute<RelativeDistanceType>("relativeDistanceType", node, scope)),
+    coordinate_system(
+      readAttribute<CoordinateSystem>("coordinateSystem", node, scope, CoordinateSystem::entity)),
+    routing_algorithm(readAttribute<RoutingAlgorithm>(
+      "routingAlgorithm", node, scope, RoutingAlgorithm::undefined)),
+    triggering_entities(triggering_entities),
+    evaluations(triggering_entities.entity_refs.size(), {Double::nan()})
+  {
+  }
+
+  auto description() const
+  {
+    auto description = std::stringstream();
+
+    description << triggering_entities.description() << "'s time-to-collision to given ";
+
+    if (time_to_collision_condition_target.is<Entity>()) {
+      description << " entity " << time_to_collision_condition_target.as<Entity>();
+    } else {
+      description << " position";
+    }
+
+    description << " = ";
+
+    print_to(description, evaluations);
+
+    description << " " << rule << " " << value << "?";
+
+    return description.str();
+  }
+
+  static auto evaluate(
+    const Entities * entities, const Entity & triggering_entity,
+    const TimeToCollisionConditionTarget & time_to_collision_condition_target,
+    CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type,
+    RoutingAlgorithm routing_algorithm, Boolean freespace)
+  {
+    auto distance = [&]() {
+      if (time_to_collision_condition_target.is<Entity>()) {
+        return RelativeDistanceCondition::evaluate(
+          entities, triggering_entity, time_to_collision_condition_target.as<Entity>(),
+          coordinate_system, relative_distance_type, routing_algorithm, freespace);
+      } else {
+        return DistanceCondition::evaluate(
+          entities, triggering_entity, time_to_collision_condition_target.as<Position>(),
+          coordinate_system, relative_distance_type, routing_algorithm, freespace);
+      }
+    };
+
+    auto speed = [&]() {
+      auto directional_dimension = [&]() {
+        switch (relative_distance_type) {
+          case RelativeDistanceType::longitudinal:
+            return std::optional<DirectionalDimension>(DirectionalDimension::longitudinal);
+          case RelativeDistanceType::lateral:
+            return std::optional<DirectionalDimension>(DirectionalDimension::lateral);
+          default:
+          case RelativeDistanceType::euclidianDistance:
+            return std::optional<DirectionalDimension>(std::nullopt);
+        };
+      };
+      if (time_to_collision_condition_target.is<Entity>()) {
+        return RelativeSpeedCondition::evaluate(
+          entities, triggering_entity, time_to_collision_condition_target.as<Entity>(),
+          directional_dimension());
+      } else {
+        return SpeedCondition::evaluate(
+          entities, triggering_entity, directional_dimension(), Compatibility::standard);
+      }
+    };
+
+    return distance() / std::max(speed(), 0.0);
+  }
+
+  auto evaluate()
+  {
+    evaluations.clear();
+
+    return asBoolean(triggering_entities.apply([this](const auto & triggering_entity) {
+      evaluations.push_back(triggering_entity.apply([this](const auto & triggering_entity) {
+        return evaluate(
+          global().entities, triggering_entity, time_to_collision_condition_target,
+          coordinate_system, relative_distance_type, routing_algorithm, freespace);
+      }));
+      return not evaluations.back().size() or std::invoke(rule, evaluations.back(), value).min();
+    }));
+  }
+};
+}  // namespace syntax
+}  // namespace openscenario_interpreter
+
+#endif  // OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_HPP_

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp

@@ -0,0 +1,45 @@
+// 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 OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_
+#define OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_
+
+#include <openscenario_interpreter/scope.hpp>
+#include <pugixml.hpp>
+
+namespace openscenario_interpreter
+{
+inline namespace syntax
+{
+/*
+   TimeToCollisionConditionTarget (OpenSCENARIO XML 1.3.1)
+
+   Target position used in the TimeToCollisionCondition. Can be defined as
+   either an explicit position, or the position of a reference entity.
+
+   <xsd:complexType name="TimeToCollisionConditionTarget">
+     <xsd:choice>
+       <xsd:element name="Position" type="Position"/>
+       <xsd:element name="EntityRef" type="EntityRef"/>
+     </xsd:choice>
+   </xsd:complexType>
+*/
+struct TimeToCollisionConditionTarget : public ComplexType
+{
+  explicit TimeToCollisionConditionTarget(const pugi::xml_node &, Scope &);
+};
+}  // namespace syntax
+}  // namespace openscenario_interpreter
+
+#endif  // OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_

openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp

@@ -24,6 +24,7 @@
 #include <openscenario_interpreter/syntax/speed_condition.hpp>
 #include <openscenario_interpreter/syntax/stand_still_condition.hpp>
 #include <openscenario_interpreter/syntax/time_headway_condition.hpp>
+#include <openscenario_interpreter/syntax/time_to_collision_condition.hpp>
 
 namespace openscenario_interpreter
 {
@@ -38,7 +39,7 @@ EntityCondition::EntityCondition(
       std::make_pair(        "CollisionCondition", [&](const auto & node) { return make<        CollisionCondition>(node, scope, triggering_entities); }),
       std::make_pair(          "OffroadCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }),
       std::make_pair(      "TimeHeadwayCondition", [&](const auto & node) { return make<      TimeHeadwayCondition>(node, scope, triggering_entities); }),
-      std::make_pair(  "TimeToCollisionCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }),
+      std::make_pair(  "TimeToCollisionCondition", [&](const auto & node) { return make<  TimeToCollisionCondition>(node, scope, triggering_entities); }),
       std::make_pair(     "AccelerationCondition", [&](const auto & node) { return make<     AccelerationCondition>(node, scope, triggering_entities); }),
       std::make_pair(       "StandStillCondition", [&](const auto & node) { return make<       StandStillCondition>(node, scope, triggering_entities); }),
       std::make_pair(            "SpeedCondition", [&](const auto & node) { return make<            SpeedCondition>(node, scope, triggering_entities); }),

openscenario/openscenario_interpreter/src/syntax/time_to_collision_condition_target.cpp

@@ -0,0 +1,38 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <openscenario_interpreter/reader/element.hpp>
+#include <openscenario_interpreter/syntax/entity.hpp>
+#include <openscenario_interpreter/syntax/entity_selection.hpp>
+#include <openscenario_interpreter/syntax/position.hpp>
+#include <openscenario_interpreter/syntax/time_to_collision_condition_target.hpp>
+
+namespace openscenario_interpreter
+{
+inline namespace syntax
+{
+TimeToCollisionConditionTarget::TimeToCollisionConditionTarget(
+  const pugi::xml_node & node, Scope & scope)
+// clang-format off
+: ComplexType(choice(node,
+    std::make_pair( "Position", [&](auto && node) { return make<Position>(std::forward<decltype(node)>(node), scope); }),
+    std::make_pair("EntityRef", [&](auto && node) { return make<  Entity>(std::forward<decltype(node)>(node), scope); })))
+// clang-format on
+{
+  if (is<EntitySelection>()) {
+    throw SyntaxError("EntitySelection is not allowed in TimeToCollisionConditionTarget.entityRef");
+  }
+}
+}  // namespace syntax
+}  // namespace openscenario_interpreter

test_runner/scenario_test_runner/config/workflow.txt

@@ -7,6 +7,7 @@ $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityConditio
 $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition.yaml
 $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition-back.yaml
 $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeSpeedCondition.yaml
+$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml
 $(find-pkg-share scenario_test_runner)/scenario/ControllerAction.AssignControllerAction.yaml
 $(find-pkg-share scenario_test_runner)/scenario/LateralAction.LaneChangeAction-RoadShoulder.yaml
 $(find-pkg-share scenario_test_runner)/scenario/LongitudinalAction.SpeedAction.yaml