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/cmath/hypot.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 OPENSCENARIO_INTERPRETER__CMATH__HYPOT_HPP_
+#define OPENSCENARIO_INTERPRETER__CMATH__HYPOT_HPP_
+
+#include <cmath>
+#include <rclcpp/rclcpp.hpp>
+
+namespace openscenario_interpreter
+{
+inline namespace cmath
+{
+/*
+   @todo: after checking all the scenario work well with
+   consider_pose_by_road_slope = true, remove this function and use
+   std::hypot(x, y, z)
+*/
+template <typename T>
+auto hypot(T x, T y, T z)
+{
+  static const auto consider_pose_by_road_slope = []() {
+    auto node = rclcpp::Node("get_parameter", "simulation");
+    node.declare_parameter("consider_pose_by_road_slope", false);
+    return node.get_parameter("consider_pose_by_road_slope").as_bool();
+  }();
+
+  return consider_pose_by_road_slope ? std::hypot(x, y, z) : std::hypot(x, y);
+}
+}  // namespace cmath
+}  // namespace openscenario_interpreter
+
+#endif  // OPENSCENARIO_INTERPRETER__CMATH__HYPOT_HPP_

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>
@@ -533,7 +534,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
@@ -551,6 +551,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
     {
@@ -564,7 +571,7 @@ class SimulatorCore
     template <typename... Ts>
     static auto evaluateSpeed(Ts &&... xs)
     {
-      return core->getCurrentTwist(std::forward<decltype(xs)>(xs)...).linear.x;
+      return core->getCurrentTwist(std::forward<decltype(xs)>(xs)...).linear;
     }
 
     template <typename... Ts>

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/distance_condition.hpp

@@ -108,43 +108,43 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua
 
   std::vector<std::valarray<double>> results;  // for description
 
-  const bool consider_z;
-
   explicit DistanceCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &);
 
   auto description() const -> std::string;
 
-  auto distance(const EntityRef &) const -> double;
-
   template <
     CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type,
     Boolean::value_type>
-  auto distance(const EntityRef &) const -> double
+  static auto distance(const EntityRef &, const Position &) -> double
   {
     throw SyntaxError(__FILE__, ":", __LINE__);
   }
 
+  static auto evaluate(
+    const Entities *, const EntityRef &, const Position &, CoordinateSystem, RelativeDistanceType,
+    RoutingAlgorithm, Boolean) -> double;
+
   auto evaluate() -> Object;
 };
 
 // Ignore spell miss due to OpenSCENARIO standard.
 // cspell: ignore euclidian
 
 // clang-format off
-template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, false>(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, true >(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, false>(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, true >(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, false>(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, true >(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, false>(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, true >(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, false>(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, true >(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::shortest,  false>(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::shortest,  true >(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::shortest,  false>(const EntityRef &) const -> double;
-template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::shortest,  true >(const EntityRef &) const -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, false>(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, true >(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, false>(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, true >(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, false>(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::entity, RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, true >(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, false>(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::undefined, true >(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, false>(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::undefined, true >(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::shortest,  false>(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::lateral,           RoutingAlgorithm::shortest,  true >(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::shortest,  false>(const EntityRef &, const Position &) -> double;
+template <> auto DistanceCondition::distance<CoordinateSystem::lane,   RelativeDistanceType::longitudinal,      RoutingAlgorithm::shortest,  true >(const EntityRef &, const Position &) -> double;
 // clang-format on
 }  // namespace syntax
 }  // namespace openscenario_interpreter

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp

@@ -24,18 +24,24 @@ namespace openscenario_interpreter
 {
 inline namespace syntax
 {
-/* ---- Entities ---------------------------------------------------------------
- *
- *  <xsd:complexType name="Entities">
- *    <xsd:sequence>
- *      <xsd:element name="ScenarioObject" minOccurs="0" maxOccurs="unbounded" type="ScenarioObject"/>
- *      <xsd:element name="EntitySelection" minOccurs="0" maxOccurs="unbounded" type="EntitySelection"/>
- *    </xsd:sequence>
- *  </xsd:complexType>
- *
- * -------------------------------------------------------------------------- */
-struct Entities : public std::unordered_map<std::string, Object>  // TODO to be data member
+/*
+   Entities (OpenSCENARIO XML 1.3)
+
+   Definition of entities (scenario objects or entity selections) in
+   a scenario.
+
+   <xsd:complexType name="Entities">
+     <xsd:sequence>
+       <xsd:element name="ScenarioObject" type="ScenarioObject" minOccurs="0" maxOccurs="unbounded"/>
+       <xsd:element name="EntitySelection" type="EntitySelection" minOccurs="0" maxOccurs="unbounded"/>
+     </xsd:sequence>
+   </xsd:complexType>
+*/
+struct Entities : private std::unordered_map<std::string, Object>
 {
+  using std::unordered_map<std::string, Object>::begin;
+  using std::unordered_map<std::string, Object>::end;
+
   explicit Entities(const pugi::xml_node &, Scope &);
 
   auto isAdded(const Entity &) const -> bool;

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp

@@ -50,8 +50,6 @@ struct ReachPositionCondition : private SimulatorCore::CoordinateSystemConversio
 
   std::vector<std::valarray<double>> results;  // for description
 
-  const bool consider_z;
-
   explicit ReachPositionCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &);
 
   auto description() const -> String;