Feature/time to collision condition

common/math/geometry/include/geometry/vector3/norm.hpp

@@ -15,6 +15,7 @@
 #ifndef GEOMETRY__VECTOR3__NORM_HPP_
 #define GEOMETRY__VECTOR3__NORM_HPP_
 
+#include <cmath>
 #include <geometry/vector3/is_like_vector3.hpp>
 
 namespace math

openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp

@@ -22,6 +22,7 @@
 #include <openscenario_interpreter/reader/evaluate.hpp>
 #include <openscenario_interpreter/syntax/parameter_type.hpp>
 #include <openscenario_interpreter/utility/highlighter.hpp>
+#include <optional>
 #include <pugixml.hpp>
 #include <regex>
 #include <scenario_simulator_exception/exception.hpp>
@@ -164,6 +165,16 @@ auto readAttribute(const std::string & name, const Node & node, const Scope & sc
     return value;
   }
 }
+
+template <typename T, typename Node, typename Scope>
+auto readAttribute(const std::string & name, const Node & node, const Scope & scope, std::nullopt_t)
+{
+  if (node.attribute(name.c_str())) {
+    return std::make_optional(readAttribute<T>(name, node, scope));
+  } else {
+    return std::optional<T>();
+  }
+}
 }  // namespace reader
 }  // namespace openscenario_interpreter
 

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -16,6 +16,7 @@
 #define OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_
 
 #include <geometry/quaternion/quaternion_to_euler.hpp>
+#include <geometry/vector3/operator.hpp>
 #include <openscenario_interpreter/error.hpp>
 #include <openscenario_interpreter/syntax/boolean.hpp>
 #include <openscenario_interpreter/syntax/double.hpp>
@@ -469,7 +470,6 @@ class SimulatorCore
       return core->checkCollision(std::forward<decltype(xs)>(xs)...);
     }
 
-    template <typename... Ts>
     static auto evaluateBoundingBoxEuclideanDistance(
       const std::string & from_entity_name,
       const std::string & to_entity_name)  // for RelativeDistanceCondition
@@ -487,6 +487,13 @@ class SimulatorCore
       return std::numeric_limits<double>::quiet_NaN();
     }
 
+    template <typename T, typename U>
+    static auto evaluateRelativeSpeed(const T & x, const U & y)
+    {
+      using math::geometry::operator-;
+      return core->getCurrentTwist(x).linear - core->getCurrentTwist(y).linear;
+    }
+
     template <typename... Ts>
     static auto evaluateSimulationTime(Ts &&... xs) -> double
     {

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/directional_dimension.hpp

@@ -0,0 +1,108 @@
+// 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__DIRECTIONAL_DIMENSION_HPP_
+#define OPENSCENARIO_INTERPRETER__SYNTAX__DIRECTIONAL_DIMENSION_HPP_
+
+namespace openscenario_interpreter
+{
+inline namespace syntax
+{
+/*
+   DirectionalDimension (OpenSCENARIO XML 1.3)
+
+   Defines the directions in the entity coordinate system.
+
+   <xsd:simpleType name="DirectionalDimension">
+     <xsd:union>
+       <xsd:simpleType>
+         <xsd:restriction base="xsd:string">
+           <xsd:enumeration value="longitudinal"/>
+           <xsd:enumeration value="lateral"/>
+           <xsd:enumeration value="vertical"/>
+         </xsd:restriction>
+       </xsd:simpleType>
+       <xsd:simpleType>
+         <xsd:restriction base="parameter"/>
+       </xsd:simpleType>
+     </xsd:union>
+   </xsd:simpleType>
+*/
+struct DirectionalDimension
+{
+  enum value_type {
+    /*
+       Longitudinal direction (along the x-axis).
+    */
+    longitudinal,  // NOTE: DEFAULT VALUE
+
+    /*
+       Lateral direction (along the y-axis).
+    */
+    lateral,
+
+    /*
+       Vertical direction (along the z-axis).
+    */
+    vertical,
+  };
+
+  value_type value;
+
+  DirectionalDimension() = default;
+
+  constexpr DirectionalDimension(value_type value) : value(value) {}
+
+  constexpr operator value_type() const noexcept { return value; }
+
+  friend auto operator>>(std::istream & istream, DirectionalDimension & datum) -> std::istream &
+  {
+    if (auto token = std::string(); istream >> token) {
+      if (token == "longitudinal") {
+        datum.value = DirectionalDimension::longitudinal;
+        return istream;
+      } else if (token == "lateral") {
+        datum.value = DirectionalDimension::lateral;
+        return istream;
+      } else if (token == "vertical") {
+        datum.value = DirectionalDimension::vertical;
+        return istream;
+      } else {
+        throw UNEXPECTED_ENUMERATION_VALUE_SPECIFIED(DirectionalDimension, token);
+      }
+    } else {
+      datum.value = DirectionalDimension::value_type();
+      return istream;
+    }
+  }
+
+  friend auto operator<<(std::ostream & ostream, const DirectionalDimension & datum)
+    -> std::ostream &
+  {
+    switch (datum) {
+      case DirectionalDimension::longitudinal:
+        return ostream << "longitudinal";
+      case DirectionalDimension::lateral:
+        return ostream << "lateral";
+      case DirectionalDimension::vertical:
+        return ostream << "vertical";
+      default:
+        throw UNEXPECTED_ENUMERATION_VALUE_ASSIGNED(DirectionalDimension, datum);
+    }
+  }
+};
+}  // namespace syntax
+}  // namespace openscenario_interpreter
+
+#endif  // OPENSCENARIO_INTERPRETER__SYNTAX__DIRECTIONAL_DIMENSION_HPP_

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp

@@ -90,21 +90,21 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi
 
   std::vector<Double> results;  // for description
 
-  const bool consider_z;
-
   explicit RelativeDistanceCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &);
 
   auto description() const -> String;
 
   template <
     CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type,
     Boolean::value_type>
-  auto distance(const EntityRef &) -> double
+  static auto distance(const EntityRef &, const EntityRef &) -> double
   {
     throw SyntaxError(__FILE__, ":", __LINE__);
   }
 
-  auto distance(const EntityRef &) -> double;
+  static auto distance(
+    const EntityRef &, const EntityRef &, const Entities *, CoordinateSystem, RelativeDistanceType,
+    RoutingAlgorithm, bool) -> double;
 
   auto evaluate() -> Object;
 };
@@ -113,20 +113,20 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi
 // cspell: ignore euclidian
 
 // clang-format off
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, false>(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, true >(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, false>(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, true >(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, false>(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, true >(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, false>(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, true >(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, false>(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, true >(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::shortest,  false>(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::shortest,  true >(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::shortest,  false>(const EntityRef &) -> double;
-template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::shortest,  true >(const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, false>(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, true >(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, false>(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, true >(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, false>(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, true >(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, false>(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, true >(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, false>(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, true >(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::shortest,  false>(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::shortest,  true >(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::shortest,  false>(const EntityRef &, const EntityRef &) -> double;
+template <> auto RelativeDistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::shortest,  true >(const EntityRef &, const EntityRef &) -> double;
 // clang-format on
 }  // namespace syntax
 }  // namespace openscenario_interpreter

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

@@ -0,0 +1,84 @@
+// 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__RELATIVE_SPEED_CONDITION_HPP_
+#define OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_
+
+#include <openscenario_interpreter/scope.hpp>
+#include <openscenario_interpreter/simulator_core.hpp>
+#include <openscenario_interpreter/syntax/directional_dimension.hpp>
+#include <openscenario_interpreter/syntax/rule.hpp>
+#include <openscenario_interpreter/syntax/triggering_entities.hpp>
+
+namespace openscenario_interpreter
+{
+inline namespace syntax
+{
+/*
+   RelativeSpeedCondition 1.3
+
+   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
+   evaluation is defined by the rule attribute. If direction is used, only the
+   projection to that direction is used in the comparison, with the triggering
+   entity/entities as the reference.
+
+   <xsd:complexType name="RelativeSpeedCondition">
+     <xsd:attribute name="entityRef" type="String" use="required"/>
+     <xsd:attribute name="rule" type="Rule" use="required"/>
+     <xsd:attribute name="value" type="Double" use="required"/>
+     <xsd:attribute name="direction" type="DirectionalDimension"/>
+   </xsd:complexType>
+*/
+struct RelativeSpeedCondition : private Scope, private SimulatorCore::ConditionEvaluation
+{
+  /*
+     Reference entity.
+  */
+  const EntityRef entity_ref;
+
+  /*
+     The operator (less, greater, equal).
+  */
+  const Rule rule;
+
+  /*
+     Relative speed value. Unit: [m/s]. Range: ]-inf..inf[. Relative speed is
+     defined as speed_rel = speed(triggering entity) - speed(reference entity)
+  */
+  const Double value;
+
+  /*
+     Direction of the speed (if not given, the total speed is considered).
+  */
+  const std::optional<DirectionalDimension> direction;
+
+  const TriggeringEntities triggering_entities;
+
+  std::vector<Double> evaluations;
+
+  explicit RelativeSpeedCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &);
+
+  auto description() const -> String;
+
+  static auto evaluate(
+    const EntityRef &, const EntityRef &, const Entities *,
+    const std::optional<DirectionalDimension> &) -> double;
+
+  auto evaluate() -> Object;
+};
+}  // namespace syntax
+}  // namespace openscenario_interpreter
+
+#endif  // OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_