From 1abe1bda84869f381d0d3b6f5b64e8e7bd5636cc Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 21 May 2024 15:05:48 +0900 Subject: [PATCH 01/11] Add new struct `TimeToCollisionCondition` Signed-off-by: yamacir-kit --- .../syntax/time_to_collision_condition.hpp | 126 ++++++++++++++++++ .../src/syntax/entity_condition.cpp | 3 +- 2 files changed, 128 insertions(+), 1 deletion(-) create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp new file mode 100644 index 00000000000..aff3deeec54 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -0,0 +1,126 @@ +// 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 +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + TimeToCollisionCondition + + 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 1.3 Representation + + + + + + + + + deprecated + + + + + + + + + + +*/ +struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation +{ + 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 evaluations; + + explicit TimeToCollisionCondition( + const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) + : freespace(readAttribute("freespace", node, scope)), + rule(readAttribute("rule", node, scope)), + value(readAttribute("value", node, scope)), + relative_distance_type( + readAttribute("relativeDistanceType", node, scope)), + coordinate_system( + readAttribute("coordinateSystem", node, scope, CoordinateSystem::entity)), + routing_algorithm(readAttribute( + "routingAlgorithm", node, scope, RoutingAlgorithm::undefined)), + triggering_entities(triggering_entities), + evaluations(triggering_entities.entity_refs.size(), Double::nan()) + { + } + + auto description() const -> String + { + auto description = std::stringstream(); + + description << triggering_entities.description() << "'s time-to-collision to given entity " + << "TODO-RELATIVE-DISTANCE-TARGET" + << " = "; + + print_to(description, evaluations); + + description << " " << rule << " " << value << "?"; + + return description.str(); + } + + auto evaluate() + { + evaluations.clear(); + + return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { + [[deprecated]] auto evaluateTimeToCollision = [](auto &&...) { return Double(); }; + evaluations.push_back(evaluateTimeToCollision(triggering_entity, "TODO")); + return std::invoke(rule, evaluations.back(), value); + })); + } +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_HPP_ diff --git a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp index 9577b8f9453..a0b9aca1a00 100644 --- a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace openscenario_interpreter { @@ -36,7 +37,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); }), From 3a6c2b9a4981fb311fdd6314d38f7f723a4bdc6f Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 21 May 2024 15:26:31 +0900 Subject: [PATCH 02/11] Add new struct `TimeToCollisionConditionTarget` Signed-off-by: yamacir-kit --- .../syntax/time_to_collision_condition.hpp | 11 ++-- .../time_to_collision_condition_target.hpp | 54 +++++++++++++++++++ 2 files changed, 61 insertions(+), 4 deletions(-) create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index aff3deeec54..6bb98167f53 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -17,6 +17,7 @@ #include #include +#include #include namespace openscenario_interpreter @@ -24,7 +25,7 @@ namespace openscenario_interpreter inline namespace syntax { /* - TimeToCollisionCondition + TimeToCollisionCondition 1.3 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 @@ -39,8 +40,6 @@ inline namespace syntax deprecated. If "coordinateSystem" or "relativeDistanceType" are set, "alongRoute" is ignored. - XSD 1.3 Representation - @@ -62,6 +61,8 @@ inline namespace syntax */ struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation { + const TimeToCollisionConditionTarget time_to_collision_condition_target; + const Boolean freespace; const Rule rule; @@ -80,7 +81,9 @@ struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation explicit TimeToCollisionCondition( const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) - : freespace(readAttribute("freespace", node, scope)), + : time_to_collision_condition_target( + readElement("TimeToCollisionConditionTarget", node, scope)), + freespace(readAttribute("freespace", node, scope)), rule(readAttribute("rule", node, scope)), value(readAttribute("value", node, scope)), relative_distance_type( diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp new file mode 100644 index 00000000000..960a87e9660 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp @@ -0,0 +1,54 @@ +// 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 +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + TimeToCollisionConditionTarget 1.3 + + Target position used in the TimeToCollisionCondition. Can be defined as + either an explicit position, or the position of a reference entity. + + + + + + + +*/ +struct TimeToCollisionConditionTarget : public ComplexType +{ + explicit 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(node), scope); }), + std::make_pair("EntityRef", [&](auto && node) { return make(std::forward(node), scope); }))) + // clang-format on + { + } +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_ From 5447363cde632787022eb475ff1c4b8840357ce2 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 21 May 2024 16:16:53 +0900 Subject: [PATCH 03/11] Add new structs `RelativeSpeedCondition` and `DirectionalDimension` Signed-off-by: yamacir-kit --- .../syntax/directional_dimension.hpp | 108 ++++++++++++++++++ .../syntax/relative_speed_condition.hpp | 96 ++++++++++++++++ .../syntax/time_to_collision_condition.hpp | 6 +- .../src/syntax/entity_condition.cpp | 3 +- 4 files changed, 209 insertions(+), 4 deletions(-) create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/directional_dimension.hpp create mode 100644 openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/directional_dimension.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/directional_dimension.hpp new file mode 100644 index 00000000000..6bafc12b920 --- /dev/null +++ b/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. + + + + + + + + + + + + + + + +*/ +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_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp new file mode 100644 index 00000000000..96d6223e9db --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp @@ -0,0 +1,96 @@ +// 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 +#include +#include +#include + +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. + + + + + + + +*/ +struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation +{ + const EntityRef entity_ref; + + const Rule rule; + + const Double value; + + const DirectionalDimension direction; + + const TriggeringEntities triggering_entities; + + std::vector evaluations; + + explicit RelativeSpeedCondition( + const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) + : entity_ref(readAttribute("entityRef", node, scope)), + rule(readAttribute("rule", node, scope)), + value(readAttribute("value", node, scope)), + direction(readAttribute("direction", node, scope)), + triggering_entities(triggering_entities), + evaluations(triggering_entities.entity_refs.size(), Double::nan()) + { + } + + auto description() const + { + auto description = std::stringstream(); + + description << triggering_entities.description() << "'s relative speed to given entity " + << entity_ref << " = "; + + print_to(description, evaluations); + + description << " " << rule << " " << value << "?"; + + return description.str(); + } + + auto evaluate() + { + evaluations.clear(); + + return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { + [[deprecated]] auto evaluateRelativeSpeed = [](auto &&...) { return Double(); }; + evaluations.push_back(evaluateRelativeSpeed(triggering_entity, entity_ref)); + return std::invoke(rule, evaluations.back(), value); + })); + } +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 6bb98167f53..2e8eee42cb2 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -25,7 +25,7 @@ namespace openscenario_interpreter inline namespace syntax { /* - TimeToCollisionCondition 1.3 + TimeToCollisionCondition (OpenSCENARIO XML 1.3) 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 @@ -97,11 +97,11 @@ struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation { } - auto description() const -> String + auto description() const { auto description = std::stringstream(); - description << triggering_entities.description() << "'s time-to-collision to given entity " + description << triggering_entities.description() << "'s time to collision to given entity " << "TODO-RELATIVE-DISTANCE-TARGET" << " = "; diff --git a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp index a0b9aca1a00..bf25305d994 100644 --- a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -41,7 +42,7 @@ EntityCondition::EntityCondition( 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); }), - std::make_pair( "RelativeSpeedCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "RelativeSpeedCondition", [&](const auto & node) { return make< RelativeSpeedCondition>(node, scope, triggering_entities); }), std::make_pair("TraveledDistanceCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), std::make_pair( "ReachPositionCondition", [&](const auto & node) { return make< ReachPositionCondition>(node, scope, triggering_entities); }), std::make_pair( "DistanceCondition", [&](const auto & node) { return make< DistanceCondition>(node, scope, triggering_entities); }), From b09ca1fc3ce9a855ef46c7309419fc5700573e90 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Tue, 21 May 2024 18:04:41 +0900 Subject: [PATCH 04/11] Add static member function `ConditionEvaluation::evaluateRelativeSpeed` Signed-off-by: yamacir-kit --- .../reader/attribute.hpp | 11 +++++ .../simulator_core.hpp | 20 ++++++--- .../syntax/relative_speed_condition.hpp | 45 ++++++++++++++++--- 3 files changed, 64 insertions(+), 12 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp index 06568802cbe..945e1a346a3 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -164,6 +165,16 @@ auto readAttribute(const std::string & name, const Node & node, const Scope & sc return value; } } + +template +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(name, node, scope)); + } else { + return std::optional(); + } +} } // namespace reader } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index ed80b3e3879..79f5f7239d6 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -15,6 +15,7 @@ #ifndef OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_ #define OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_ +#include #include #include #include @@ -482,12 +483,6 @@ class SimulatorCore return core->getCurrentAccel(std::forward(xs)...).linear.x; } - template - static auto evaluateCollisionCondition(Ts &&... xs) -> bool - { - return core->checkCollision(std::forward(xs)...); - } - template static auto evaluateBoundingBoxEuclideanDistance(Ts &&... xs) // for RelativeDistanceCondition { @@ -500,6 +495,19 @@ class SimulatorCore } } + template + static auto evaluateCollisionCondition(Ts &&... xs) -> bool + { + return core->checkCollision(std::forward(xs)...); + } + + template + static auto evaluateRelativeSpeed(const T & x, const U & y) + { + using math::geometry::operator-; + return core->getCurrentTwist(x).linear - core->getCurrentTwist(y).linear; + } + template static auto evaluateSimulationTime(Ts &&... xs) -> double { diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp index 96d6223e9db..d535388c5dc 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp @@ -15,6 +15,7 @@ #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ #define OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ +#include #include #include #include @@ -42,13 +43,26 @@ inline namespace syntax */ struct RelativeSpeedCondition : 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; - const DirectionalDimension direction; + /* + Direction of the speed (if not given, the total speed is considered). + */ + const std::optional direction; const TriggeringEntities triggering_entities; @@ -59,7 +73,7 @@ struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation : entity_ref(readAttribute("entityRef", node, scope)), rule(readAttribute("rule", node, scope)), value(readAttribute("value", node, scope)), - direction(readAttribute("direction", node, scope)), + direction(readAttribute("direction", node, scope, std::nullopt)), triggering_entities(triggering_entities), evaluations(triggering_entities.entity_refs.size(), Double::nan()) { @@ -69,8 +83,13 @@ struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation { auto description = std::stringstream(); - description << triggering_entities.description() << "'s relative speed to given entity " - << entity_ref << " = "; + description << triggering_entities.description() << "'s relative "; + + if (direction) { + description << *direction << " "; + } + + description << "speed to given entity " << entity_ref << " = "; print_to(description, evaluations); @@ -84,8 +103,22 @@ struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation evaluations.clear(); return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { - [[deprecated]] auto evaluateRelativeSpeed = [](auto &&...) { return Double(); }; - evaluations.push_back(evaluateRelativeSpeed(triggering_entity, entity_ref)); + evaluations.push_back([this](auto && v) { + if (direction) { + switch (*direction) { + case DirectionalDimension::longitudinal: + return v.x; + case DirectionalDimension::lateral: + return v.y; + case DirectionalDimension::vertical: + return v.z; + default: + return math::geometry::norm(v); + } + } else { + return math::geometry::norm(v); + } + }(evaluateRelativeSpeed(triggering_entity, entity_ref))); return std::invoke(rule, evaluations.back(), value); })); } From d270cdcaf0d100b0949aff869756959029fa8c72 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 22 May 2024 16:58:51 +0900 Subject: [PATCH 05/11] Move entity existence check into `distance` from speceialized `distance` Signed-off-by: yamacir-kit --- .../syntax/relative_distance_condition.cpp | 385 +++++++----------- 1 file changed, 157 insertions(+), 228 deletions(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 6309d00bbb3..56d28b7388d 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -66,31 +66,20 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x); - } else { - return Double::nan(); - } + return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x); } -/** - * @note This implementation differs from the OpenSCENARIO standard. See the section "6.4. Distances" in the OpenSCENARIO User Guide. - */ template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - return std::abs( - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x); - } else { - return Double::nan(); - } + /** + @note This implementation differs from the OpenSCENARIO standard. See the + section "6.4. Distances" in the OpenSCENARIO User Guide. + */ + return std::abs( + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x); } template <> @@ -98,35 +87,24 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y); - } else { - return Double::nan(); - } + return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y); } -/** - * @note This implementation differs from the OpenSCENARIO standard. See the section "6.4. Distances" in the OpenSCENARIO User Guide. - */ template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - return std::abs( - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y); - } else { - return Double::nan(); - } + /** + @note This implementation differs from the OpenSCENARIO standard. See the + section "6.4. Distances" in the OpenSCENARIO User Guide. + */ + return std::abs( + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y); } -// @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) +// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x, y, z) +auto hypot(double x, double y, double z, bool consider_z) { return consider_z ? std::hypot(x, y, z) : std::hypot(x, y); } @@ -136,17 +114,11 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, true>(const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - 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); - } else { - return Double::nan(); - } + 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); } template <> @@ -154,16 +126,10 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, false>(const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { - 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); - } else { - return Double::nan(); - } + 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); } template <> @@ -171,27 +137,21 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().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( - makeNativeRelativeLanePosition(triggering_entity, entity_ref)) - .offset; - } else { - return Double::nan(); - } + /* + 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( + makeNativeRelativeLanePosition(triggering_entity, entity_ref)) + .offset; } template <> @@ -199,27 +159,21 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().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( - makeNativeBoundingBoxRelativeLanePosition(triggering_entity, entity_ref)) - .offset; - } else { - return Double::nan(); - } + /* + 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( + makeNativeBoundingBoxRelativeLanePosition(triggering_entity, entity_ref)) + .offset; } template <> @@ -227,27 +181,21 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().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( - makeNativeRelativeLanePosition(triggering_entity, entity_ref)) - .s; - } else { - return Double::nan(); - } + /* + 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( + makeNativeRelativeLanePosition(triggering_entity, entity_ref)) + .s; } template <> @@ -255,27 +203,21 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().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( - makeNativeBoundingBoxRelativeLanePosition(triggering_entity, entity_ref)) - .s; - } else { - return Double::nan(); - } + /* + 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( + makeNativeBoundingBoxRelativeLanePosition(triggering_entity, entity_ref)) + .s; } template <> @@ -283,16 +225,10 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, entity_ref, RoutingAlgorithm::shortest)) - .offset); - } else { - return Double::nan(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, entity_ref, RoutingAlgorithm::shortest)) + .offset); } template <> @@ -300,16 +236,10 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().is_added) { - return std::abs( - static_cast( - makeNativeRelativeLanePosition(triggering_entity, entity_ref, RoutingAlgorithm::shortest)) - .offset); - } else { - return Double::nan(); - } + return std::abs( + static_cast( + makeNativeRelativeLanePosition(triggering_entity, entity_ref, RoutingAlgorithm::shortest)) + .offset); } template <> @@ -317,16 +247,10 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, true>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, entity_ref, RoutingAlgorithm::shortest)) - .s); - } else { - return Double::nan(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, entity_ref, RoutingAlgorithm::shortest)) + .s); } template <> @@ -334,64 +258,64 @@ auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, false>( const EntityRef & triggering_entity) -> double { - if ( - global().entities->at(entity_ref).as().is_added and - global().entities->at(triggering_entity).as().is_added) { - return std::abs( - static_cast( - makeNativeRelativeLanePosition(triggering_entity, entity_ref, RoutingAlgorithm::shortest)) - .s); - } else { - return Double::nan(); - } + return std::abs( + static_cast( + makeNativeRelativeLanePosition(triggering_entity, entity_ref, RoutingAlgorithm::shortest)) + .s); } -#define SWITCH_COORDINATE_SYSTEM(FUNCTION, ...) \ - switch (coordinate_system) { \ - case CoordinateSystem::entity: \ - FUNCTION(__VA_ARGS__, CoordinateSystem::entity); \ - break; \ - case CoordinateSystem::lane: \ - FUNCTION(__VA_ARGS__, CoordinateSystem::lane); \ - break; \ - case CoordinateSystem::road: \ - FUNCTION(__VA_ARGS__, CoordinateSystem::road); \ - break; \ - case CoordinateSystem::trajectory: \ - FUNCTION(__VA_ARGS__, CoordinateSystem::trajectory); \ - break; \ +#define SWITCH_COORDINATE_SYSTEM(FUNCTION, ...) \ + switch (coordinate_system) { \ + case CoordinateSystem::entity: \ + FUNCTION(__VA_ARGS__, CoordinateSystem::entity); \ + break; \ + case CoordinateSystem::lane: \ + FUNCTION(__VA_ARGS__, CoordinateSystem::lane); \ + break; \ + case CoordinateSystem::road: \ + FUNCTION(__VA_ARGS__, CoordinateSystem::road); \ + break; \ + case CoordinateSystem::trajectory: \ + FUNCTION(__VA_ARGS__, CoordinateSystem::trajectory); \ + break; \ + default: \ + throw UNEXPECTED_ENUMERATION_VALUE_ASSIGNED(CoordinateSystem, coordinate_system); \ } -#define SWITCH_RELATIVE_DISTANCE_TYPE(FUNCTION, ...) \ - switch (relative_distance_type) { \ - case RelativeDistanceType::longitudinal: \ - FUNCTION(__VA_ARGS__, RelativeDistanceType::longitudinal); \ - break; \ - case RelativeDistanceType::lateral: \ - FUNCTION(__VA_ARGS__, RelativeDistanceType::lateral); \ - break; \ - case RelativeDistanceType::euclidianDistance: \ - FUNCTION(__VA_ARGS__, RelativeDistanceType::euclidianDistance); \ - break; \ +#define SWITCH_RELATIVE_DISTANCE_TYPE(FUNCTION, ...) \ + switch (relative_distance_type) { \ + case RelativeDistanceType::longitudinal: \ + FUNCTION(__VA_ARGS__, RelativeDistanceType::longitudinal); \ + break; \ + case RelativeDistanceType::lateral: \ + FUNCTION(__VA_ARGS__, RelativeDistanceType::lateral); \ + break; \ + case RelativeDistanceType::euclidianDistance: \ + FUNCTION(__VA_ARGS__, RelativeDistanceType::euclidianDistance); \ + break; \ + default: \ + throw UNEXPECTED_ENUMERATION_VALUE_ASSIGNED(RelativeDistanceType, relative_distance_type); \ } -#define SWITCH_ROUTING_ALGORITHM(FUNCTION, ...) \ - switch (routing_algorithm) { \ - case RoutingAlgorithm::assigned_route: \ - FUNCTION(__VA_ARGS__, RoutingAlgorithm::assigned_route); \ - break; \ - case RoutingAlgorithm::fastest: \ - FUNCTION(__VA_ARGS__, RoutingAlgorithm::fastest); \ - break; \ - case RoutingAlgorithm::least_intersections: \ - FUNCTION(__VA_ARGS__, RoutingAlgorithm::least_intersections); \ - break; \ - case RoutingAlgorithm::shortest: \ - FUNCTION(__VA_ARGS__, RoutingAlgorithm::shortest); \ - break; \ - case RoutingAlgorithm::undefined: \ - FUNCTION(__VA_ARGS__, RoutingAlgorithm::undefined); \ - break; \ +#define SWITCH_ROUTING_ALGORITHM(FUNCTION, ...) \ + switch (routing_algorithm) { \ + case RoutingAlgorithm::assigned_route: \ + FUNCTION(__VA_ARGS__, RoutingAlgorithm::assigned_route); \ + break; \ + case RoutingAlgorithm::fastest: \ + FUNCTION(__VA_ARGS__, RoutingAlgorithm::fastest); \ + break; \ + case RoutingAlgorithm::least_intersections: \ + FUNCTION(__VA_ARGS__, RoutingAlgorithm::least_intersections); \ + break; \ + case RoutingAlgorithm::shortest: \ + FUNCTION(__VA_ARGS__, RoutingAlgorithm::shortest); \ + break; \ + case RoutingAlgorithm::undefined: \ + FUNCTION(__VA_ARGS__, RoutingAlgorithm::undefined); \ + break; \ + default: \ + throw UNEXPECTED_ENUMERATION_VALUE_ASSIGNED(RoutingAlgorithm, routing_algorithm); \ } #define SWITCH_FREESPACE(FUNCTION, ...) \ @@ -401,9 +325,14 @@ auto RelativeDistanceCondition::distance< auto RelativeDistanceCondition::distance(const EntityRef & triggering_entity) -> double { - SWITCH_COORDINATE_SYSTEM( - SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); - return Double::nan(); + if ( + global().entities->at(triggering_entity).as().is_added and + global().entities->at(entity_ref).as().is_added) { + SWITCH_COORDINATE_SYSTEM( + SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); + } else { + return Double::nan(); + } } auto RelativeDistanceCondition::evaluate() -> Object From 3be8459641964857a2b9d89cc195957a2aed6023 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 22 May 2024 18:58:29 +0900 Subject: [PATCH 06/11] Update `RelativeDistanceCondition::distance` to static member function Signed-off-by: yamacir-kit --- .../syntax/relative_distance_condition.hpp | 36 +++++----- .../syntax/relative_distance_condition.cpp | 72 ++++++++++--------- 2 files changed, 55 insertions(+), 53 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp index 39926681b26..846eca5f40b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp @@ -90,8 +90,6 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi std::vector results; // for description - const bool consider_z; - explicit RelativeDistanceCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); auto description() const -> String; @@ -99,12 +97,14 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi 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(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; -template <> auto RelativeDistanceCondition::distance(const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; +template <> auto RelativeDistanceCondition::distance(const EntityRef &, const EntityRef &) -> double; // clang-format on } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 56d28b7388d..4fe56f1e425 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -38,12 +38,7 @@ RelativeDistanceCondition::RelativeDistanceCondition( rule(readAttribute("rule", node, scope)), value(readAttribute("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()) { } @@ -64,7 +59,7 @@ auto RelativeDistanceCondition::description() const -> String template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x); } @@ -72,7 +67,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /** @note This implementation differs from the OpenSCENARIO standard. See the @@ -85,7 +80,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs(makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y); } @@ -93,7 +88,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /** @note This implementation differs from the OpenSCENARIO standard. See the @@ -104,38 +99,40 @@ 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) -auto hypot(double x, double y, double z, bool consider_z) +auto hypot(double x, double y, double z) { + static auto consider_z = []() { + 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_z ? std::hypot(x, y, z) : std::hypot(x, y); } template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - true>(const EntityRef & triggering_entity) -> double + true>(const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { - 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); + const auto relative_world = + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref); + return hypot(relative_world.position.x, relative_world.position.y, relative_world.position.z); } template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - false>(const EntityRef & triggering_entity) -> double + false>(const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { - 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); + const auto relative_world = makeNativeRelativeWorldPosition(triggering_entity, entity_ref); + return hypot(relative_world.position.x, relative_world.position.y, relative_world.position.z); } template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /* For historical reasons, signed distances are returned when @@ -157,7 +154,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /* For historical reasons, signed distances are returned when @@ -179,7 +176,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /* For historical reasons, signed distances are returned when @@ -201,7 +198,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { /* For historical reasons, signed distances are returned when @@ -223,7 +220,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs(static_cast( makeNativeBoundingBoxRelativeLanePosition( @@ -234,7 +231,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs( static_cast( @@ -245,7 +242,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs(static_cast( makeNativeBoundingBoxRelativeLanePosition( @@ -256,7 +253,7 @@ auto RelativeDistanceCondition::distance< template <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { return std::abs( static_cast( @@ -321,13 +318,16 @@ auto RelativeDistanceCondition::distance< #define SWITCH_FREESPACE(FUNCTION, ...) \ return freespace ? FUNCTION(__VA_ARGS__, true) : FUNCTION(__VA_ARGS__, false) -#define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity) +#define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, entity_ref) -auto RelativeDistanceCondition::distance(const EntityRef & triggering_entity) -> double +auto RelativeDistanceCondition::distance( + const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities & entities, + CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, + RoutingAlgorithm routing_algorithm, bool freespace) -> double { if ( - global().entities->at(triggering_entity).as().is_added and - global().entities->at(entity_ref).as().is_added) { + entities.at(triggering_entity).as().is_added and + entities.at(entity_ref).as().is_added) { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); } else { @@ -340,7 +340,9 @@ auto RelativeDistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { - results.push_back(distance(triggering_entity)); + results.push_back(distance( + triggering_entity, entity_ref, *global().entities, coordinate_system, relative_distance_type, + routing_algorithm, freespace)); return rule(static_cast(results.back()), value); })); } From 6535eebbe8d9a77a7aff6dc493b41ba994b5d0a1 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 3 Jun 2024 11:24:46 +0900 Subject: [PATCH 07/11] Add new static member function `RelativeSpeedCondition::evaluate` Signed-off-by: yamacir-kit --- .../include/geometry/vector3/norm.hpp | 1 + .../syntax/relative_distance_condition.hpp | 2 +- .../syntax/relative_speed_condition.hpp | 63 ++----------- .../syntax/time_to_collision_condition.hpp | 30 +++++- .../syntax/relative_distance_condition.cpp | 8 +- .../src/syntax/relative_speed_condition.cpp | 92 +++++++++++++++++++ 6 files changed, 133 insertions(+), 63 deletions(-) create mode 100644 openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp diff --git a/common/math/geometry/include/geometry/vector3/norm.hpp b/common/math/geometry/include/geometry/vector3/norm.hpp index 44fa57c9e62..6746c7ec7c6 100644 --- a/common/math/geometry/include/geometry/vector3/norm.hpp +++ b/common/math/geometry/include/geometry/vector3/norm.hpp @@ -15,6 +15,7 @@ #ifndef GEOMETRY__VECTOR3__NORM_HPP_ #define GEOMETRY__VECTOR3__NORM_HPP_ +#include #include namespace math diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp index 846eca5f40b..475f91ff739 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp @@ -103,7 +103,7 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi } static auto distance( - const EntityRef &, const EntityRef &, const Entities &, CoordinateSystem, RelativeDistanceType, + const EntityRef &, const EntityRef &, const Entities *, CoordinateSystem, RelativeDistanceType, RoutingAlgorithm, bool) -> double; auto evaluate() -> Object; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp index d535388c5dc..d42b50db0dd 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp @@ -15,11 +15,11 @@ #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ #define OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ -#include #include #include #include -#include +#include +#include namespace openscenario_interpreter { @@ -41,7 +41,7 @@ inline namespace syntax */ -struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation +struct RelativeSpeedCondition : private Scope, private SimulatorCore::ConditionEvaluation { /* Reference entity. @@ -68,60 +68,15 @@ struct RelativeSpeedCondition : private SimulatorCore::ConditionEvaluation std::vector evaluations; - explicit RelativeSpeedCondition( - const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) - : entity_ref(readAttribute("entityRef", node, scope)), - rule(readAttribute("rule", node, scope)), - value(readAttribute("value", node, scope)), - direction(readAttribute("direction", node, scope, std::nullopt)), - triggering_entities(triggering_entities), - evaluations(triggering_entities.entity_refs.size(), Double::nan()) - { - } + explicit RelativeSpeedCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); - auto description() const - { - auto description = std::stringstream(); + auto description() const -> String; - description << triggering_entities.description() << "'s relative "; + static auto evaluate( + const EntityRef &, const EntityRef &, const Entities *, + const std::optional &) -> double; - if (direction) { - description << *direction << " "; - } - - description << "speed to given entity " << entity_ref << " = "; - - print_to(description, evaluations); - - description << " " << rule << " " << value << "?"; - - return description.str(); - } - - auto evaluate() - { - evaluations.clear(); - - return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { - evaluations.push_back([this](auto && v) { - if (direction) { - switch (*direction) { - case DirectionalDimension::longitudinal: - return v.x; - case DirectionalDimension::lateral: - return v.y; - case DirectionalDimension::vertical: - return v.z; - default: - return math::geometry::norm(v); - } - } else { - return math::geometry::norm(v); - } - }(evaluateRelativeSpeed(triggering_entity, entity_ref))); - return std::invoke(rule, evaluations.back(), value); - })); - } + auto evaluate() -> Object; }; } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 2e8eee42cb2..39e7e776a1d 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #include #include @@ -59,7 +61,7 @@ inline namespace syntax */ -struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation +struct TimeToCollisionCondition : private Scope, private SimulatorCore::ConditionEvaluation { const TimeToCollisionConditionTarget time_to_collision_condition_target; @@ -81,7 +83,8 @@ struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation explicit TimeToCollisionCondition( const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) - : time_to_collision_condition_target( + : Scope(scope), + time_to_collision_condition_target( readElement("TimeToCollisionConditionTarget", node, scope)), freespace(readAttribute("freespace", node, scope)), rule(readAttribute("rule", node, scope)), @@ -117,8 +120,27 @@ struct TimeToCollisionCondition : private SimulatorCore::ConditionEvaluation evaluations.clear(); return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { - [[deprecated]] auto evaluateTimeToCollision = [](auto &&...) { return Double(); }; - evaluations.push_back(evaluateTimeToCollision(triggering_entity, "TODO")); + [[deprecated]] auto evaluateTimeToCollision = + [this]( + const EntityRef & triggering_entity, + const TimeToCollisionConditionTarget & time_to_collision_condition_target) { + if (time_to_collision_condition_target.is()) { + std::cerr << "RELATIVE DISTANCE = " + << RelativeDistanceCondition::distance( + triggering_entity, time_to_collision_condition_target.as(), + global().entities, coordinate_system, relative_distance_type, + routing_algorithm, freespace) + << ", " + << "RELATIVE SPEED = " + << RelativeSpeedCondition::evaluate( + triggering_entity, time_to_collision_condition_target.as(), + global().entities, std::nullopt) + << std::endl; + } + return Double(); + }; + evaluations.push_back( + evaluateTimeToCollision(triggering_entity, time_to_collision_condition_target)); return std::invoke(rule, evaluations.back(), value); })); } diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 8740ef9761c..349ebae69ae 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -328,13 +328,13 @@ auto RelativeDistanceCondition::distance< #define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, entity_ref) auto RelativeDistanceCondition::distance( - const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities & entities, + const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities * entities, CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, RoutingAlgorithm routing_algorithm, bool freespace) -> double { if ( - entities.at(triggering_entity).as().is_added and - entities.at(entity_ref).as().is_added) { + entities->at(triggering_entity).as().is_added and + entities->at(entity_ref).as().is_added) { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); } else { @@ -348,7 +348,7 @@ auto RelativeDistanceCondition::evaluate() -> Object return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { results.push_back(distance( - triggering_entity, entity_ref, *global().entities, coordinate_system, relative_distance_type, + triggering_entity, entity_ref, global().entities, coordinate_system, relative_distance_type, routing_algorithm, freespace)); return rule(static_cast(results.back()), value); })); diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp new file mode 100644 index 00000000000..0e7d30bd03f --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp @@ -0,0 +1,92 @@ +// 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 +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +RelativeSpeedCondition::RelativeSpeedCondition( + const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) +: Scope(scope), + entity_ref(readAttribute("entityRef", node, scope)), + rule(readAttribute("rule", node, scope)), + value(readAttribute("value", node, scope)), + direction(readAttribute("direction", node, scope, std::nullopt)), + triggering_entities(triggering_entities), + evaluations(triggering_entities.entity_refs.size(), Double::nan()) +{ +} + +auto RelativeSpeedCondition::description() const -> String +{ + auto description = std::stringstream(); + + description << triggering_entities.description() << "'s relative "; + + if (direction) { + description << *direction << " "; + } + + description << "speed to given entity " << entity_ref << " = "; + + print_to(description, evaluations); + + description << " " << rule << " " << value << "?"; + + return description.str(); +} + +auto RelativeSpeedCondition::evaluate( + const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities * entities, + const std::optional & direction) -> double +{ + if ( + entities->at(triggering_entity).as().is_added and + entities->at(entity_ref).as().is_added) { + if (const auto v = evaluateRelativeSpeed(triggering_entity, entity_ref); direction) { + switch (*direction) { + case DirectionalDimension::longitudinal: + return v.x; + case DirectionalDimension::lateral: + return v.y; + case DirectionalDimension::vertical: + return v.z; + default: + return math::geometry::norm(v); + } + } else { + return math::geometry::norm(v); + } + } else { + return Double::nan(); + } +} + +auto RelativeSpeedCondition::evaluate() -> Object +{ + evaluations.clear(); + + return asBoolean(triggering_entities.apply([this](auto && triggering_entity) { + evaluations.push_back(evaluate(triggering_entity, entity_ref, global().entities, direction)); + return std::invoke(rule, evaluations.back(), value); + })); +} +} // namespace syntax +} // namespace openscenario_interpreter From be16d09e3ed0e6865a8ac8cd42a638edd84c5188 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 26 Jun 2024 16:44:01 +0900 Subject: [PATCH 08/11] Remove data member `DistanceCondition::consider_z` Signed-off-by: yamacir-kit --- .../syntax/distance_condition.hpp | 36 +++++----- .../src/syntax/distance_condition.cpp | 69 +++++++++---------- 2 files changed, 49 insertions(+), 56 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index 61ea2db6345..f64e250326e 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -107,18 +107,18 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua std::vector 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; + // TODO STATIC + auto distance(const EntityRef &) -> double; template < CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type, Boolean::value_type> - auto distance(const EntityRef &) const -> double + // TODO STATIC + auto distance(const EntityRef &) -> double { throw SyntaxError(__FILE__, ":", __LINE__); } @@ -130,20 +130,20 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua // cspell: ignore euclidian // clang-format off -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; -template <> auto DistanceCondition::distance(const EntityRef &) const -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &) -> double; // clang-format on } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index 4e817bb7286..f759f485c79 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -46,12 +46,7 @@ DistanceCondition::DistanceCondition( value(readAttribute("value", node, scope)), position(readElement("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 supported = { RoutingAlgorithm::value_type::shortest, RoutingAlgorithm::value_type::undefined}; @@ -127,7 +122,7 @@ auto DistanceCondition::description() const -> std::string #define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity) -auto DistanceCondition::distance(const EntityRef & triggering_entity) const -> double +auto DistanceCondition::distance(const EntityRef & triggering_entity) -> double { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); @@ -135,15 +130,21 @@ auto DistanceCondition::distance(const EntityRef & triggering_entity) const -> d } // @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) +static auto hypot(const double x, const double y, const double z) { + static auto consider_z = []() { + 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_z ? std::hypot(x, y, z) : std::hypot(x, y); } template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - false>(const EntityRef & triggering_entity) const -> double + false>(const EntityRef & triggering_entity) -> double { return apply( overload( @@ -151,29 +152,25 @@ auto DistanceCondition::distance< const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(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(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(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(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); } @@ -181,7 +178,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - true>(const EntityRef & triggering_entity) const -> double + true>(const EntityRef & triggering_entity) -> double { return apply( overload( @@ -189,29 +186,25 @@ auto DistanceCondition::distance< const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(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(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(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(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); } @@ -219,7 +212,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -249,7 +242,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -279,7 +272,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -309,7 +302,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -339,7 +332,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -388,7 +381,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -437,7 +430,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -486,7 +479,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -535,7 +528,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -588,7 +581,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -641,7 +634,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( @@ -694,7 +687,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity) -> double { return apply( overload( From 77a1b31e289734ae59baeff140fc62b3c55d4acc Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 26 Jun 2024 16:57:36 +0900 Subject: [PATCH 09/11] Add `const Position &` to the argument of `DistanceCondition::distance` Signed-off-by: yamacir-kit --- .../syntax/distance_condition.hpp | 33 +++++++++-------- .../src/syntax/distance_condition.cpp | 35 ++++++++++--------- 2 files changed, 34 insertions(+), 34 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index f64e250326e..6ace25ce376 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -112,13 +112,12 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua auto description() const -> std::string; // TODO STATIC - auto distance(const EntityRef &) -> double; + auto distance(const EntityRef &, const Position &) -> double; template < CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type, Boolean::value_type> - // TODO STATIC - auto distance(const EntityRef &) -> double + auto distance(const EntityRef &, const Position &) -> double { throw SyntaxError(__FILE__, ":", __LINE__); } @@ -130,20 +129,20 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua // cspell: ignore euclidian // clang-format off -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; -template <> auto DistanceCondition::distance(const EntityRef &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; +template <> auto DistanceCondition::distance(const EntityRef &, const Position &) -> double; // clang-format on } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index f759f485c79..e59f6cb38d7 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -120,9 +120,10 @@ auto DistanceCondition::description() const -> std::string #define SWITCH_FREESPACE(FUNCTION, ...) \ return freespace ? FUNCTION(__VA_ARGS__, true) : FUNCTION(__VA_ARGS__, false) -#define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity) +#define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, position) -auto DistanceCondition::distance(const EntityRef & triggering_entity) -> double +auto DistanceCondition::distance(const EntityRef & triggering_entity, const Position & position) + -> double { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); @@ -144,7 +145,7 @@ static auto hypot(const double x, const double y, const double z) template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - false>(const EntityRef & triggering_entity) -> double + false>(const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -178,7 +179,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - true>(const EntityRef & triggering_entity) -> double + true>(const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -212,7 +213,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -242,7 +243,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -272,7 +273,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -302,7 +303,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -332,7 +333,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -381,7 +382,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -430,7 +431,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -479,7 +480,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -528,7 +529,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -581,7 +582,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -634,7 +635,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -687,7 +688,7 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -742,7 +743,7 @@ auto DistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(distance(triggering_entity)); + results.push_back(distance(triggering_entity, position)); return rule(static_cast(results.back()), value); })); } From 9b13ccd31853c522ce6bf87b80aeceb97f110820 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 26 Jun 2024 17:20:03 +0900 Subject: [PATCH 10/11] Update member function `CoordinateSystem::distance` to be static member Signed-off-by: yamacir-kit --- .../syntax/distance_condition.hpp | 2 +- .../src/syntax/distance_condition.cpp | 409 ++++++------------ 2 files changed, 143 insertions(+), 268 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index 6ace25ce376..4068c498995 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -117,7 +117,7 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua template < CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type, Boolean::value_type> - auto distance(const EntityRef &, const Position &) -> double + static auto distance(const EntityRef &, const Position &) -> double { throw SyntaxError(__FILE__, ":", __LINE__); } diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index e59f6cb38d7..5a73c536e7f 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -125,9 +125,12 @@ auto DistanceCondition::description() const -> std::string auto DistanceCondition::distance(const EntityRef & triggering_entity, const Position & position) -> double { - SWITCH_COORDINATE_SYSTEM( - SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); - return Double::nan(); + if (global().entities->ref(triggering_entity).as().is_added) { + SWITCH_COORDINATE_SYSTEM( + SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); + } else { + 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) @@ -338,43 +341,27 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return makeNativeRelativeLanePosition( - triggering_entity, static_cast(position)) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return makeNativeRelativeLanePosition( + triggering_entity, static_cast(position)) + .offset; }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }), position); } @@ -387,43 +374,27 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position)) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position)) + .offset; }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .offset; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .offset; }), position); } @@ -436,43 +407,27 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return makeNativeRelativeLanePosition( - triggering_entity, static_cast(position)) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return makeNativeRelativeLanePosition( + triggering_entity, static_cast(position)) + .s; }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).template as().is_added) { - return static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }), position); } @@ -485,43 +440,27 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position)) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position)) + .s; }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).template as().is_added) { - return static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position))) - .s; - } else { - return std::numeric_limits::quiet_NaN(); - } + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position))) + .s; }), position); } @@ -534,47 +473,31 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest) + .offset); }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }), position); } @@ -587,47 +510,31 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest) + .offset); }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .offset); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .offset); }), position); } @@ -640,47 +547,31 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest) + .s); }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).template as().is_added) { - return std::abs(static_cast( - makeNativeRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }), position); } @@ -693,47 +584,31 @@ auto DistanceCondition::distance< return apply( overload( [&](const WorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }, [&](const RelativeWorldPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }, [&](const RelativeObjectPosition & position) { - if (global().entities->ref(triggering_entity).as().is_added) { - return std::abs(makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest) + .s); }, [&](const LanePosition & position) { - if (global().entities->ref(triggering_entity).template as().is_added) { - return std::abs(static_cast( - makeNativeBoundingBoxRelativeLanePosition( - triggering_entity, static_cast(position), - RoutingAlgorithm::shortest)) - .s); - } else { - return std::numeric_limits::quiet_NaN(); - } + return std::abs(static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, static_cast(position), + RoutingAlgorithm::shortest)) + .s); }), position); } From f95eccb3711f4a036b9f67d8de25a901c3aa5b0f Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 27 Jun 2024 14:33:59 +0900 Subject: [PATCH 11/11] Rename `(Relative)?DistanceCondition::distance` to `evaluate` Signed-off-by: yamacir-kit --- .../syntax/distance_condition.hpp | 7 ++++--- .../syntax/relative_distance_condition.hpp | 2 +- .../syntax/speed_condition.hpp | 4 +++- .../syntax/time_to_collision_condition.hpp | 16 +++++++++++++--- .../src/syntax/distance_condition.cpp | 12 ++++++++---- .../src/syntax/relative_distance_condition.cpp | 4 ++-- .../src/syntax/speed_condition.cpp | 18 ++++++++++++++++-- 7 files changed, 47 insertions(+), 16 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index 4068c498995..453a31fed35 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -111,9 +111,6 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua auto description() const -> std::string; - // TODO STATIC - auto distance(const EntityRef &, const Position &) -> double; - template < CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type, Boolean::value_type> @@ -122,6 +119,10 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua throw SyntaxError(__FILE__, ":", __LINE__); } + static auto evaluate( + const EntityRef &, const Position &, const Entities *, CoordinateSystem, RelativeDistanceType, + RoutingAlgorithm, bool) -> double; + auto evaluate() -> Object; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp index 475f91ff739..bf09beca6d2 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp @@ -102,7 +102,7 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi throw SyntaxError(__FILE__, ":", __LINE__); } - static auto distance( + static auto evaluate( const EntityRef &, const EntityRef &, const Entities *, CoordinateSystem, RelativeDistanceType, RoutingAlgorithm, bool) -> double; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp index 29d0be6eb99..9e3398a4ecb 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp @@ -37,7 +37,7 @@ inline namespace syntax * * * -------------------------------------------------------------------------- */ -struct SpeedCondition : private SimulatorCore::ConditionEvaluation +struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluation { const Double value; @@ -51,6 +51,8 @@ struct SpeedCondition : private SimulatorCore::ConditionEvaluation auto description() const -> String; + static auto evaluate(const EntityRef &, const Entities *) -> double; + auto evaluate() -> Object; }; } // namespace syntax diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 39e7e776a1d..23140651a67 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -17,8 +17,10 @@ #include #include +#include #include #include +#include #include #include @@ -126,16 +128,24 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio const TimeToCollisionConditionTarget & time_to_collision_condition_target) { if (time_to_collision_condition_target.is()) { std::cerr << "RELATIVE DISTANCE = " - << RelativeDistanceCondition::distance( + << RelativeDistanceCondition::evaluate( triggering_entity, time_to_collision_condition_target.as(), global().entities, coordinate_system, relative_distance_type, routing_algorithm, freespace) - << ", " - << "RELATIVE SPEED = " + << ", RELATIVE SPEED = " << RelativeSpeedCondition::evaluate( triggering_entity, time_to_collision_condition_target.as(), global().entities, std::nullopt) << std::endl; + } else { + std::cerr << "DISTANCE = " + << DistanceCondition::evaluate( + triggering_entity, time_to_collision_condition_target.as(), + global().entities, coordinate_system, relative_distance_type, + routing_algorithm, freespace) + << ", RELATIVE SPEED = " + << SpeedCondition::evaluate(triggering_entity, global().entities) + << std::endl; } return Double(); }; diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index 5a73c536e7f..4621497b6f0 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -122,10 +122,12 @@ auto DistanceCondition::description() const -> std::string #define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, position) -auto DistanceCondition::distance(const EntityRef & triggering_entity, const Position & position) - -> double +auto DistanceCondition::evaluate( + const EntityRef & triggering_entity, const Position & position, const Entities * entities, + CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, + RoutingAlgorithm routing_algorithm, bool freespace) -> double { - if (global().entities->ref(triggering_entity).as().is_added) { + if (entities->ref(triggering_entity).as().is_added) { SWITCH_COORDINATE_SYSTEM( SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); } else { @@ -618,7 +620,9 @@ auto DistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(distance(triggering_entity, position)); + results.push_back(evaluate( + triggering_entity, position, global().entities, coordinate_system, relative_distance_type, + routing_algorithm, freespace)); return rule(static_cast(results.back()), value); })); } diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 349ebae69ae..36203a6479b 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -327,7 +327,7 @@ auto RelativeDistanceCondition::distance< #define DISTANCE(...) distance<__VA_ARGS__>(triggering_entity, entity_ref) -auto RelativeDistanceCondition::distance( +auto RelativeDistanceCondition::evaluate( const EntityRef & triggering_entity, const EntityRef & entity_ref, const Entities * entities, CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, RoutingAlgorithm routing_algorithm, bool freespace) -> double @@ -347,7 +347,7 @@ auto RelativeDistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { - results.push_back(distance( + results.push_back(evaluate( triggering_entity, entity_ref, global().entities, coordinate_system, relative_distance_type, routing_algorithm, freespace)); return rule(static_cast(results.back()), value); diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp index 95f4f9b2497..4c4c34e131f 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp @@ -13,7 +13,10 @@ // limitations under the License. #include +#include #include +#include +#include #include #include @@ -23,7 +26,8 @@ inline namespace syntax { SpeedCondition::SpeedCondition( const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) -: value(readAttribute("value", node, scope)), +: Scope(scope), + value(readAttribute("value", node, scope)), compare(readAttribute("rule", node, scope)), triggering_entities(triggering_entities), results(triggering_entities.entity_refs.size(), Double::nan()) @@ -43,12 +47,22 @@ auto SpeedCondition::description() const -> String return description.str(); } +auto SpeedCondition::evaluate(const EntityRef & triggering_entity, const Entities * entities) + -> double +{ + if (entities->ref(triggering_entity).as().is_added) { + return evaluateSpeed(triggering_entity); + } else { + return Double::nan(); + } +} + auto SpeedCondition::evaluate() -> Object { results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(evaluateSpeed(triggering_entity)); + results.push_back(evaluate(triggering_entity, global().entities)); return compare(results.back(), value); })); }