diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 685737ef982..ca50d48a45c 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -598,7 +598,16 @@ class SimulatorCore template static auto evaluateSpeed(Ts &&... xs) { - return core->getCurrentTwist(std::forward(xs)...).linear.x; + /* + The function name "evaluateSpeed" stands for "evaluate SpeedCondition" + and is a part used to implement `SpeedCondition::evaluate`. + SpeedCondition can be evaluated in three directions: longitudinal, + lateral, and vertical, based on the attribute direction. Therefore, + please note that this function returns velocity, that is, a vector, + rather than speed, contrary to the name "evaluateSpeed". + */ + const auto linear = core->getCurrentTwist(std::forward(xs)...).linear; + return Eigen::Vector3d(linear.x, linear.y, linear.z); } template 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..9c2bf1ffb92 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,36 @@ 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.1) + + 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 +67,11 @@ struct SpeedCondition : private SimulatorCore::ConditionEvaluation auto description() const -> String; + static auto evaluate(const Entities *, const Entity &) -> Eigen::Vector3d; + + static auto evaluate( + const Entities *, const Entity &, const std::optional &) -> 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/src/syntax/speed_action.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp index ff317f7a48f..935d62e434a 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp @@ -14,9 +14,9 @@ #include #include -#include #include #include +#include #include namespace openscenario_interpreter @@ -55,7 +55,9 @@ auto SpeedAction::accomplished() -> bool }; auto check = [this](auto && actor) { - auto evaluation = actor.apply([](const auto & object) { return evaluateSpeed(object); }); + const auto evaluation = actor.apply([this](const auto & actor) { + return SpeedCondition::evaluate(global().entities, actor, std::nullopt); + }); if (speed_action_target.is()) { return not evaluation.size() or equal_to>()( @@ -66,14 +68,18 @@ 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, + std::nullopt) + 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, + std::nullopt) * 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..cc8d64b0c41 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -23,8 +24,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 +41,49 @@ 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) + -> Eigen::Vector3d +{ + if (entities->isAdded(triggering_entity)) { + return evaluateSpeed(triggering_entity); + } else { + return Eigen::Vector3d(Double::nan(), Double::nan(), Double::nan()); + } +} + +auto SpeedCondition::evaluate( + const Entities * entities, const Entity & triggering_entity, + const std::optional & direction) -> double +{ + if (const Eigen::Vector3d v = evaluate(entities, triggering_entity); direction) { + switch (*direction) { + default: + case DirectionalDimension::longitudinal: + return v.x(); + case DirectionalDimension::lateral: + return v.y(); + case DirectionalDimension::vertical: + return v.z(); + } + } else { + return v.norm(); + } +} + 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(); + return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { + results.push_back(triggering_entity.apply([&](const auto & triggering_entity) { + return evaluate(global().entities, triggering_entity, direction); + })); + 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..e294a09523d 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,15 @@ 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 & actor) { + return SpeedCondition::evaluate(global().entities, actor, std::nullopt); + }); 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, std::nullopt)) .min(); } else { return equal_to>()(speeds, speed_profile_entry.speed).min();