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/cmath/hypot.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/cmath/hypot.hpp new file mode 100644 index 00000000000..23a19b9e0c9 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/cmath/hypot.hpp @@ -0,0 +1,44 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPENSCENARIO_INTERPRETER__CMATH__HYPOT_HPP_ +#define OPENSCENARIO_INTERPRETER__CMATH__HYPOT_HPP_ + +#include +#include + +namespace openscenario_interpreter +{ +inline namespace cmath +{ +/* + @todo: after checking all the scenario work well with + consider_pose_by_road_slope = true, remove this function and use + std::hypot(x, y, z) +*/ +template +auto hypot(T x, T y, T z) +{ + static const auto consider_pose_by_road_slope = []() { + auto node = rclcpp::Node("get_parameter", "simulation"); + node.declare_parameter("consider_pose_by_road_slope", false); + return node.get_parameter("consider_pose_by_road_slope").as_bool(); + }(); + + return consider_pose_by_road_slope ? std::hypot(x, y, z) : std::hypot(x, y); +} +} // namespace cmath +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__CMATH__HYPOT_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 393b4a11420..6fd055886fc 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -16,6 +16,7 @@ #define OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_ #include +#include #include #include #include @@ -533,7 +534,6 @@ class SimulatorCore return core->checkCollision(std::forward(xs)...); } - template static auto evaluateBoundingBoxEuclideanDistance( const std::string & from_entity_name, const std::string & to_entity_name) // for RelativeDistanceCondition @@ -551,6 +551,13 @@ class SimulatorCore return std::numeric_limits::quiet_NaN(); } + 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 { @@ -564,7 +571,7 @@ class SimulatorCore template static auto evaluateSpeed(Ts &&... xs) { - return core->getCurrentTwist(std::forward(xs)...).linear.x; + return core->getCurrentTwist(std::forward(xs)...).linear; } template 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/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index d7f297bf54f..b1d7357bb98 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -108,22 +108,22 @@ 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; - template < CoordinateSystem::value_type, RelativeDistanceType::value_type, RoutingAlgorithm::value_type, Boolean::value_type> - auto distance(const EntityRef &) const -> double + static auto distance(const EntityRef &, const Position &) -> double { throw SyntaxError(__FILE__, ":", __LINE__); } + static auto evaluate( + const Entities *, const EntityRef &, const Position &, CoordinateSystem, RelativeDistanceType, + RoutingAlgorithm, Boolean) -> double; + auto evaluate() -> Object; }; @@ -131,20 +131,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 &, 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/include/openscenario_interpreter/syntax/entities.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp index d0c3d753aa4..7e68e5774ea 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp @@ -24,18 +24,24 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- Entities --------------------------------------------------------------- - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct Entities : public std::unordered_map // TODO to be data member +/* + Entities (OpenSCENARIO XML 1.3) + + Definition of entities (scenario objects or entity selections) in + a scenario. + + + + + + + +*/ +struct Entities : private std::unordered_map { + using std::unordered_map::begin; + using std::unordered_map::end; + explicit Entities(const pugi::xml_node &, Scope &); auto isAdded(const Entity &) const -> bool; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp index 0f981287244..d9503ca8979 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp @@ -50,8 +50,6 @@ struct ReachPositionCondition : private SimulatorCore::CoordinateSystemConversio std::vector> results; // for description - const bool consider_z; - explicit ReachPositionCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); auto description() const -> String; 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 80265549339..c038d37efe3 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 @@ -92,8 +92,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; @@ -101,12 +99,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 evaluate( + const Entities *, const Entity &, const Entity &, CoordinateSystem, RelativeDistanceType, + RoutingAlgorithm, Boolean) -> double; auto evaluate() -> Object; }; @@ -115,20 +115,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/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..5920e0ed3b7 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_speed_condition.hpp @@ -0,0 +1,84 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ + +#include +#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 Scope, private SimulatorCore::ConditionEvaluation +{ + /* + Reference entity. + */ + const Entity entity_ref; + + /* + The operator (less, greater, equal). + */ + const Rule rule; + + /* + Relative speed value. Unit: [m/s]. Range: ]-inf..inf[. Relative speed is + defined as speed_rel = speed(triggering entity) - speed(reference entity) + */ + const Double value; + + /* + Direction of the speed (if not given, the total speed is considered). + */ + const std::optional direction; + + const TriggeringEntities triggering_entities; + + std::vector> evaluations; + + explicit RelativeSpeedCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); + + auto description() const -> String; + + static auto evaluate( + const Entities *, const Entity &, const Entity &, + const std::optional & = std::nullopt) -> double; + + auto evaluate() -> Object; +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_SPEED_CONDITION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp index c37b96b513f..899ffd69da2 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp @@ -38,8 +38,7 @@ inline namespace syntax * * -------------------------------------------------------------------------- */ struct SpeedAction : private Scope, // NOTE: Required for access to actors - private SimulatorCore::ActionApplication, - private SimulatorCore::ConditionEvaluation + private SimulatorCore::ActionApplication { const TransitionDynamics speed_action_dynamics; 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 c33ac44e1f7..05a87962d34 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -27,22 +28,37 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- SpeedCondition --------------------------------------------------------- - * - * Compares a triggering entity's/entities' speed to a target speed. The - * logical operator for the comparison is given by the rule attribute. - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct SpeedCondition : private SimulatorCore::ConditionEvaluation +/* + SpeedCondition (OpenSCENARIO XML 1.3) + + Compares a triggering entity's/entities' speed to a target speed. + The logical operator for the comparison is given by the rule + attribute. If direction is used, only the projection to that + direction is used in the comparison. + + + + + + +*/ +struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluation { + /* + The operator (less, greater, equal). + */ + const Rule rule; + + /* + Speed value of the speed condition. Unit: [m/s]. + */ const Double value; - const Rule compare; + /* + Direction of the speed (if not given, the total speed is + considered). + */ + const std::optional direction; const TriggeringEntities triggering_entities; @@ -52,6 +68,10 @@ struct SpeedCondition : private SimulatorCore::ConditionEvaluation auto description() const -> String; + static auto evaluate( + const Entities *, const Entity &, const std::optional & = std::nullopt) + -> double; + auto evaluate() -> Object; }; } // namespace syntax diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp index 19550531412..b0ee4cbbbf7 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp @@ -40,8 +40,7 @@ inline namespace syntax * * -------------------------------------------------------------------------- */ struct SpeedProfileAction : private Scope, // NOTE: Required for access to actors - private SimulatorCore::ActionApplication, - private SimulatorCore::ConditionEvaluation + private SimulatorCore::ActionApplication { /* Reference entity. If set, the speed values will be interpreted as relative 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..61a1b0a76b5 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -0,0 +1,180 @@ +// 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 +#include +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + 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 + 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. + + + + + + + + + deprecated + + + + + + + + + + +*/ +struct TimeToCollisionCondition : private Scope, private SimulatorCore::ConditionEvaluation +{ + const TimeToCollisionConditionTarget time_to_collision_condition_target; + + const Boolean freespace; + + const Rule rule; + + const Double value; + + const RelativeDistanceType relative_distance_type; + + const CoordinateSystem coordinate_system; + + const RoutingAlgorithm routing_algorithm; + + const TriggeringEntities triggering_entities; + + std::vector> evaluations; + + explicit TimeToCollisionCondition( + const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) + : Scope(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( + 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 + { + 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(); + } + + static auto evaluate( + const Entities * entities, const Entity & triggering_entity, + const TimeToCollisionConditionTarget & time_to_collision_condition_target, + CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, + RoutingAlgorithm routing_algorithm, Boolean freespace) + { + if (time_to_collision_condition_target.is()) { + const auto relative_distance = RelativeDistanceCondition::evaluate( + entities, // + triggering_entity, // + time_to_collision_condition_target.as(), // + coordinate_system, // + relative_distance_type, // + routing_algorithm, // + freespace); + + const auto relative_speed = RelativeSpeedCondition::evaluate( + entities, // + triggering_entity, // + time_to_collision_condition_target.as()); + + // std::cerr << "RELATIVE DISTANCE = " << relative_distance + // << ", RELATIVE SPEED = " << relative_speed << std::endl; + + return relative_distance / relative_speed; + } else { + const auto distance = DistanceCondition::evaluate( + entities, // + triggering_entity, // + time_to_collision_condition_target.as(), // + coordinate_system, // + relative_distance_type, // + routing_algorithm, // + freespace); + + const auto speed = SpeedCondition::evaluate(entities, triggering_entity); + + // std::cerr << "DISTANCE = " << distance << ", RELATIVE SPEED = " << speed << std::endl; + + return distance / speed; + } + } + + auto evaluate() + { + evaluations.clear(); + + return asBoolean(triggering_entities.apply([this](const auto & triggering_entity) { + evaluations.push_back(triggering_entity.apply([this](const auto & triggering_entity) { + return evaluate( + global().entities, triggering_entity, time_to_collision_condition_target, + coordinate_system, relative_distance_type, routing_algorithm, freespace); + })); + return not evaluations.back().size() or std::invoke(rule, evaluations.back(), value).min(); + })); + } +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_HPP_ 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..ac1b051008a --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition_target.hpp @@ -0,0 +1,45 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_ + +#include +#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 &, Scope &); +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__TIME_TO_COLLISION_CONDITION_TARGET_HPP_ diff --git a/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp b/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp index d0408597bf5..ae11d05f50c 100644 --- a/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp @@ -47,7 +47,7 @@ auto AddEntityAction::endsImmediately() noexcept -> bool // auto AddEntityAction::operator()(const EntityRef & entity_ref) const -> void try { - const auto entity = global().entities->at(entity_ref); + const auto entity = global().entities->ref(entity_ref); const auto add_entity = overload( [&](const Vehicle & vehicle) { diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index fd90d905e38..03353aa0af6 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include @@ -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}; @@ -125,25 +120,26 @@ 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) const -> double +auto DistanceCondition::evaluate( + const Entities * entities, const EntityRef & triggering_entity, const Position & position, + CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, + RoutingAlgorithm routing_algorithm, Boolean freespace) -> double { - SWITCH_COORDINATE_SYSTEM( - SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); - return Double::nan(); -} - -// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z) -static double hypot(const double x, const double y, const double z, const bool consider_z) -{ - return consider_z ? std::hypot(x, y, z) : std::hypot(x, y); + if (entities->ref(triggering_entity).as().is_added) { + SWITCH_COORDINATE_SYSTEM( + SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); + return Double::nan(); + } else { + return Double::nan(); + } } template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, RoutingAlgorithm::undefined, - false>(const EntityRef & triggering_entity) const -> double + false>(const EntityRef & triggering_entity, const Position & position) -> double { return apply( overload( @@ -151,29 +147,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 +173,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, const Position & position) -> double { return apply( overload( @@ -189,29 +181,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 +207,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, const Position & position) -> double { return apply( overload( @@ -249,7 +237,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, const Position & position) -> double { return apply( overload( @@ -279,7 +267,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, const Position & position) -> double { return apply( overload( @@ -309,7 +297,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, const Position & position) -> double { return apply( overload( @@ -339,48 +327,32 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity, const Position & position) -> double { 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); } @@ -388,48 +360,32 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity, const Position & position) -> double { 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); } @@ -437,48 +393,32 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity, const Position & position) -> double { 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); } @@ -486,48 +426,32 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity, const Position & position) -> double { 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); } @@ -535,52 +459,36 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity, const Position & position) -> double { 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); } @@ -588,52 +496,36 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity, const Position & position) -> double { 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); } @@ -641,52 +533,36 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity, const Position & position) -> double { 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); } @@ -694,52 +570,36 @@ auto DistanceCondition::distance< template <> auto DistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) const -> double + const EntityRef & triggering_entity, const Position & position) -> double { 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); } @@ -749,8 +609,11 @@ auto DistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back( - triggering_entity.apply([&](const auto & object) { return distance(object); })); + results.push_back(triggering_entity.apply([&](const auto & triggering_entity) { + return evaluate( + global().entities, triggering_entity, position, coordinate_system, relative_distance_type, + routing_algorithm, freespace); + })); return not results.back().size() or rule(results.back(), value).min(); })); } diff --git a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp index df89c5ba0fe..554c5259963 100644 --- a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp @@ -20,9 +20,11 @@ #include #include #include +#include #include #include #include +#include namespace openscenario_interpreter { @@ -37,11 +39,11 @@ 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); }), - 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); }), diff --git a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp index e546d94f63f..440c21ce6ba 100644 --- a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -31,12 +32,7 @@ ReachPositionCondition::ReachPositionCondition( position(readElement("Position", node, scope)), compare(Rule::lessThan), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), {Double::nan()}), - consider_z([]() { - rclcpp::Node node{"get_parameter", "simulation"}; - node.declare_parameter("consider_pose_by_road_slope", false); - return node.get_parameter("consider_pose_by_road_slope").as_bool(); - }()) + results(triggering_entities.entity_refs.size(), {Double::nan()}) { } @@ -53,12 +49,6 @@ auto ReachPositionCondition::description() const -> String return description.str(); } -// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z) -static double hypot(const double x, const double y, const double z, const bool consider_z) -{ - return consider_z ? std::hypot(x, y, z) : std::hypot(x, y); -} - auto ReachPositionCondition::evaluate() -> Object { // TODO USE DistanceCondition::distance @@ -66,22 +56,22 @@ auto ReachPositionCondition::evaluate() -> Object [&](const WorldPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); + return hypot(pose.position.x, pose.position.y, pose.position.z); }, [&](const RelativeWorldPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); + return hypot(pose.position.x, pose.position.y, pose.position.z); }, [&](const RelativeObjectPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); + return hypot(pose.position.x, pose.position.y, pose.position.z); }, [&](const LanePosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); + return hypot(pose.position.x, pose.position.y, pose.position.z); }); results.clear(); diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 6988fd54dcf..5811c9208bc 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include // TEMPORARY (TODO REMOVE THIS LINE) @@ -39,12 +40,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()}) { std::set supported = { RoutingAlgorithm::value_type::shortest, RoutingAlgorithm::value_type::undefined}; @@ -71,346 +67,266 @@ 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 { - 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 + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> 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 <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> 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(); - } -} - -// @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) + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> double { - return consider_z ? std::hypot(x, y, z) : std::hypot(x, y); + /** + @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); } 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 { - 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(); - } + 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 { - 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(); - } + 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 { - 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 <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> 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 <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> 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 <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::undefined, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> 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 <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> 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 <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::lateral, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> 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 <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, true>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> 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 <> auto RelativeDistanceCondition::distance< CoordinateSystem::lane, RelativeDistanceType::longitudinal, RoutingAlgorithm::shortest, false>( - const EntityRef & triggering_entity) -> double + const EntityRef & triggering_entity, const EntityRef & entity_ref) -> 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, ...) \ 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::evaluate( + const Entities * entities, const Entity & triggering_entity, const Entity & entity_ref, + CoordinateSystem coordinate_system, RelativeDistanceType relative_distance_type, + RoutingAlgorithm routing_algorithm, Boolean freespace) -> double { - SWITCH_COORDINATE_SYSTEM( - SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); - return Double::nan(); + if (entities->isAdded(triggering_entity) and entities->isAdded(entity_ref)) { + SWITCH_COORDINATE_SYSTEM( + SWITCH_RELATIVE_DISTANCE_TYPE, SWITCH_ROUTING_ALGORITHM, SWITCH_FREESPACE, DISTANCE); + } else { + return Double::nan(); + } } auto RelativeDistanceCondition::evaluate() -> Object @@ -418,8 +334,11 @@ auto RelativeDistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { - results.push_back( - triggering_entity.apply([&](const auto & object) { return distance(object); })); + results.push_back(triggering_entity.apply([&](const auto & triggering_entity) { + return evaluate( + global().entities, triggering_entity, entity_ref, coordinate_system, relative_distance_type, + routing_algorithm, freespace); + })); return not results.back().size() or rule(results.back(), value).min(); })); } 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..a9c6d49be05 --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/relative_speed_condition.cpp @@ -0,0 +1,98 @@ +// 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 +#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), 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()}) +{ + if (entity_ref.is()) { + throw SyntaxError("EntitySelection is not allowed in RelativeSpeedCondition.entityRef"); + } +} + +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 Entities * entities, const Entity & triggering_entity, const Entity & entity_ref, + const std::optional & direction) -> double +{ + if ( + triggering_entity.apply([&](const auto & each) { return entities->isAdded(each); }).min() and + entities->isAdded(entity_ref)) { + 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](const auto & triggering_entity) { + evaluations.push_back(triggering_entity.apply([this](const auto & triggering_entity) { + return evaluate(global().entities, triggering_entity, entity_ref, direction); + })); + return not evaluations.back().size() or std::invoke(rule, evaluations.back(), value).min(); + })); +} +} // namespace syntax +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp index ff317f7a48f..a0811838241 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include namespace openscenario_interpreter @@ -55,7 +56,8 @@ auto SpeedAction::accomplished() -> bool }; auto check = [this](auto && actor) { - auto evaluation = actor.apply([](const auto & object) { return evaluateSpeed(object); }); + auto evaluation = actor.apply( + [this](const auto & actor) { return SpeedCondition::evaluate(global().entities, actor); }); if (speed_action_target.is()) { return not evaluation.size() or equal_to>()( @@ -66,14 +68,16 @@ auto SpeedAction::accomplished() -> bool case SpeedTargetValueType::delta: return not evaluation.size() or equal_to>()( - evaluateSpeed(speed_action_target.as().entity_ref) + + SpeedCondition::evaluate( + global().entities, speed_action_target.as().entity_ref) + speed_action_target.as().value, evaluation) .min(); case SpeedTargetValueType::factor: return not evaluation.size() or equal_to>()( - evaluateSpeed(speed_action_target.as().entity_ref) * + SpeedCondition::evaluate( + global().entities, speed_action_target.as().entity_ref) * speed_action_target.as().value, evaluation) .min(); diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp index b8e3a6640b8..dd4c858269b 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include +#include #include +#include +#include #include #include @@ -23,8 +27,10 @@ inline namespace syntax { SpeedCondition::SpeedCondition( const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) -: value(readAttribute("value", node, scope)), - compare(readAttribute("rule", node, scope)), +: Scope(scope), + rule(readAttribute("rule", node, scope)), + value(readAttribute("value", node, scope)), + direction(readAttribute("direction", node, scope, std::nullopt)), triggering_entities(triggering_entities), results(triggering_entities.entity_refs.size(), {Double::nan()}) { @@ -38,19 +44,44 @@ auto SpeedCondition::description() const -> String print_to(description, results); - description << " " << compare << " " << value << "?"; + description << " " << rule << " " << value << "?"; return description.str(); } +auto SpeedCondition::evaluate( + const Entities * entities, const Entity & triggering_entity, + const std::optional & direction) -> double +{ + if (entities->isAdded(triggering_entity)) { + if (const auto v = evaluateSpeed(triggering_entity); 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 SpeedCondition::evaluate() -> Object { results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back( - triggering_entity.apply([&](const auto & object) { return evaluateSpeed(object); })); - return not results.back().size() or compare(results.back(), value).min(); + results.push_back(triggering_entity.apply([&](const auto & triggering_entity) { + return evaluate(global().entities, triggering_entity); + })); + return not results.back().size() or std::invoke(rule, results.back(), value).min(); })); } } // namespace syntax diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp index 2e1ef370611..4e241addd8b 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -120,12 +121,14 @@ auto SpeedProfileAction::run() -> void { for (auto && [actor, iter] : accomplishments) { auto accomplished = [this](const auto & actor, const auto & speed_profile_entry) { - auto speeds = actor.apply([&](const auto & object) { return evaluateSpeed(object); }); + auto speeds = actor.apply( + [&](const auto & object) { return SpeedCondition::evaluate(global().entities, object); }); if (not speeds.size()) { return true; } else if (entity_ref) { return equal_to>()( - speeds, speed_profile_entry.speed + evaluateSpeed(entity_ref)) + speeds, speed_profile_entry.speed + + SpeedCondition::evaluate(global().entities, entity_ref)) .min(); } else { return equal_to>()(speeds, speed_profile_entry.speed).min(); diff --git a/openscenario/openscenario_interpreter/src/syntax/time_to_collision_condition_target.cpp b/openscenario/openscenario_interpreter/src/syntax/time_to_collision_condition_target.cpp new file mode 100644 index 00000000000..829793c7a54 --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/time_to_collision_condition_target.cpp @@ -0,0 +1,38 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +TimeToCollisionConditionTarget::TimeToCollisionConditionTarget( + const pugi::xml_node & node, Scope & scope) +// clang-format off +: ComplexType(choice(node, + std::make_pair( "Position", [&](auto && node) { return make(std::forward(node), scope); }), + std::make_pair("EntityRef", [&](auto && node) { return make< Entity>(std::forward(node), scope); }))) +// clang-format on +{ + if (is()) { + throw SyntaxError("EntitySelection is not allowed in TimeToCollisionConditionTarget.entityRef"); + } +} +} // namespace syntax +} // namespace openscenario_interpreter