Refactor/interpreter

docs/developer_guide/OpenSCENARIOSupport.md

@@ -55,7 +55,7 @@ The OpenSCENARIO XML standard [does not define](https://releases.asam.net/OpenSC
 
 ### Command
 
-OpenSCENARIO XML standard states that `CustomCommandAction` can be used to either issue a command to the simulation environment or start an external script. 
+OpenSCENARIO XML standard states that `CustomCommandAction` can be used to either issue a command to the simulation environment or start an external script.
 
 For OpenSCENARIO interpreters implemented in scripting languages such as Python, this action is often implemented as a call to an external script file written in the same language as the host language. However, `scenario_simulator_v2` is implemented in C++ and we cannot simply implement such a feature. Therefore, `scenario_simulator_v2` treats the string given in `CustomCommandAction.type` as a command and executes it on a subprocess, as `sh` does.
 
@@ -308,7 +308,7 @@ Currently, the only way to know the result of the simulation is by viewing the s
 | DynamicsDimension                            | 1.3               |                                            |
 | DynamicsShape                                | 1.3 (partial)     | [detail](#DynamicsShape)                   |
 | EndOfRoadCondition                           | unimplemented     |                                            |
-| Entities                                     | 1.3 (partial)     | [detail](#Entities)                        |
+| Entities                                     | 1.3.1             |                                            |
 | EntityAction                                 | 1.3               |                                            |
 | EntityCondition                              | 1.3 (partial)     | [detail](#EntityCondition)                 |
 | EntityDistribution                           | unimplemented     |                                            |
@@ -637,10 +637,6 @@ Currently, the only way to know the result of the simulation is by viewing the s
 
 - Enumeration literal `sinusoidal` is **not** supported.
 
-#### Entities
-
-- Property `EntitySelection` is **not** supported.
-
 #### EntityCondition
 
 - Properties `EndOfRoadCondition`, `OffroadCondition`, `TimeToCollisionCondition`, `RelativeDistanceCondition`, `TraveledDistanceCondition`, `AngleCondition`, and `RelativeAngleCondition` are **not** supported.

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

@@ -108,8 +108,6 @@ 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;

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

@@ -24,18 +24,25 @@ 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.1)
+
+   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>::at;
+  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;

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

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

openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <cmath>
+#include <openscenario_interpreter/cmath/hypot.hpp>
 #include <openscenario_interpreter/error.hpp>
 #include <openscenario_interpreter/reader/attribute.hpp>
 #include <openscenario_interpreter/reader/element.hpp>
@@ -46,12 +46,7 @@ DistanceCondition::DistanceCondition(
   value(readAttribute<Double>("value", node, scope)),
   position(readElement<Position>("Position", node, scope)),
   triggering_entities(triggering_entities),
-  results(triggering_entities.entity_refs.size(), {Double::nan()}),
-  consider_z([]() {
-    rclcpp::Node node{"get_parameter", "simulation"};
-    node.declare_parameter("consider_pose_by_road_slope", false);
-    return node.get_parameter("consider_pose_by_road_slope").as_bool();
-  }())
+  results(triggering_entities.entity_refs.size(), {Double::nan()})
 {
   std::set<RoutingAlgorithm::value_type> supported = {
     RoutingAlgorithm::value_type::shortest, RoutingAlgorithm::value_type::undefined};
@@ -134,12 +129,6 @@ auto DistanceCondition::distance(const EntityRef & triggering_entity) const -> d
   return Double::nan();
 }
 
-// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z)
-static double hypot(const double x, const double y, const double z, const bool consider_z)
-{
-  return consider_z ? std::hypot(x, y, z) : std::hypot(x, y);
-}
-
 template <>
 auto DistanceCondition::distance<
   CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined,
@@ -151,29 +140,25 @@ auto DistanceCondition::distance<
         const auto relative_world = makeNativeRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
         return hypot(
-          relative_world.position.x, relative_world.position.y, relative_world.position.z,
-          consider_z);
+          relative_world.position.x, relative_world.position.y, relative_world.position.z);
       },
       [&](const RelativeWorldPosition & position) {
         const auto relative_world = makeNativeRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
         return hypot(
-          relative_world.position.x, relative_world.position.y, relative_world.position.z,
-          consider_z);
+          relative_world.position.x, relative_world.position.y, relative_world.position.z);
       },
       [&](const RelativeObjectPosition & position) {
         const auto relative_world = makeNativeRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
         return hypot(
-          relative_world.position.x, relative_world.position.y, relative_world.position.z,
-          consider_z);
+          relative_world.position.x, relative_world.position.y, relative_world.position.z);
       },
       [&](const LanePosition & position) {
         const auto relative_world = makeNativeRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
         return hypot(
-          relative_world.position.x, relative_world.position.y, relative_world.position.z,
-          consider_z);
+          relative_world.position.x, relative_world.position.y, relative_world.position.z);
       }),
     position);
 }
@@ -189,29 +174,25 @@ auto DistanceCondition::distance<
         const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
         return hypot(
-          relative_world.position.x, relative_world.position.y, relative_world.position.z,
-          consider_z);
+          relative_world.position.x, relative_world.position.y, relative_world.position.z);
       },
       [&](const RelativeWorldPosition & position) {
         const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
         return hypot(
-          relative_world.position.x, relative_world.position.y, relative_world.position.z,
-          consider_z);
+          relative_world.position.x, relative_world.position.y, relative_world.position.z);
       },
       [&](const RelativeObjectPosition & position) {
         const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
         return hypot(
-          relative_world.position.x, relative_world.position.y, relative_world.position.z,
-          consider_z);
+          relative_world.position.x, relative_world.position.y, relative_world.position.z);
       },
       [&](const LanePosition & position) {
         const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition(
           triggering_entity, static_cast<NativeWorldPosition>(position));
         return hypot(
-          relative_world.position.x, relative_world.position.y, relative_world.position.z,
-          consider_z);
+          relative_world.position.x, relative_world.position.y, relative_world.position.z);
       }),
     position);
 }

openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <openscenario_interpreter/cmath/hypot.hpp>
 #include <openscenario_interpreter/reader/attribute.hpp>
 #include <openscenario_interpreter/reader/element.hpp>
 #include <openscenario_interpreter/simulator_core.hpp>
@@ -31,12 +32,7 @@ ReachPositionCondition::ReachPositionCondition(
   position(readElement<Position>("Position", node, scope)),
   compare(Rule::lessThan),
   triggering_entities(triggering_entities),
-  results(triggering_entities.entity_refs.size(), {Double::nan()}),
-  consider_z([]() {
-    rclcpp::Node node{"get_parameter", "simulation"};
-    node.declare_parameter("consider_pose_by_road_slope", false);
-    return node.get_parameter("consider_pose_by_road_slope").as_bool();
-  }())
+  results(triggering_entities.entity_refs.size(), {Double::nan()})
 {
 }
 
@@ -53,35 +49,29 @@ auto ReachPositionCondition::description() const -> String
   return description.str();
 }
 
-// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z)
-static double hypot(const double x, const double y, const double z, const bool consider_z)
-{
-  return consider_z ? std::hypot(x, y, z) : std::hypot(x, y);
-}
-
 auto ReachPositionCondition::evaluate() -> Object
 {
   // TODO USE DistanceCondition::distance
   const auto distance = overload(
     [&](const WorldPosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
+      return hypot(pose.position.x, pose.position.y, pose.position.z);
     },
     [&](const RelativeWorldPosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
+      return hypot(pose.position.x, pose.position.y, pose.position.z);
     },
     [&](const RelativeObjectPosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
+      return hypot(pose.position.x, pose.position.y, pose.position.z);
     },
     [&](const LanePosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
+      return hypot(pose.position.x, pose.position.y, pose.position.z);
     });
 
   results.clear();

openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <openscenario_interpreter/cmath/hypot.hpp>
 #include <openscenario_interpreter/error.hpp>
 #include <openscenario_interpreter/reader/attribute.hpp>
 #include <openscenario_interpreter/syntax/entities.hpp>  // TEMPORARY (TODO REMOVE THIS LINE)
@@ -39,12 +40,7 @@ RelativeDistanceCondition::RelativeDistanceCondition(
   rule(readAttribute<Rule>("rule", node, scope)),
   value(readAttribute<Double>("value", node, scope)),
   triggering_entities(triggering_entities),
-  results(triggering_entities.entity_refs.size(), {Double::nan()}),
-  consider_z([]() {
-    rclcpp::Node node{"get_parameter", "simulation"};
-    node.declare_parameter("consider_pose_by_road_slope", false);
-    return node.get_parameter("consider_pose_by_road_slope").as_bool();
-  }())
+  results(triggering_entities.entity_refs.size(), {Double::nan()})
 {
   std::set<RoutingAlgorithm::value_type> supported = {
     RoutingAlgorithm::value_type::shortest, RoutingAlgorithm::value_type::undefined};
@@ -132,12 +128,6 @@ auto RelativeDistanceCondition::distance<
   }
 }
 
-// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z)
-static double hypot(const double x, const double y, const double z, const bool consider_z)
-{
-  return consider_z ? std::hypot(x, y, z) : std::hypot(x, y);
-}
-
 template <>
 auto RelativeDistanceCondition::distance<
   CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined,
@@ -149,8 +139,7 @@ auto RelativeDistanceCondition::distance<
     return hypot(
       makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x,
       makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y,
-      makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.z,
-      consider_z);
+      makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.z);
   } else {
     return Double::nan();
   }
@@ -167,7 +156,7 @@ auto RelativeDistanceCondition::distance<
     return hypot(
       makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x,
       makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y,
-      makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.z, consider_z);
+      makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.z);
   } else {
     return Double::nan();
   }