diff --git a/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp b/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp index 0e967d66821..02a4c57b120 100644 --- a/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp +++ b/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp @@ -36,4 +36,4 @@ inline auto concatenate = [](auto &&... xs) { } // namespace scenario_simulator_exception } // namespace common -#endif // OPENSCENARIO_INTERPRETER__CONCATENATE_HPP_ +#endif // SCENARIO_SIMULATOR_EXCEPTION__CONCATENATE_HPP_ diff --git a/common/scenario_simulator_exception/include/scenario_simulator_exception/fold.hpp b/common/scenario_simulator_exception/include/scenario_simulator_exception/fold.hpp index 13ec28b8bc2..2f2ae4ba034 100644 --- a/common/scenario_simulator_exception/include/scenario_simulator_exception/fold.hpp +++ b/common/scenario_simulator_exception/include/scenario_simulator_exception/fold.hpp @@ -52,4 +52,4 @@ constexpr decltype(auto) fold_right(F && f, T && x, Ts &&... xs) } // namespace scenario_simulator_exception } // namespace common -#endif // OPENSCENARIO_INTERPRETER__FOLD_HPP_ +#endif // SCENARIO_SIMULATOR_EXCEPTION__FOLD_HPP_ diff --git a/docs/release/ReleaseNotes.md b/docs/release/ReleaseNotes.md index cdee7450628..225831750b5 100644 --- a/docs/release/ReleaseNotes.md +++ b/docs/release/ReleaseNotes.md @@ -117,6 +117,8 @@ Major Changes :race_car: :red_car: :blue_car: | UserDefinedValueCondition `RelativeHeadingCondition` | Added one-argument version to `RelativeHeadingCondition`. This version of `RelativeHeadingCondition` returns the lane coordinate system heading of the entity with the name given in the first argument. | `openscenario_interpreter` | [#978](https://github.com/tier4/scenario_simulator_v2/pull/978) | [yamacir-kit](https://github.com/yamacir-kit) | | OpenSCENARIO 1.2 `Controller.Properties.Property` | Added support for delaying the publication of object detection data by setting the value `detectedObjectPublishingDelay` (in seconds) to `Controller.Properties.Property`. | `openscenario_interpreter` | [#986](https://github.com/tier4/scenario_simulator_v2/pull/986) | [yamacir-kit](https://github.com/yamacir-kit) | | OpenSCENARIO 1.2 `EnvironmentAction` | The parsing of `EnvironmentAction` is now supported | `openscenario_interpreter` | [#980](https://github.com/tier4/scenario_simulator_v2/pull/980) | [f0reachARR](https://github.com/f0reachARR) | +| OpenSCENARIO 1.2 `ParameterValueDistribution` | Start supporting parameter sweeping using official OpenSCENARIO feature ( it was supported by TIER IV V2 format previously) | `openscenario_interpreter`, `openscenario_preprocessor` | [#878](https://github.com/tier4/scenario_simulator_v2/pull/878) | [HansRobo](https://github.com/HansRobo) | + Bug Fixes:bug: diff --git a/openscenario/openscenario_interpreter/CMakeLists.txt b/openscenario/openscenario_interpreter/CMakeLists.txt index a68a5360e5f..4a5da8a9d90 100644 --- a/openscenario/openscenario_interpreter/CMakeLists.txt +++ b/openscenario/openscenario_interpreter/CMakeLists.txt @@ -29,6 +29,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/object.cpp src/evaluate.cpp src/openscenario_interpreter.cpp + src/parameter_distribution.cpp src/record.cpp src/scope.cpp) @@ -46,6 +47,10 @@ install( FILES ${CMAKE_CURRENT_SOURCE_DIR}/features DESTINATION lib/${PROJECT_NAME}) +install( + DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/test + DESTINATION share/${PROJECT_NAME}) + # ------------------------------------------------------------------------------ # test # ------------------------------------------------------------------------------ @@ -54,7 +59,9 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() ament_add_gtest(test_syntax test/test_syntax.cpp) + ament_add_gtest(test_parameter_value_distribution test/test_parameter_value_distribution.cpp) target_link_libraries(test_syntax ${PROJECT_NAME}) + target_link_libraries(test_parameter_value_distribution ${PROJECT_NAME}) endif() ament_auto_package() diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/parameter_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/parameter_distribution.hpp new file mode 100644 index 00000000000..9b39fda6c27 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/parameter_distribution.hpp @@ -0,0 +1,88 @@ +// 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__PARAMETER_DISTRIBUTION_HPP_ +#define OPENSCENARIO_INTERPRETER__PARAMETER_DISTRIBUTION_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +// parameter set like {a: 1.0, b: 2.0,...} +using ParameterSet = std::unordered_map; +using ParameterSetSharedPtr = std::shared_ptr; +// list of ParameterList +using ParameterDistribution = std::vector; +using SingleUnnamedParameterDistribution = std::vector; + +// TODO(HansRobo): implement parallel derivable parameter distribution with this base struct +struct ParallelDerivableParameterValueDistributionBase +{ + virtual auto derive( + std::size_t local_index, std::size_t local_size, std::size_t global_index, + std::size_t global_size) -> ParameterSet = 0; + + virtual auto getNumberOfDeriveScenarios() const -> std::size_t + { + throw Error("getNumberOfDeriveScenarios() is not implemented"); + } +}; + +// generator types distribution +struct SingleParameterDistributionBase +{ + virtual auto derive() -> SingleUnnamedParameterDistribution = 0; +}; + +struct MultiParameterDistributionBase +{ + virtual auto derive() -> ParameterDistribution = 0; +}; + +struct StochasticParameterDistributionBase +{ + virtual auto derive() -> Object = 0; +}; + +// container types of distribution data generator +struct ParameterDistributionContainer +{ + virtual auto derive() -> ParameterDistribution = 0; +}; + +auto mergeParameterDistribution( + const ParameterDistribution & distribution, const ParameterDistribution & additional_distribution) + -> ParameterDistribution; + +template +ParameterDistribution mergeParameterDistributionList( + const ParameterDistribution & base_distribution, + const std::list & distribution_list) +{ + ParameterDistribution merged_distribution{base_distribution}; + for (const auto & additional_distribution : distribution_list) { + merged_distribution = mergeParameterDistribution( + merged_distribution, + apply( + [](const auto & distribution) { return distribution.derive(); }, additional_distribution)); + } + return merged_distribution; +} +} // namespace openscenario_interpreter +#endif // OPENSCENARIO_INTERPRETER__PARAMETER_DISTRIBUTION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp index 4f6cfe13ed5..d26dfc1726d 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -192,7 +193,8 @@ class Scope std::list actors; - double seed; // NOTE: `seed` is used only for sharing randomSeed in Stochastic now + // NOTE: `random_engine` is used only for sharing random number generator in Stochastic now + std::default_random_engine random_engine; Scope() = delete; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic.hpp index e40f96c6674..4d13538eced 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic.hpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__DETERMINISTIC_HPP_ -#define OPENSCENARIO_INTERPRETER__DETERMINISTIC_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_HPP_ +#include #include #include #include @@ -23,21 +24,25 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- Deterministic 1.2 ------------------------------------------------------ - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct Deterministic +/* + Deterministic (OpenSCENARIO XML 1.3) + + Top level container containing all deterministic distribution elements. + + + + + + +*/ +struct Deterministic : public ParameterDistributionContainer { const std::list deterministic_parameter_distributions; explicit Deterministic(const pugi::xml_node &, Scope & scope); + + auto derive() -> ParameterDistribution override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__DETERMINISTIC_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_multi_parameter_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_multi_parameter_distribution.hpp index 589f36861b5..c1e53315858 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_multi_parameter_distribution.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_multi_parameter_distribution.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_HPP_ -#define OPENSCENARIO_INTERPRETER__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_HPP_ #include #include @@ -23,19 +23,24 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- DeterministicMultiParameterDistribution 1.2 ---------------------------- - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct DeterministicMultiParameterDistribution : public DeterministicMultiParameterDistributionType +/* + DeterministicMultiParameterDistribution (OpenSCENARIO XML 1.3) + + Container for a deterministic distribution which is applied to multiple parameters. + + + + + + +*/ +struct DeterministicMultiParameterDistribution : public DeterministicMultiParameterDistributionType, + public ParameterDistributionContainer { explicit DeterministicMultiParameterDistribution(const pugi::xml_node &, Scope & scope); + + auto derive() -> ParameterDistribution override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_multi_parameter_distribution_type.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_multi_parameter_distribution_type.hpp index 3091fe51b62..89460a9230c 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_multi_parameter_distribution_type.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_multi_parameter_distribution_type.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_TYPE_HPP_ -#define OPENSCENARIO_INTERPRETER__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_TYPE_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_TYPE_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_TYPE_HPP_ #include #include @@ -23,24 +23,22 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- DeterministicMultiParameterDistributionType 1.2 ------------------------ - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + DeterministicMultiParameterDistributionType (OpenSCENARIO XML 1.3) + + A deterministic distribution type which can be applied to multiple parameters. + + + + + + +*/ struct DeterministicMultiParameterDistributionType : public ValueSetDistribution { explicit DeterministicMultiParameterDistributionType(const pugi::xml_node &, Scope & scope); }; -//DEFINE_LAZY_VISITOR( -// DeterministicMultiParameterDistributionType, -// CASE(ValueSetDistribution), // -//); } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_TYPE_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_MULTI_PARAMETER_DISTRIBUTION_TYPE_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_parameter_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_parameter_distribution.hpp index c81226d3886..298de0d85a1 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_parameter_distribution.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_parameter_distribution.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__DETERMINISTIC_PARAMETER_DISTRIBUTION_HPP_ -#define OPENSCENARIO_INTERPRETER__DETERMINISTIC_PARAMETER_DISTRIBUTION_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_PARAMETER_DISTRIBUTION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_PARAMETER_DISTRIBUTION_HPP_ #include #include @@ -24,18 +24,18 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- DeterministicParameterDistribution 1.2 --------------------------------- - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + DeterministicParameterDistribution (OpenSCENARIO XML 1.3) + + Either a DeterministicMultiParameterDistribution or a DeterministicSingleParameterDistribution + + + + + + + +*/ struct DeterministicParameterDistribution : public Group { explicit DeterministicParameterDistribution(const pugi::xml_node &, Scope & scope); @@ -48,4 +48,4 @@ DEFINE_LAZY_VISITOR( ); } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__DETERMINISTIC_PARAMETER_DISTRIBUTION_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_PARAMETER_DISTRIBUTION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_single_parameter_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_single_parameter_distribution.hpp index 7bb90d254a9..5de8634cc07 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_single_parameter_distribution.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_single_parameter_distribution.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_HPP_ -#define OPENSCENARIO_INTERPRETER__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_HPP_ #include #include @@ -23,23 +23,28 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- DeterministicSingleParameterDistribution 1.2 --------------------------- - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + DeterministicSingleParameterDistribution (OpenSCENARIO XML 1.3) + + Container for a deterministic distribution which is applied to a single parameter. + + + + + + + +*/ struct DeterministicSingleParameterDistribution -: public DeterministicSingleParameterDistributionType +: public DeterministicSingleParameterDistributionType, + public ParameterDistributionContainer { const String parameter_name; explicit DeterministicSingleParameterDistribution(const pugi::xml_node &, Scope &); + + auto derive() -> ParameterDistribution override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_single_parameter_distribution_type.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_single_parameter_distribution_type.hpp index bd21b92dc5e..868c9884526 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_single_parameter_distribution_type.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/deterministic_single_parameter_distribution_type.hpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_TYPE_HPP_ -#define OPENSCENARIO_INTERPRETER__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_TYPE_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_TYPE_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_TYPE_HPP_ +#include #include #include #include @@ -25,17 +26,19 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- DeterministicSingleParameterDistributionType 1.2 ----------------------- - * - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + DeterministicSingleParameterDistributionType (OpenSCENARIO XML 1.3) + + A deterministic distribution type which can be applied to a single parameter. + + + + + + + + +*/ struct DeterministicSingleParameterDistributionType : public Group { explicit DeterministicSingleParameterDistributionType(const pugi::xml_node &, Scope & scope); @@ -43,10 +46,10 @@ struct DeterministicSingleParameterDistributionType : public Group DEFINE_LAZY_VISITOR( DeterministicSingleParameterDistributionType, - CASE(DistributionSet), // - CASE(DistributionRange), // - CASE(UserDefinedDistribution), // + CASE(DistributionSet), // + CASE(DistributionRange), // + // CASE(UserDefinedDistribution), // ); } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_TYPE_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__DETERMINISTIC_SINGLE_PARAMETER_DISTRIBUTION_TYPE_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_definition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_definition.hpp index 6fcba2c500f..b5c9994ff3b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_definition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_definition.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__DISTRIBUTION_DEFINITION_HPP_ -#define OPENSCENARIO_INTERPRETER__DISTRIBUTION_DEFINITION_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__DISTRIBUTION_DEFINITION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__DISTRIBUTION_DEFINITION_HPP_ #include #include @@ -24,16 +24,18 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- DistributionDefinition 1.2 --------------------------------------------- - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + DistributionDefinition (OpenSCENARIO XML 1.3) + + Indicates whether the content defines a deterministic or stochastic parameter distribution. + + + + + + + +*/ struct DistributionDefinition : public Group { explicit DistributionDefinition(const pugi::xml_node &, Scope & scope); @@ -47,4 +49,4 @@ DEFINE_LAZY_VISITOR( } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__DISTRIBUTION_DEFINITION_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__DISTRIBUTION_DEFINITION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_range.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_range.hpp index 924eb626f18..7b508b20ddc 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_range.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_range.hpp @@ -15,6 +15,7 @@ #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__DISTRIBUTION_RANGE_HPP_ #define OPENSCENARIO_INTERPRETER__SYNTAX__DISTRIBUTION_RANGE_HPP_ +#include #include #include #include @@ -23,20 +24,30 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- DistributionRange 1.2 -------------------------------------------------- - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct DistributionRange : private Scope, public ComplexType +/* + DistributionRange (OpenSCENARIO XML 1.3) + + A range of values used for a deterministic distribution. + The range starts with lower limit, Each additional value is defined + by adding the step value to the previous value until the value is greater than upper limit. + Upper limit can be part of the range. + + + + + + + +*/ +struct DistributionRange : private Scope, public ComplexType, public SingleParameterDistributionBase { + const Double step_width; + const Range range; explicit DistributionRange(const pugi::xml_node &, Scope &); + + auto derive() -> SingleUnnamedParameterDistribution override; }; } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_set.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_set.hpp index 9244765ef98..4f91ee05bba 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_set.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_set.hpp @@ -12,30 +12,36 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__DISTRIBUTION_SET_HPP_ -#define OPENSCENARIO_INTERPRETER__DISTRIBUTION_SET_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__DISTRIBUTION_SET_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__DISTRIBUTION_SET_HPP_ +#include #include namespace openscenario_interpreter { inline namespace syntax { -/* ---- DistributionSet 1.2 ---------------------------------------------------- - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct DistributionSet : private Scope, public ComplexType +/* + DistributionSet (OpenSCENARIO XML 1.3) + + A set of possible values which can occur in a deterministic distribution. + + + + + + +*/ + +struct DistributionSet : private Scope, public ComplexType, public SingleParameterDistributionBase { const std::list elements; explicit DistributionSet(const pugi::xml_node &, Scope & scope); + + auto derive() -> SingleUnnamedParameterDistribution override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__DISTRIBUTION_SET_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__DISTRIBUTION_SET_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_set_element.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_set_element.hpp index 36081c509d6..f8a9cb3b28d 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_set_element.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distribution_set_element.hpp @@ -23,13 +23,15 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- DistributionSetElement 1.2 --------------------------------------------- - * - * - * - * - * - * ------------------------------------------------------------------------ */ +/* + DistributionSetElement (OpenSCENARIO XML 1.3) + + Indicates an element for a deterministic distribution. + + + + +*/ struct DistributionSetElement { const String value; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/histogram.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/histogram.hpp index 81088f04a03..bfc8a60a22b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/histogram.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/histogram.hpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__HISTOGRAM_HPP_ -#define OPENSCENARIO_INTERPRETER__HISTOGRAM_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__HISTOGRAM_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__HISTOGRAM_HPP_ +#include #include #include #include @@ -23,45 +24,32 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- Histogram 1.2 ---------------------------------------------------------- - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + Histogram (OpenSCENARIO XML 1.3) -struct Histogram : public ComplexType, private Scope + Histogram which can be applied to a single parameter. + + + + + + +*/ + +struct Histogram : public ComplexType, private Scope, public StochasticParameterDistributionBase { /** - * Note: HistogramBin must be stored in continuous range and ascending order, to `bins` + * Note: HistogramBin must be stored in continuous range and ascending order to `bins` + * due to restriction of `BinAdapter` */ const std::list bins; - struct BinAdaptor - { - explicit BinAdaptor(const std::list & bins) - { - intervals.emplace_back(bins.front().range.lower_limit.data); - for (const auto & bin : bins) { - intervals.emplace_back(bin.range.lower_limit.data); - densities.emplace_back(bin.weight.data); - } - intervals.emplace_back(bins.back().range.upper_limit.data); - } - std::vector intervals, densities; - } bin_adaptor; - std::piecewise_constant_distribution distribute; - std::mt19937 random_engine; - explicit Histogram(const pugi::xml_node &, Scope & scope); - auto evaluate() -> Object; + auto derive() -> Object override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__HISTOGRAM_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__HISTOGRAM_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/histogram_bin.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/histogram_bin.hpp index 19e65a4b6d5..3babd4ed866 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/histogram_bin.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/histogram_bin.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__HISTOGRAM_BIN_HPP_ -#define OPENSCENARIO_INTERPRETER__HISTOGRAM_BIN_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__HISTOGRAM_BIN_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__HISTOGRAM_BIN_HPP_ #include #include @@ -23,16 +23,18 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- HistogramBin 1.2 ------------------------------------------------------- - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + HistogramBin (OpenSCENARIO XML 1.3) + + A bin in a histogram. + + + + + + + +*/ struct HistogramBin : public ComplexType { const Range range; @@ -43,4 +45,4 @@ struct HistogramBin : public ComplexType }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__HISTOGRAM_BIN_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__HISTOGRAM_BIN_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/log_normal_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/log_normal_distribution.hpp new file mode 100644 index 00000000000..1f211e304f4 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/log_normal_distribution.hpp @@ -0,0 +1,62 @@ +// 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__LOG_NORMAL_DISTRIBUTION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__LOG_NORMAL_DISTRIBUTION_HPP_ + +#include +#include +#include +#include +#include +#include + +// cspell: ignore lognormal + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + LogNormalDistribution (OpenSCENARIO XML 1.3) + + Defines a log normal distribution. + + + + + + + + +*/ +struct LogNormalDistribution : public ComplexType, + private Scope, + public StochasticParameterDistributionBase +{ + const Range range; + + const Double expected_value; + + const Double variance; + + std::lognormal_distribution distribute; + + explicit LogNormalDistribution(const pugi::xml_node &, Scope & scope); + + auto derive() -> Object override; +}; +} // namespace syntax +} // namespace openscenario_interpreter +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__LOG_NORMAL_DISTRIBUTION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/normal_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/normal_distribution.hpp index 4ff2ec715f4..f1566582c4a 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/normal_distribution.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/normal_distribution.hpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__NORMAL_DISTRIBUTION_HPP_ -#define OPENSCENARIO_INTERPRETER__NORMAL_DISTRIBUTION_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__NORMAL_DISTRIBUTION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__NORMAL_DISTRIBUTION_HPP_ +#include #include #include #include @@ -25,19 +26,22 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- NormalDistribution 1.2 ------------------------------------------------- - * - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ - -struct NormalDistribution : public ComplexType, private Scope +/* + NormalDistribution (OpenSCENARIO XML 1.3) + + Normal distribution which can be applied to a single parameter. + + + + + + + + +*/ +struct NormalDistribution : public ComplexType, + private Scope, + public StochasticParameterDistributionBase { const Range range; @@ -47,12 +51,10 @@ struct NormalDistribution : public ComplexType, private Scope std::normal_distribution distribute; - std::mt19937 random_engine; - explicit NormalDistribution(const pugi::xml_node &, Scope & scope); - auto evaluate() -> Object; + auto derive() -> Object override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__NORMAL_DISTRIBUTION_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__NORMAL_DISTRIBUTION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_assignment.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_assignment.hpp index c34331b2f0c..8118a4b6f5f 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_assignment.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_assignment.hpp @@ -23,14 +23,19 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- ParameterAssignment-------------------------------------------------- - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + ParameterAssignment (OpenSCENARIO XML 1.3) + + Assignment of a value to a named parameter. It is either used when importing + types of vehicles, controllers etc. from a catalog during startup of a simulator. + It is also used when generating concrete scenarios from logical scenarios + with ParameterValueSets during runtime of a scenario generator. + + + + + +*/ struct ParameterAssignment { explicit ParameterAssignment(const pugi::xml_node & node, Scope & scope) @@ -41,6 +46,7 @@ struct ParameterAssignment } const std::string parameterRef; + const std::string value; }; } // namespace syntax diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_assignments.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_assignments.hpp index c90b054104d..a8e23a2d348 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_assignments.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_assignments.hpp @@ -24,14 +24,6 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- ParameterAssignment-------------------------------------------------- - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ struct ParameterAssignments : std::list { ParameterAssignments() = default; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_distribution.hpp index 4722301a4fc..8bc021a368c 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_distribution.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_distribution.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__PARAMETER_VALUE_DISTRIBUTION_HPP_ -#define OPENSCENARIO_INTERPRETER__PARAMETER_VALUE_DISTRIBUTION_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__PARAMETER_VALUE_DISTRIBUTION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__PARAMETER_VALUE_DISTRIBUTION_HPP_ #include #include @@ -22,22 +22,28 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- ParameterValueDistribution 1.2 ----------------------------------------- - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct ParameterValueDistribution : public DistributionDefinition +/* + ParameterValueDistribution (OpenSCENARIO XML 1.3) + + The ParameterValueDistribution represents + the top level container of a parameter distribution file. + + + + + + + +*/ +struct ParameterValueDistribution : public DistributionDefinition, + public ParameterDistributionContainer { const File scenario_file; explicit ParameterValueDistribution(const pugi::xml_node &, Scope & scope); + + auto derive() -> ParameterDistribution override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__PARAMETER_VALUE_DISTRIBUTION_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__PARAMETER_VALUE_DISTRIBUTION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_distribution_definition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_distribution_definition.hpp index 50b3ead719b..47c79289d37 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_distribution_definition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_distribution_definition.hpp @@ -22,15 +22,17 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- ParameterValueDistributionDefinition 1.2 ------------------------------ - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + ParameterValueDistributionDefinition (OpenSCENARIO XML 1.3) + + A marker stating that the OpenSCENARIO file is a parameter value distribution. + + + + + + +*/ struct ParameterValueDistributionDefinition : public ParameterValueDistribution { explicit ParameterValueDistributionDefinition(const pugi::xml_node &, Scope &); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_set.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_set.hpp index 236e73b1d9f..e4d436fa220 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_set.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/parameter_value_set.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__PARAMETER_VALUE_SET_HPP_ -#define OPENSCENARIO_INTERPRETER__PARAMETER_VALUE_SET_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__PARAMETER_VALUE_SET_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__PARAMETER_VALUE_SET_HPP_ #include @@ -21,21 +21,25 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- ParameterValueSet 1.2 -------------------------------------------------- - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + ParameterValueSet (OpenSCENARIO XML 1.3) + + Set of parameter values that have to be assigned for a single concrete scenario. + + + + + + +*/ struct ParameterValueSet : private Scope { const std::list parameter_assignments; explicit ParameterValueSet(const pugi::xml_node &, Scope & scope); + + auto evaluate() const -> std::shared_ptr>; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__PARAMETER_VALUE_SET_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__PARAMETER_VALUE_SET_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/poisson_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/poisson_distribution.hpp index 1eecd63de1d..b4ac24a69b7 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/poisson_distribution.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/poisson_distribution.hpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__POISSON_DISTRIBUTION_HPP_ -#define OPENSCENARIO_INTERPRETER__POISSON_DISTRIBUTION_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__POISSON_DISTRIBUTION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__POISSON_DISTRIBUTION_HPP_ +#include #include #include #include @@ -24,17 +25,21 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- PoissonDistribution 1.2 ------------------------------------------------ - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct PoissonDistribution : public ComplexType, private Scope +/* + PoissonDistribution (OpenSCENARIO XML 1.3) + + Poisson distribution which can be applied to a single parameter. + + + + + + + +*/ +struct PoissonDistribution : public ComplexType, + private Scope, + public StochasticParameterDistributionBase { const Range range; @@ -42,12 +47,10 @@ struct PoissonDistribution : public ComplexType, private Scope std::poisson_distribution<> distribute; - std::mt19937 random_engine; - explicit PoissonDistribution(const pugi::xml_node &, Scope & scope); - auto evaluate() -> Object; + auto derive() -> Object override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__POISSON_DISTRIBUTION_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__POISSON_DISTRIBUTION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/probability_distribution_set.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/probability_distribution_set.hpp index 29e2fb164aa..501c7c29352 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/probability_distribution_set.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/probability_distribution_set.hpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__PROBABILITY_DISTRIBUTION_SET_HPP_ -#define OPENSCENARIO_INTERPRETER__PROBABILITY_DISTRIBUTION_SET_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__PROBABILITY_DISTRIBUTION_SET_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__PROBABILITY_DISTRIBUTION_SET_HPP_ +#include #include #include @@ -22,17 +23,21 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- ProbabilityDistributionSet 1.2 ----------------------------------------- - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ - -struct ProbabilityDistributionSet : public ComplexType, private Scope +/* + ProbabilityDistributionSet (OpenSCENARIO XML 1.3) + + Container for a set of single values with a defined probability. + + + + + + +*/ + +struct ProbabilityDistributionSet : public ComplexType, + private Scope, + public StochasticParameterDistributionBase { const std::vector elements; @@ -52,12 +57,10 @@ struct ProbabilityDistributionSet : public ComplexType, private Scope std::discrete_distribution distribute; - std::mt19937 random_engine; - explicit ProbabilityDistributionSet(const pugi::xml_node &, Scope & scope); - auto evaluate() -> Object; + auto derive() -> Object override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__PROBABILITY_DISTRIBUTION_SET_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__PROBABILITY_DISTRIBUTION_SET_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/probability_distribution_set_element.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/probability_distribution_set_element.hpp index cc00506623e..5132d7c7dbf 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/probability_distribution_set_element.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/probability_distribution_set_element.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__PROBABILITY_DISTRIBUTION_SET_ELEMENT_HPP_ -#define OPENSCENARIO_INTERPRETER__PROBABILITY_DISTRIBUTION_SET_ELEMENT_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__PROBABILITY_DISTRIBUTION_SET_ELEMENT_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__PROBABILITY_DISTRIBUTION_SET_ELEMENT_HPP_ #include #include @@ -23,14 +23,16 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- ProbabilityDistributionSetElement 1.2 ---------------------------------- - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + ProbabilityDistributionSetElement (OpenSCENARIO XML 1.3) + + Indicates a value and probability in a stochastic distribution. + + + + + +*/ struct ProbabilityDistributionSetElement : public ComplexType { const String value; @@ -41,4 +43,4 @@ struct ProbabilityDistributionSetElement : public ComplexType }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__PROBABILITY_DISTRIBUTION_SET_ELEMENT_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__PROBABILITY_DISTRIBUTION_SET_ELEMENT_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/range.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/range.hpp index c0bda56a5fd..65e11824c11 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/range.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/range.hpp @@ -23,14 +23,16 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- Range 1.2 -------------------------------------------------------------- - * - * - * - * - * - * - * ------------------------------------------------------------------------ */ +/* + Range (OpenSCENARIO XML 1.3) + + Indicates a range for a distribution where the following rule applies: lowerLimit <= value <= upperLimit. + + + + + +*/ struct Range { const Double lower_limit = Double::infinity(); @@ -40,6 +42,11 @@ struct Range Range() = default; explicit Range(const pugi::xml_node &, Scope &); + + auto evaluate(const Double::value_type value) const -> Double::value_type + { + return std::clamp(value, lower_limit.data, upper_limit.data); + } }; } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic.hpp index 424556701b5..e291a2420f0 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic.hpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__STOCHASTIC_HPP_ -#define OPENSCENARIO_INTERPRETER__STOCHASTIC_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__STOCHASTIC_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__STOCHASTIC_HPP_ +#include #include #include #include @@ -25,29 +26,32 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- Stochastic 1.2 --------------------------------------------------------- - * - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ - -struct Stochastic : public ComplexType +/* + Stochastic (OpenSCENARIO XML 1.3) + + Top level container for all stochastic distribution elements. + + + + + + + + +*/ + +struct Stochastic : public ComplexType, public ParameterDistributionContainer, private Scope { const UnsignedInt number_of_test_runs; const Double random_seed; - const StochasticDistribution stochastic_distribution; + std::list stochastic_distributions; explicit Stochastic(const pugi::xml_node &, Scope & scope); + + auto derive() -> ParameterDistribution override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__STOCHASTIC_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__STOCHASTIC_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic_distribution.hpp index 37eb08e97e5..722c44357c1 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic_distribution.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic_distribution.hpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__STOCHASTIC_DISTRIBUTION_HPP_ -#define OPENSCENARIO_INTERPRETER__STOCHASTIC_DISTRIBUTION_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__STOCHASTIC_DISTRIBUTION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__STOCHASTIC_DISTRIBUTION_HPP_ +#include #include #include #include @@ -23,22 +24,27 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- StochasticDistribution 1.2 --------------------------------------------- - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct StochasticDistribution : public StochasticDistributionType +/* + StochasticDistribution (OpenSCENARIO XML 1.3) + + Container for a stochastic distribution which applies to a single parameter. + + + + + + + +*/ +struct StochasticDistribution : public StochasticDistributionType, + public StochasticParameterDistributionBase { const String parameter_name; explicit StochasticDistribution(const pugi::xml_node &, Scope & scope); + + auto derive() -> Object override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__STOCHASTIC_DISTRIBUTION_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__STOCHASTIC_DISTRIBUTION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic_distribution_type.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic_distribution_type.hpp index e7092757ef0..6bae5544653 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic_distribution_type.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stochastic_distribution_type.hpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__STOCHASTIC_DISTRIBUTION_TYPE_HPP_ -#define OPENSCENARIO_INTERPRETER__STOCHASTIC_DISTRIBUTION_TYPE_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__STOCHASTIC_DISTRIBUTION_TYPE_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__STOCHASTIC_DISTRIBUTION_TYPE_HPP_ #include #include +#include #include #include #include @@ -28,20 +29,23 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- StochasticDistributionType 1.2 ----------------------------------------- - * - * - * - * - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + StochasticDistributionType (OpenSCENARIO XML 1.3) + + Container for a stochastic distribution type which can be applied to a single parameter. + + + + + + + + + + + + +*/ struct StochasticDistributionType : public Group { explicit StochasticDistributionType(const pugi::xml_node &, Scope & scope); @@ -51,11 +55,12 @@ DEFINE_LAZY_VISITOR( StochasticDistributionType, CASE(ProbabilityDistributionSet), // CASE(NormalDistribution), // + CASE(LogNormalDistribution), // CASE(UniformDistribution), // CASE(PoissonDistribution), // - CASE(Histogram), // - CASE(UserDefinedDistribution), // + CASE(Histogram) // + // CASE(UserDefinedDistribution), // ); } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__STOCHASTIC_DISTRIBUTION_TYPE_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__STOCHASTIC_DISTRIBUTION_TYPE_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/uniform_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/uniform_distribution.hpp index 50c5771697b..fd6e42bf9a4 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/uniform_distribution.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/uniform_distribution.hpp @@ -12,38 +12,42 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__UNIFORM_DISTRIBUTION_HPP_ -#define OPENSCENARIO_INTERPRETER__UNIFORM_DISTRIBUTION_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__UNIFORM_DISTRIBUTION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__UNIFORM_DISTRIBUTION_HPP_ +#include #include #include #include +#include namespace openscenario_interpreter { inline namespace syntax { -/* ---- UniformDistribution 1.2 ------------------------------------------------ - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct UniformDistribution : public ComplexType, private Scope +/* + UniformDistribution (OpenSCENARIO XML 1.3) + + Uniform distribution which can be applied to a single parameter. + + + + + + +*/ +struct UniformDistribution : public ComplexType, + private Scope, + public StochasticParameterDistributionBase { const Range range; std::uniform_real_distribution distribute; - std::mt19937 random_engine; - explicit UniformDistribution(const pugi::xml_node &, Scope & scope); - auto evaluate() -> Object; + auto derive() -> Object override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__UNIFORM_DISTRIBUTION_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__UNIFORM_DISTRIBUTION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_distribution.hpp index 1d8e3343963..d870fe6285c 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_distribution.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_distribution.hpp @@ -22,24 +22,27 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- UserDefinedDistribution 1.2 -------------------------------------------- - * - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + UserDefinedDistribution (OpenSCENARIO XML 1.3) + + Indicates a user defined distribution which can be deterministic or stochastic. + This distribution contains a property with the name type and some user defined content. + + + + + + + + +*/ struct UserDefinedDistribution : private Scope, public ComplexType { const String type; explicit UserDefinedDistribution(const pugi::xml_node &, const Scope &); - // TODO: implement evaluate()?` + auto evaluate() -> Object; }; } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/value_set_distribution.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/value_set_distribution.hpp index b57e233c0ab..296f07853f8 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/value_set_distribution.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/value_set_distribution.hpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OPENSCENARIO_INTERPRETER__VALUE_SET_DISTRIBUTION_HPP_ -#define OPENSCENARIO_INTERPRETER__VALUE_SET_DISTRIBUTION_HPP_ +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__VALUE_SET_DISTRIBUTION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__VALUE_SET_DISTRIBUTION_HPP_ +#include #include #include @@ -22,23 +23,25 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- ValueSetDistribution 1.2 ----------------------------------------------- - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ -struct ValueSetDistribution : public Scope +/* + ValueSetDistribution (OpenSCENARIO XML 1.3) + + Deterministic multi-parameter distribution, where one or multiple sets of parameter values can be defined. + + + + + + +*/ +struct ValueSetDistribution : public Scope, public MultiParameterDistributionBase { const std::list parameter_value_sets; explicit ValueSetDistribution(const pugi::xml_node &, Scope & scope); - // TODO: implement evaluate() + auto derive() -> ParameterDistribution override; }; } // namespace syntax } // namespace openscenario_interpreter -#endif // OPENSCENARIO_INTERPRETER__VALUE_SET_DISTRIBUTION_HPP_ +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__VALUE_SET_DISTRIBUTION_HPP_ diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 0ee8dddd8df..bbe33b61423 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -26,6 +26,7 @@ tier4_simulation_msgs traffic_simulator traffic_simulator_msgs + ament_index_cpp ament_cmake_clang_format ament_cmake_copyright @@ -33,6 +34,7 @@ ament_cmake_pep257 ament_cmake_xmllint ament_lint_auto + ament_index_cpp ament_cmake diff --git a/openscenario/openscenario_interpreter/src/parameter_distribution.cpp b/openscenario/openscenario_interpreter/src/parameter_distribution.cpp new file mode 100644 index 00000000000..30496508288 --- /dev/null +++ b/openscenario/openscenario_interpreter/src/parameter_distribution.cpp @@ -0,0 +1,43 @@ +// 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 + +namespace openscenario_interpreter +{ +auto mergeParameterDistribution( + const ParameterDistribution & distribution, const ParameterDistribution & additional_distribution) + -> ParameterDistribution +{ + if (distribution.empty() and additional_distribution.empty()) { + throw common::Error( + "trying to merge two parameter distributions but failed because both are empty"); + } else if (distribution.empty()) { + return additional_distribution; + } else if (additional_distribution.empty()) { + return distribution; + } else { + ParameterDistribution merged_distribution; + merged_distribution.reserve(distribution.size() * additional_distribution.size()); + for (const ParameterSetSharedPtr & parameter_set : distribution) { + for (const ParameterSetSharedPtr & additional_parameter_set : additional_distribution) { + auto merged_set = ParameterSet{*parameter_set}; + merged_set.insert(additional_parameter_set->cbegin(), additional_parameter_set->cend()); + merged_distribution.emplace_back(std::make_shared(merged_set)); + } + } + return merged_distribution; + } +} +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/deterministic.cpp b/openscenario/openscenario_interpreter/src/syntax/deterministic.cpp index d02ba05e42a..8c3d20174b4 100644 --- a/openscenario/openscenario_interpreter/src/syntax/deterministic.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/deterministic.cpp @@ -24,5 +24,17 @@ Deterministic::Deterministic(const pugi::xml_node & node, Scope & scope) readGroups(node, scope)) { } + +auto Deterministic::derive() -> ParameterDistribution +{ + ParameterDistribution distribution; + for (auto additional_distribution : deterministic_parameter_distributions) { + distribution = mergeParameterDistribution( + distribution, + apply( + [](auto & distribution) { return distribution.derive(); }, additional_distribution)); + } + return distribution; +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/deterministic_multi_parameter_distribution.cpp b/openscenario/openscenario_interpreter/src/syntax/deterministic_multi_parameter_distribution.cpp index 88ca2a52b9c..7e4563239fb 100644 --- a/openscenario/openscenario_interpreter/src/syntax/deterministic_multi_parameter_distribution.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/deterministic_multi_parameter_distribution.cpp @@ -24,5 +24,10 @@ DeterministicMultiParameterDistribution::DeterministicMultiParameterDistribution : DeterministicMultiParameterDistributionType(node, scope) { } + +auto DeterministicMultiParameterDistribution::derive() -> ParameterDistribution +{ + return DeterministicMultiParameterDistributionType::derive(); +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/deterministic_parameter_distribution.cpp b/openscenario/openscenario_interpreter/src/syntax/deterministic_parameter_distribution.cpp index 1fa382f686b..728dc0af531 100644 --- a/openscenario/openscenario_interpreter/src/syntax/deterministic_parameter_distribution.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/deterministic_parameter_distribution.cpp @@ -19,11 +19,62 @@ namespace openscenario_interpreter { inline namespace syntax { +/** + * Note: + * Most Groups are used only once in one scope of OpenSCENARIO. + * And they can use `choice` function to load the group from the scenario, despite they don't have `xsd:choice` attribute. + * But, DeterministicParameterDistribution group can be used many times in same scope. + * That prevents from using `choice` function. + * Now, we defined new function `readGroup` instead of `choice` + */ +template +auto readGroup(const pugi::xml_node & node, Ts &&... xs) +{ + const std::unordered_map> callees{ + std::forward(xs)...}; + + std::unordered_map specs{}; + + auto print_keys_to = [&](auto & os, const auto & xs) -> decltype(auto) { + if (not xs.empty()) { + for (auto iter = std::begin(xs); iter != std::end(xs); ++iter) { + os << std::get<0>(*iter); + + switch (std::distance(iter, std::end(xs))) { + case 1: + return os; + + case 2: + os << " and "; + break; + + default: + os << ", "; + break; + } + } + } + + return os; + }; + + if (auto callee = callees.find(node.name()); callee != callees.end()) { + return callee->second(node); + } else { + std::stringstream what; + what << "Class " << node.name() << " requires one of following elements: "; + print_keys_to(what, callees); + what << ". But no element specified"; + std::cout << what.str() << std::endl; + throw SyntaxError(what.str()); + } +} + DeterministicParameterDistribution::DeterministicParameterDistribution( - const pugi::xml_node & node, Scope & scope) + const pugi::xml_node & tree, Scope & scope) // clang-format off : Group( - choice(node, + readGroup(tree, std::make_pair("DeterministicMultiParameterDistribution", [&](auto && node){return make(node,scope);}), std::make_pair("DeterministicSingleParameterDistribution", [&](auto && node){return make(node,scope);}))) // clang-format on diff --git a/openscenario/openscenario_interpreter/src/syntax/deterministic_single_parameter_distribution.cpp b/openscenario/openscenario_interpreter/src/syntax/deterministic_single_parameter_distribution.cpp index a6eb09ad078..af24c36f697 100644 --- a/openscenario/openscenario_interpreter/src/syntax/deterministic_single_parameter_distribution.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/deterministic_single_parameter_distribution.cpp @@ -25,5 +25,20 @@ DeterministicSingleParameterDistribution::DeterministicSingleParameterDistributi parameter_name(readAttribute("parameterName", node, scope)) { } + +auto DeterministicSingleParameterDistribution::derive() -> ParameterDistribution +{ + ParameterDistribution distribution; + return apply( + [&](auto & unnamed_distribution) { + ParameterDistribution distribution; + for (const auto & unnamed_parameter : unnamed_distribution.derive()) { + distribution.emplace_back( + std::make_shared(ParameterSet{{parameter_name, unnamed_parameter}})); + } + return distribution; + }, + *this); +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/deterministic_single_parameter_distribution_type.cpp b/openscenario/openscenario_interpreter/src/syntax/deterministic_single_parameter_distribution_type.cpp index 76296e5e95d..cce7e14edac 100644 --- a/openscenario/openscenario_interpreter/src/syntax/deterministic_single_parameter_distribution_type.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/deterministic_single_parameter_distribution_type.cpp @@ -20,10 +20,10 @@ namespace openscenario_interpreter inline namespace syntax { DeterministicSingleParameterDistributionType::DeterministicSingleParameterDistributionType( - const pugi::xml_node & node, Scope & scope) + const pugi::xml_node & tree, Scope & scope) // clang-format off : Group( - choice(node, + choice(tree, std::make_pair("DistributionSet", [&](auto && node){ return make(node, scope);}), std::make_pair("DistributionRange", [&](auto && node){ return make(node, scope);}), std::make_pair("UserDefinedDistribution", [&](auto && node){ return make(node, scope);}))) diff --git a/openscenario/openscenario_interpreter/src/syntax/distribution_definition.cpp b/openscenario/openscenario_interpreter/src/syntax/distribution_definition.cpp index 7499024a894..002386aeedc 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distribution_definition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distribution_definition.cpp @@ -23,8 +23,8 @@ DistributionDefinition::DistributionDefinition(const pugi::xml_node & tree, Scop // clang-format off : Group( choice(tree, - std::make_pair("Deterministic", [&](auto && node){return make(node,scope);}), - std::make_pair("Stochastic", [&](auto && node){return make(node,scope);}))) + std::make_pair("Deterministic", [&](auto && node){return make(node,scope);}), + std::make_pair("Stochastic", [&](auto && node){return make(node,scope);}))) // clang-format on { } diff --git a/openscenario/openscenario_interpreter/src/syntax/distribution_range.cpp b/openscenario/openscenario_interpreter/src/syntax/distribution_range.cpp index f90a7697f96..90941427dc1 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distribution_range.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distribution_range.cpp @@ -20,9 +20,22 @@ namespace openscenario_interpreter inline namespace syntax { DistributionRange::DistributionRange(const pugi::xml_node & node, Scope & scope) -: Scope(scope), range(readElement("Range", node, local())) +: Scope(scope), + step_width(readAttribute("stepWidth", node, local())), + range(readElement("Range", node, local())) { } +auto DistributionRange::derive() -> SingleUnnamedParameterDistribution +{ + SingleUnnamedParameterDistribution unnamed_distribution; + const auto number_of_parameters = + static_cast((range.upper_limit - range.lower_limit) / step_width + 1); + for (std::size_t i = 0; i < number_of_parameters; ++i) { + unnamed_distribution.emplace_back( + make(range.lower_limit + static_cast(i) * step_width)); + } + return unnamed_distribution; +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/distribution_set.cpp b/openscenario/openscenario_interpreter/src/syntax/distribution_set.cpp index eb2b62b07a0..7a023200e43 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distribution_set.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distribution_set.cpp @@ -23,5 +23,14 @@ DistributionSet::DistributionSet(const pugi::xml_node & node, Scope & scope) : Scope(scope), elements(readElements("Element", node, local())) { } + +auto DistributionSet::derive() -> SingleUnnamedParameterDistribution +{ + SingleUnnamedParameterDistribution distribution; + for (const auto & element : elements) { + distribution.emplace_back(make(element.value)); + } + return distribution; +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/histogram.cpp b/openscenario/openscenario_interpreter/src/syntax/histogram.cpp index 5017ff1752c..3a65b6c9be9 100644 --- a/openscenario/openscenario_interpreter/src/syntax/histogram.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/histogram.cpp @@ -20,15 +20,19 @@ namespace openscenario_interpreter inline namespace syntax { Histogram::Histogram(const pugi::xml_node & node, openscenario_interpreter::Scope & scope) -: Scope(scope), - bins(readElements("Bin", node, scope)), - bin_adaptor(bins), - distribute( - bin_adaptor.intervals.begin(), bin_adaptor.intervals.end(), bin_adaptor.densities.begin()), - random_engine(scope.seed) +: Scope(scope), bins(readElements("Bin", node, scope)), distribute([this]() { + std::vector intervals, densities; + intervals.emplace_back(bins.front().range.lower_limit.data); + for (const auto & bin : bins) { + intervals.emplace_back(bin.range.upper_limit.data); + densities.emplace_back(bin.weight.data); + } + return std::piecewise_constant_distribution::param_type( + intervals.begin(), intervals.end(), densities.begin()); + }()) { } -auto Histogram::evaluate() -> Object { return make(distribute(random_engine)); } +auto Histogram::derive() -> Object { return make(distribute(random_engine)); } } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/histogram_bin.cpp b/openscenario/openscenario_interpreter/src/syntax/histogram_bin.cpp index efba445abcf..f11d485b6a2 100644 --- a/openscenario/openscenario_interpreter/src/syntax/histogram_bin.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/histogram_bin.cpp @@ -20,7 +20,7 @@ namespace openscenario_interpreter inline namespace syntax { HistogramBin::HistogramBin(const pugi::xml_node & node, openscenario_interpreter::Scope & scope) -: range(readElement("range", node, scope)), +: range(readElement("Range", node, scope)), weight(readAttribute("weight", node, scope)) { } diff --git a/openscenario/openscenario_interpreter/src/syntax/log_normal_distribution.cpp b/openscenario/openscenario_interpreter/src/syntax/log_normal_distribution.cpp new file mode 100644 index 00000000000..891e72e1867 --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/log_normal_distribution.cpp @@ -0,0 +1,48 @@ +// 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 + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +LogNormalDistribution::LogNormalDistribution( + const pugi::xml_node & node, openscenario_interpreter::Scope & scope) +: Scope(scope), + range(readElement("Range", node, scope)), + expected_value(readAttribute("expectedValue", node, scope)), + variance(readAttribute("variance", node, scope)), + distribute( + static_cast(expected_value.data), std::sqrt(static_cast(variance.data))) +{ +} + +auto LogNormalDistribution::derive() -> Object +{ + return make([&]() { + double value; + auto in_range = [this](double value) { + return range.lower_limit.data <= value and value <= range.upper_limit.data; + }; + do { + value = distribute(random_engine); + } while (not in_range(value)); + return value; + }()); +} +} // namespace syntax +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/normal_distribution.cpp b/openscenario/openscenario_interpreter/src/syntax/normal_distribution.cpp index 03ee4cdd54d..cb8c404de0b 100644 --- a/openscenario/openscenario_interpreter/src/syntax/normal_distribution.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/normal_distribution.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include @@ -22,14 +23,26 @@ inline namespace syntax NormalDistribution::NormalDistribution( const pugi::xml_node & node, openscenario_interpreter::Scope & scope) : Scope(scope), - range(readElement("range", node, scope)), + range(readElement("Range", node, scope)), expected_value(readAttribute("expectedValue", node, scope)), variance(readAttribute("variance", node, scope)), - distribute(static_cast(expected_value.data), static_cast(variance.data)), - random_engine(scope.seed) + distribute( + static_cast(expected_value.data), std::sqrt(static_cast(variance.data))) { } -auto NormalDistribution::evaluate() -> Object { return make(distribute(random_engine)); } +auto NormalDistribution::derive() -> Object +{ + return make([&]() { + double value; + auto in_range = [this](double value) { + return range.lower_limit.data <= value and value <= range.upper_limit.data; + }; + do { + value = distribute(random_engine); + } while (not in_range(value)); + return value; + }()); +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/open_scenario_category.cpp b/openscenario/openscenario_interpreter/src/syntax/open_scenario_category.cpp index 2c4d6e898c6..c065a989c5f 100644 --- a/openscenario/openscenario_interpreter/src/syntax/open_scenario_category.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/open_scenario_category.cpp @@ -25,9 +25,9 @@ OpenScenarioCategory::OpenScenarioCategory(const pugi::xml_node & tree, Scope & : Group( // clang-format off choice(tree, - std::make_pair("Storyboard", [&](auto && ) { return make(tree, scope);}), // DIRTY HACK!!! - std::make_pair("Catalog", [&](auto && ) { return make(tree, scope);}), - std::make_pair("ParameterValueDistribution",[&](auto && node) { return make(node, scope);}))) + std::make_pair("Storyboard", [&](auto &&) { return make(tree, scope);}), // DIRTY HACK!!! + std::make_pair("Catalog", [&](auto &&) { return make(tree, scope);}), + std::make_pair("ParameterValueDistribution",[&](auto &&) { return make(tree, scope);}))) // clang-format on { } diff --git a/openscenario/openscenario_interpreter/src/syntax/parameter_value_distribution.cpp b/openscenario/openscenario_interpreter/src/syntax/parameter_value_distribution.cpp index 63a2c7eb5ad..8dbb0ae61bc 100644 --- a/openscenario/openscenario_interpreter/src/syntax/parameter_value_distribution.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/parameter_value_distribution.cpp @@ -24,5 +24,11 @@ ParameterValueDistribution::ParameterValueDistribution( : DistributionDefinition(node, scope), scenario_file(readElement("ScenarioFile", node, scope)) { } + +auto ParameterValueDistribution::derive() -> ParameterDistribution +{ + return apply( + [](auto & distribution) { return distribution.derive(); }, *this); +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/parameter_value_distribution_definition.cpp b/openscenario/openscenario_interpreter/src/syntax/parameter_value_distribution_definition.cpp index a95da7efc4e..3deb58221a4 100644 --- a/openscenario/openscenario_interpreter/src/syntax/parameter_value_distribution_definition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/parameter_value_distribution_definition.cpp @@ -22,7 +22,7 @@ inline namespace syntax ParameterValueDistributionDefinition::ParameterValueDistributionDefinition( const pugi::xml_node & node, Scope & scope) : ParameterValueDistribution( - readElement("parameterValueDistribution", node, scope)) + readElement("ParameterValueDistribution", node, scope)) { } } // namespace syntax diff --git a/openscenario/openscenario_interpreter/src/syntax/parameter_value_set.cpp b/openscenario/openscenario_interpreter/src/syntax/parameter_value_set.cpp index 0c727b73516..e3538eaa0ec 100644 --- a/openscenario/openscenario_interpreter/src/syntax/parameter_value_set.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/parameter_value_set.cpp @@ -25,5 +25,15 @@ ParameterValueSet::ParameterValueSet( parameter_assignments(readElements("ParameterAssignment", node, local())) { } + +auto ParameterValueSet::evaluate() const -> std::shared_ptr> +{ + std::shared_ptr> parameters = + std::make_shared>(); + for (const auto & parameter : parameter_assignments) { + (*parameters)[parameter.parameterRef] = make(parameter.value); + } + return parameters; +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/poisson_distribution.cpp b/openscenario/openscenario_interpreter/src/syntax/poisson_distribution.cpp index 032bf5a8b31..ba62599d0fc 100644 --- a/openscenario/openscenario_interpreter/src/syntax/poisson_distribution.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/poisson_distribution.cpp @@ -22,13 +22,24 @@ inline namespace syntax PoissonDistribution::PoissonDistribution( const pugi::xml_node & node, openscenario_interpreter::Scope & scope) : Scope(scope), - range(readElement("range", node, scope)), + range(readElement("Range", node, scope)), expected_value(readAttribute("expectedValue", node, scope)), - distribute(expected_value.data), - random_engine(scope.seed) + distribute(expected_value.data) { } -auto PoissonDistribution::evaluate() -> Object { return make(distribute(random_engine)); } +auto PoissonDistribution::derive() -> Object +{ + return make([&]() { + double value; + auto in_range = [this](double value) { + return range.lower_limit.data <= value and value <= range.upper_limit.data; + }; + do { + value = distribute(random_engine); + } while (not in_range(value)); + return value; + }()); +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/probability_distribution_set.cpp b/openscenario/openscenario_interpreter/src/syntax/probability_distribution_set.cpp index 94dd9c72619..d635089a7c7 100644 --- a/openscenario/openscenario_interpreter/src/syntax/probability_distribution_set.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/probability_distribution_set.cpp @@ -32,15 +32,14 @@ ProbabilityDistributionSet::ProbabilityDistributionSet( elements( generateVector(readElements("Element", node, scope))), adaptor(elements), - distribute(adaptor.probabilities.begin(), adaptor.probabilities.end()), - random_engine(scope.seed) + distribute(adaptor.probabilities.begin(), adaptor.probabilities.end()) { } -auto ProbabilityDistributionSet::evaluate() -> Object +auto ProbabilityDistributionSet::derive() -> Object { - size_t index = distribute(random_engine); - return elements.at(index); + std::size_t index = distribute(random_engine); + return make(elements.at(index).value); } } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/stochastic.cpp b/openscenario/openscenario_interpreter/src/syntax/stochastic.cpp index ef57a177ee3..cf1978ada73 100644 --- a/openscenario/openscenario_interpreter/src/syntax/stochastic.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/stochastic.cpp @@ -25,12 +25,30 @@ inline namespace syntax the OpenSCENARIO standard, so it may change in the future. */ Stochastic::Stochastic(const pugi::xml_node & node, Scope & scope) -: number_of_test_runs(readAttribute("numberOfTestRuns", node, scope)), - random_seed( - scope.seed = static_cast(readAttribute("randomSeed", node, scope, 0))), - stochastic_distribution( - readElement("StochasticDistribution", node, scope)) +: Scope(scope), + number_of_test_runs(readAttribute("numberOfTestRuns", node, scope)), + random_seed([&] { + auto seed = static_cast(readAttribute("randomSeed", node, scope, 0)); + scope.random_engine.seed(seed); + return seed; + }()), + stochastic_distributions( + readElements("StochasticDistribution", node, scope)) { } + +auto Stochastic::derive() -> ParameterDistribution +{ + ParameterDistribution distribution; + for (std::size_t i = 0; i < number_of_test_runs; i++) { + ParameterSetSharedPtr parameter_set = std::make_shared(); + for (auto & stochastic_distribution : stochastic_distributions) { + auto derived = stochastic_distribution.derive(); + parameter_set->emplace(stochastic_distribution.parameter_name, derived); + } + distribution.emplace_back(parameter_set); + } + return distribution; +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution.cpp b/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution.cpp index f4b25346a0d..963ddd6ac8a 100644 --- a/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution.cpp @@ -24,5 +24,11 @@ StochasticDistribution::StochasticDistribution(const pugi::xml_node & node, Scop parameter_name(readAttribute("parameterName", node, scope)) { } + +auto StochasticDistribution::derive() -> Object +{ + return apply( + [](auto & unnamed_distribution) { return unnamed_distribution.derive(); }, *this); +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution_type.cpp b/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution_type.cpp index 4f2d334ae5b..62f068c5289 100644 --- a/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution_type.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution_type.cpp @@ -24,6 +24,7 @@ StochasticDistributionType::StochasticDistributionType(const pugi::xml_node & no : Group( choice(node, std::make_pair("ProbabilityDistributionSet", [&](auto && node){return make(node, scope);}), + std::make_pair("LogNormalDistribution", [&](auto && node){return make(node, scope);}), std::make_pair("NormalDistribution", [&](auto && node){return make(node, scope);}), std::make_pair("UniformDistribution", [&](auto && node){return make(node, scope);}), std::make_pair("PoissonDistribution", [&](auto && node){return make(node, scope);}), diff --git a/openscenario/openscenario_interpreter/src/syntax/uniform_distribution.cpp b/openscenario/openscenario_interpreter/src/syntax/uniform_distribution.cpp index 8bf6bb49284..c38379a76b8 100644 --- a/openscenario/openscenario_interpreter/src/syntax/uniform_distribution.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/uniform_distribution.cpp @@ -22,12 +22,23 @@ inline namespace syntax UniformDistribution::UniformDistribution( const pugi::xml_node & node, openscenario_interpreter::Scope & scope) : Scope(scope), - range(readElement("range", node, scope)), - distribute(range.lower_limit.data, range.upper_limit.data), - random_engine(scope.seed) + range(readElement("Range", node, scope)), + distribute(range.lower_limit.data, range.upper_limit.data) { } -auto UniformDistribution::evaluate() -> Object { return make(distribute(random_engine)); } +auto UniformDistribution::derive() -> Object +{ + return make([&]() { + double value; + auto in_range = [this](double value) { + return range.lower_limit.data <= value and value <= range.upper_limit.data; + }; + do { + value = distribute(random_engine); + } while (not in_range(value)); + return value; + }()); +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/user_defined_distribution.cpp b/openscenario/openscenario_interpreter/src/syntax/user_defined_distribution.cpp index f5cf2920405..60bd74e10c0 100644 --- a/openscenario/openscenario_interpreter/src/syntax/user_defined_distribution.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/user_defined_distribution.cpp @@ -24,5 +24,10 @@ UserDefinedDistribution::UserDefinedDistribution(const pugi::xml_node & node, co : Scope(scope), type(readAttribute("type", node, local())) { } + +auto UserDefinedDistribution::evaluate() -> Object +{ + throw common::Error("UserDefinedDistribution is not implemented yet"); +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/value_set_distribution.cpp b/openscenario/openscenario_interpreter/src/syntax/value_set_distribution.cpp index c171ba2a4cb..eafe1a64f2c 100644 --- a/openscenario/openscenario_interpreter/src/syntax/value_set_distribution.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/value_set_distribution.cpp @@ -25,5 +25,14 @@ ValueSetDistribution::ValueSetDistribution( parameter_value_sets(readElements("ParameterValueSet", node, scope)) { } + +auto ValueSetDistribution::derive() -> ParameterDistribution +{ + ParameterDistribution parameters; + for (const auto & parameter_value_set : parameter_value_sets) { + parameters.emplace_back(parameter_value_set.evaluate()); + } + return parameters; +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.DistributionRange.xosc b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.DistributionRange.xosc new file mode 100644 index 00000000000..33d6ea35699 --- /dev/null +++ b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.DistributionRange.xosc @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.DistributionSet.xosc b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.DistributionSet.xosc new file mode 100644 index 00000000000..900551807d3 --- /dev/null +++ b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.DistributionSet.xosc @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.ValueSetDistribution.xosc b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.ValueSetDistribution.xosc new file mode 100644 index 00000000000..3c053ea0fcd --- /dev/null +++ b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.ValueSetDistribution.xosc @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.xosc b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.xosc new file mode 100644 index 00000000000..c1811ce047b --- /dev/null +++ b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Deterministic.xosc @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.Histogram.xosc b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.Histogram.xosc new file mode 100644 index 00000000000..aba60e51f92 --- /dev/null +++ b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.Histogram.xosc @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.LogNormalDistribution.xosc b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.LogNormalDistribution.xosc new file mode 100644 index 00000000000..fa30d31f18e --- /dev/null +++ b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.LogNormalDistribution.xosc @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.NormalDistribution.xosc b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.NormalDistribution.xosc new file mode 100644 index 00000000000..1f46ec69d76 --- /dev/null +++ b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.NormalDistribution.xosc @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.PoissonDistribution.xosc b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.PoissonDistribution.xosc new file mode 100644 index 00000000000..8a5aadff8b8 --- /dev/null +++ b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.PoissonDistribution.xosc @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.ProbabilityDistributionSet.xosc b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.ProbabilityDistributionSet.xosc new file mode 100644 index 00000000000..aa552b138a6 --- /dev/null +++ b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.ProbabilityDistributionSet.xosc @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.UniformDistribution.xosc b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.UniformDistribution.xosc new file mode 100644 index 00000000000..933394612e5 --- /dev/null +++ b/openscenario/openscenario_interpreter/test/parameter_value_distribution/Stochastic.UniformDistribution.xosc @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/openscenario/openscenario_interpreter/test/parameter_value_distribution/sample.xosc b/openscenario/openscenario_interpreter/test/parameter_value_distribution/sample.xosc new file mode 100644 index 00000000000..d8933818459 --- /dev/null +++ b/openscenario/openscenario_interpreter/test/parameter_value_distribution/sample.xosc @@ -0,0 +1,146 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/openscenario/openscenario_interpreter/test/test_parameter_value_distribution.cpp b/openscenario/openscenario_interpreter/test/test_parameter_value_distribution.cpp new file mode 100644 index 00000000000..a499d2b9341 --- /dev/null +++ b/openscenario/openscenario_interpreter/test/test_parameter_value_distribution.cpp @@ -0,0 +1,359 @@ +// 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 +#include +#include +#include +#include + +TEST(syntax, dummy) { ASSERT_TRUE(true); } + +using openscenario_interpreter::OpenScenario; +using openscenario_interpreter::ParameterDistribution; +using openscenario_interpreter::ParameterValueDistribution; +using openscenario_interpreter::ParameterValueDistributionDefinition; + +void checkParameterValueDistribution(const std::string path, const ParameterDistribution & expected) +{ + OpenScenario script{path}; + auto actual = script.category.as().derive(); + EXPECT_EQ(expected.size(), actual.size()); + + for (std::size_t i = 0; i < expected.size(); i++) { + const auto & expected_parameter_list = expected.at(i); + const auto & actual_parameter_list = actual.at(i); + EXPECT_EQ(expected_parameter_list->size(), actual_parameter_list->size()); + for (const auto & [expected_parameter_name, expected_parameter] : *expected_parameter_list) { + const auto & actual_parameter = actual_parameter_list->at(expected_parameter_name); + using openscenario_interpreter::Boolean; + using openscenario_interpreter::Double; + using openscenario_interpreter::Integer; + using openscenario_interpreter::String; + using openscenario_interpreter::UnsignedInt; + using openscenario_interpreter::UnsignedShort; + if (expected_parameter.is()) { + EXPECT_EQ(expected_parameter.as(), actual_parameter.as()); + } else if (expected_parameter.is()) { + EXPECT_EQ(expected_parameter.as(), actual_parameter.as()); + } else if (expected_parameter.is()) { + EXPECT_EQ(expected_parameter.as(), actual_parameter.as()); + } else if (expected_parameter.is()) { + EXPECT_EQ(expected_parameter.as(), actual_parameter.as()); + } else if (expected_parameter.is()) { + EXPECT_DOUBLE_EQ(expected_parameter.as(), actual_parameter.as()); + } else if (expected_parameter.is()) { + EXPECT_EQ(expected_parameter.as(), actual_parameter.as()); + } else { + THROW_SYNTAX_ERROR(std::quoted(expected_parameter_name), "is unexpected type"); + } + } + } +} + +template +auto makeParameterListSharedPtr(const std::string & name, U value) +{ + auto parameter_set = std::make_shared(); + (*parameter_set)[name] = openscenario_interpreter::make(value); + return parameter_set; +} + +TEST(ParameterValueDistribution, DistributionRange) +{ + using ament_index_cpp::get_package_share_directory; + using openscenario_interpreter::Double; + + std::string path = get_package_share_directory("openscenario_interpreter") + + "/test/parameter_value_distribution/Deterministic.DistributionRange.xosc"; + + double offset0 = -1.0 + 0.53 * 0; // -1.0 + double offset1 = -1.0 + 0.53 * 1; // -0.47 + double offset2 = -1.0 + 0.53 * 2; // 0.06 + double offset3 = -1.0 + 0.53 * 3; // 0.59 + + ParameterDistribution expected_distribution; + expected_distribution.push_back(makeParameterListSharedPtr("offset", offset0)); + expected_distribution.push_back(makeParameterListSharedPtr("offset", offset1)); + expected_distribution.push_back(makeParameterListSharedPtr("offset", offset2)); + expected_distribution.push_back(makeParameterListSharedPtr("offset", offset3)); + + checkParameterValueDistribution(path, expected_distribution); +} + +TEST(ParameterValueDistribution, DistributionSet) +{ + using ament_index_cpp::get_package_share_directory; + using openscenario_interpreter::String; + + std::string path = get_package_share_directory("openscenario_interpreter") + + "/test/parameter_value_distribution/Deterministic.DistributionSet.xosc"; + + ParameterDistribution expected_distribution; + expected_distribution.push_back(makeParameterListSharedPtr("LANE_ID", "34510")); + expected_distribution.push_back(makeParameterListSharedPtr("LANE_ID", "34513")); + expected_distribution.push_back(makeParameterListSharedPtr("LANE_ID", "34564")); + + checkParameterValueDistribution(path, expected_distribution); +} + +TEST(ParameterValueDistribution, ValueSetDistribution) +{ + using ament_index_cpp::get_package_share_directory; + using openscenario_interpreter::make; + using openscenario_interpreter::String; + + std::string path = get_package_share_directory("openscenario_interpreter") + + "/test/parameter_value_distribution/Deterministic.ValueSetDistribution.xosc"; + + ParameterDistribution expected_distribution; + auto parameter_set = std::make_shared(); + (*parameter_set)["LANE_ID"] = make("34564"); + (*parameter_set)["offset"] = make("1.0"); + expected_distribution.push_back(parameter_set); + + checkParameterValueDistribution(path, expected_distribution); +} + +TEST(ParameterValueDistribution, Deterministic) +{ + using ament_index_cpp::get_package_share_directory; + using openscenario_interpreter::Double; + using openscenario_interpreter::make; + using openscenario_interpreter::String; + + std::string path = get_package_share_directory("openscenario_interpreter") + + "/test/parameter_value_distribution/Deterministic.xosc"; + + ParameterDistribution expected_distribution; + auto make_parameter_set = [](double offset, std::string lane_id) { + auto parameter_set = std::make_shared(); + (*parameter_set)["offset"] = make(offset); + (*parameter_set)["LANE_ID"] = make(lane_id); + return parameter_set; + }; + double offset0 = -1.0 + 0.53 * 0; // -1.0 + double offset1 = -1.0 + 0.53 * 1; // -0.47 + double offset2 = -1.0 + 0.53 * 2; // 0.06 + double offset3 = -1.0 + 0.53 * 3; // 0.59 + std::string lane_id0 = "34510"; + std::string lane_id1 = "34513"; + std::string lane_id2 = "34564"; + + { + expected_distribution.push_back(make_parameter_set(offset0, lane_id0)); + expected_distribution.push_back(make_parameter_set(offset0, lane_id1)); + expected_distribution.push_back(make_parameter_set(offset0, lane_id2)); + } + { + expected_distribution.push_back(make_parameter_set(offset1, lane_id0)); + expected_distribution.push_back(make_parameter_set(offset1, lane_id1)); + expected_distribution.push_back(make_parameter_set(offset1, lane_id2)); + } + { + expected_distribution.push_back(make_parameter_set(offset2, lane_id0)); + expected_distribution.push_back(make_parameter_set(offset2, lane_id1)); + expected_distribution.push_back(make_parameter_set(offset2, lane_id2)); + } + { + expected_distribution.push_back(make_parameter_set(offset3, lane_id0)); + expected_distribution.push_back(make_parameter_set(offset3, lane_id1)); + expected_distribution.push_back(make_parameter_set(offset3, lane_id2)); + } + + checkParameterValueDistribution(path, expected_distribution); +} + +// Histogram +TEST(ParameterValueDistribution, Histogram) +{ + using ament_index_cpp::get_package_share_directory; + using openscenario_interpreter::Double; + + std::string path = get_package_share_directory("openscenario_interpreter") + + "/test/parameter_value_distribution/Stochastic.Histogram.xosc"; + + ParameterDistribution expected_distribution; + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.73692442452247864)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.082699735953560394)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.5620816275750421)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.35772943348137098)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.86938579245347758)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.038832744045494971)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.93085577907030514)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.059400386282114415)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.98460362787680167)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.8663155254748649)); + expected_distribution.push_back(makeParameterListSharedPtr("offset", 0.3735454248180905)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.86087298993938566)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.053857555519812195)); + expected_distribution.push_back(makeParameterListSharedPtr("offset", 0.3078379243610454)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.40238118899835329)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.52439607999726823)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.90507097322737606)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.34353154767998462)); + expected_distribution.push_back(makeParameterListSharedPtr("offset", 0.5128209722651722)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.26932265821684642)); + + // NOTE: + // 1, range: -1.0 ~ -0.5, ideal: 5/20, actual: 6/20 + // 2: range: -0.5 ~ 0.5, ideal: 10/20, actual: 10/20 + // 3: range: 0.5 ~ 1.0, ideal: 5/20, actual: 4/20 + + checkParameterValueDistribution(path, expected_distribution); +} + +TEST(ParameterValueDistribution, LogNormalDistribution) +{ + using ament_index_cpp::get_package_share_directory; + using openscenario_interpreter::Double; + + std::string path = get_package_share_directory("openscenario_interpreter") + + "/test/parameter_value_distribution/Stochastic.LogNormalDistribution.xosc"; + + ParameterDistribution expected_distribution; + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.679981508546370405632330857770)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.032166799749118187012886238563)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.033371713868564217841949925969)); + + checkParameterValueDistribution(path, expected_distribution); +} + +TEST(ParameterValueDistribution, NormalDistribution) +{ + using ament_index_cpp::get_package_share_directory; + using openscenario_interpreter::Double; + + std::string path = get_package_share_directory("openscenario_interpreter") + + "/test/parameter_value_distribution/Stochastic.NormalDistribution.xosc"; + + ParameterDistribution expected_distribution; + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.086242833039257865701543437353)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.768496409013107117935703627154)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", 0.483866059556305461164527059736)); + + checkParameterValueDistribution(path, expected_distribution); +} + +TEST(ParameterValueDistribution, PoissonDistribution) +{ + using ament_index_cpp::get_package_share_directory; + using openscenario_interpreter::Double; + using openscenario_interpreter::make; + using openscenario_interpreter::String; + + std::string path = get_package_share_directory("openscenario_interpreter") + + "/test/parameter_value_distribution/Stochastic.PoissonDistribution.xosc"; + + ParameterDistribution expected_distribution; + expected_distribution.push_back(makeParameterListSharedPtr("offset", 0.0)); + expected_distribution.push_back(makeParameterListSharedPtr("offset", 0.0)); + expected_distribution.push_back(makeParameterListSharedPtr("offset", 0.0)); + + checkParameterValueDistribution(path, expected_distribution); +} + +TEST(ParameterValueDistribution, ProbabilityDistributionSet) +{ + using ament_index_cpp::get_package_share_directory; + using openscenario_interpreter::String; + + std::string path = + get_package_share_directory("openscenario_interpreter") + + "/test/parameter_value_distribution/Stochastic.ProbabilityDistributionSet.xosc"; + + ParameterDistribution expected_distribution; + expected_distribution.push_back(makeParameterListSharedPtr("offset", "1")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "2")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "1")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "2")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "3")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "2")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "1")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "2")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "1")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "1")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "2")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "3")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "2")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "2")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "2")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "3")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "1")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "2")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "3")); + expected_distribution.push_back(makeParameterListSharedPtr("offset", "2")); + + // NOTE: + // 1, ideal: 5/20, actual: 6/20 + // 2: ideal: 10/20, actual: 10/20 + // 3: ideal: 5/20, actual: 4/20 + + checkParameterValueDistribution(path, expected_distribution); +} + +TEST(ParameterValueDistribution, UniformDistribution) +{ + using ament_index_cpp::get_package_share_directory; + using openscenario_interpreter::Double; + + std::string path = get_package_share_directory("openscenario_interpreter") + + "/test/parameter_value_distribution/Stochastic.UniformDistribution.xosc"; + + ParameterDistribution expected_distribution; + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.736924424522478638266420603031)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.082699735953560393753036805720)); + expected_distribution.push_back( + makeParameterListSharedPtr("offset", -0.562081627575042097610946711939)); + + checkParameterValueDistribution(path, expected_distribution); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/openscenario/openscenario_preprocessor/CMakeLists.txt b/openscenario/openscenario_preprocessor/CMakeLists.txt index a910a63014f..9c6d69fbcaf 100644 --- a/openscenario/openscenario_preprocessor/CMakeLists.txt +++ b/openscenario/openscenario_preprocessor/CMakeLists.txt @@ -11,17 +11,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_auto REQUIRED) +find_package(XercesC REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}.cpp) +ament_auto_add_executable(${PROJECT_NAME}_node + src/${PROJECT_NAME}.cpp + src/${PROJECT_NAME}_node.cpp) -target_link_libraries(${PROJECT_NAME} glog) - -rclcpp_components_register_nodes(${PROJECT_NAME} "openscenario_preprocessor::Preprocessor") - -ament_auto_add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp) -target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}) +target_link_libraries(${PROJECT_NAME}_node XercesC::XercesC) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/openscenario_preprocessor.hpp b/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/openscenario_preprocessor.hpp index 59fb50905f9..5f6d1cf226f 100644 --- a/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/openscenario_preprocessor.hpp +++ b/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/openscenario_preprocessor.hpp @@ -15,60 +15,48 @@ #ifndef OPENSCENARIO_PREPROCESSOR__OPENSCENARIO_PREPROCESSOR_HPP_ #define OPENSCENARIO_PREPROCESSOR__OPENSCENARIO_PREPROCESSOR_HPP_ -#include -#include #include #include -#include -#include -#include -#include +#include +#include namespace openscenario_preprocessor { -struct ScenarioSet +struct Scenario { - ScenarioSet() = default; + Scenario() = default; - explicit ScenarioSet(openscenario_preprocessor_msgs::srv::Load::Request & load_request) + explicit Scenario(const boost::filesystem::path & path, double frame_rate) + : path(path), frame_rate(frame_rate) { - path = load_request.path; - frame_rate = load_request.frame_rate; } - auto getDeriveResponse() -> openscenario_preprocessor_msgs::srv::Derive::Response - { - openscenario_preprocessor_msgs::srv::Derive::Response response; - response.path = path; - response.frame_rate = frame_rate; - return response; - } - - std::string path; + boost::filesystem::path path; float frame_rate; }; -class Preprocessor : public rclcpp::Node +class Preprocessor { public: - explicit Preprocessor(const rclcpp::NodeOptions &); - -private: - void preprocessScenario(ScenarioSet &); - - [[nodiscard]] bool validateXOSC(const boost::filesystem::path &, bool); + explicit Preprocessor(const boost::filesystem::path & output_directory) + : validate(), output_directory(output_directory) + { + if (not boost::filesystem::exists(output_directory)) { + boost::filesystem::create_directories(output_directory); + } + } - rclcpp::Service::SharedPtr load_server; +protected: + void preprocessScenario(const Scenario &); - rclcpp::Service::SharedPtr derive_server; + std::queue preprocessed_scenarios; - rclcpp::Service::SharedPtr - check_server; + std::mutex preprocessed_scenarios_mutex; - std::deque preprocessed_scenarios; + openscenario_validator::OpenSCENARIOValidator validate; - std::mutex preprocessed_scenarios_mutex; + boost::filesystem::path output_directory; }; } // namespace openscenario_preprocessor diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 92ee715718b..6b56d90d785 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -1,6 +1,5 @@ - openscenario_preprocessor 1.17.0 @@ -10,10 +9,11 @@ ament_cmake_auto - libgoogle-glog-dev openscenario_interpreter openscenario_preprocessor_msgs + openscenario_validator rclcpp + xerces ament_cmake_clang_format ament_cmake_copyright diff --git a/openscenario/openscenario_preprocessor/src/openscenario_preprocessor.cpp b/openscenario/openscenario_preprocessor/src/openscenario_preprocessor.cpp index c5b45cedab8..79c478b6aa9 100644 --- a/openscenario/openscenario_preprocessor/src/openscenario_preprocessor.cpp +++ b/openscenario/openscenario_preprocessor/src/openscenario_preprocessor.cpp @@ -13,102 +13,75 @@ // limitations under the License. #include +#include +#include #include -#include +#include #include -#include namespace openscenario_preprocessor { -Preprocessor::Preprocessor(const rclcpp::NodeOptions & options) -: rclcpp::Node("openscenario_preprocessor", options), - load_server(create_service( - "~/load", - [this]( - const openscenario_preprocessor_msgs::srv::Load::Request::SharedPtr request, - openscenario_preprocessor_msgs::srv::Load::Response::SharedPtr response) -> void { - auto lock = std::lock_guard(preprocessed_scenarios_mutex); - try { - auto s = ScenarioSet(*request); - preprocessScenario(s); - response->has_succeeded = true; - response->message = "success"; - } catch (std::exception & e) { - response->has_succeeded = false; - response->message = e.what(); - preprocessed_scenarios.clear(); - } - })), - derive_server(create_service( - "~/derive", - [this]( - const openscenario_preprocessor_msgs::srv::Derive::Request::SharedPtr, - openscenario_preprocessor_msgs::srv::Derive::Response::SharedPtr response) -> void { - auto lock = std::lock_guard(preprocessed_scenarios_mutex); - if (preprocessed_scenarios.empty()) { - response->path = "no output"; - } else { - *response = preprocessed_scenarios.front().getDeriveResponse(); - preprocessed_scenarios.pop_front(); - } - })), - check_server(create_service( - "~/check", - [this]( - const openscenario_preprocessor_msgs::srv::CheckDerivativeRemained::Request::SharedPtr, - openscenario_preprocessor_msgs::srv::CheckDerivativeRemained::Response::SharedPtr response) - -> void { - auto lock = std::lock_guard(preprocessed_scenarios_mutex); - response->derivative_remained = not preprocessed_scenarios.empty(); - })) -{ -} - -bool Preprocessor::validateXOSC(const boost::filesystem::path & file_name, bool verbose = false) -{ - auto result = - concealer::dollar("ros2 run openscenario_utility validation.py " + file_name.string()); - if (verbose) { - std::cout << "validate : " << result << std::endl; - } - return result.find("All xosc files given are standard compliant.") != std::string::npos; -} - -void Preprocessor::preprocessScenario(ScenarioSet & scenario) +void Preprocessor::preprocessScenario(const Scenario & scenario) { using openscenario_interpreter::OpenScenario; using openscenario_interpreter::ParameterValueDistribution; + using openscenario_interpreter::ParameterValueDistributionDefinition; + + validate(scenario.path); + + if (OpenScenario script{scenario.path}; + script.category.is()) { + auto & parameter_value_distribution = script.category.as(); + auto scenario_file_path = parameter_value_distribution.scenario_file.filepath; - if (validateXOSC(scenario.path)) { - if (auto script = std::make_shared(scenario.path); - script->category.is()) { - std::cout << "ParameterValueDistribution!!" << std::endl; - auto base_scenario_path = - script->category.as().scenario_file.filepath; - std::cout << "base_scenario_path : " << base_scenario_path << std::endl; - if (boost::filesystem::exists(base_scenario_path)) { - if (validateXOSC(base_scenario_path, true)) { - // TODO : implement in feature/parameter_value_distribution branch - // - // parameters = evaluate( parameter_value_distribution( given scenario ) ) - // for( auto derived_scenario : embedParameter( linked scenario, parameters) - // preprocessed_scenarios.emplace_back({derived_scenario, derive_server}); - } else { - throw common::Error("base scenario is not valid : " + base_scenario_path.string()); + if (boost::filesystem::exists(scenario_file_path)) { + validate(scenario_file_path); + + OpenScenario scenario_file{scenario_file_path}; + + auto p = parameter_value_distribution.derive(); + + for (const auto & parameter_list : p | boost::adaptors::indexed()) { + pugi::xml_document derived_script; + + derived_script.reset(scenario_file.script); // deep copy + + auto parameter_declarations = + derived_script.document_element() + .select_node(pugi::xpath_query{"/OpenSCENARIO/ParameterDeclarations"}) + .node(); + + // embedding parameter values + for (const auto & [name, value] : *parameter_list.value()) { + if ( + auto parameter_node = + parameter_declarations.find_child_by_attribute("name", name.c_str())) { + parameter_node.attribute("value").set_value( + boost::lexical_cast(value).c_str()); + } else { + std::cerr << "Parameter " << std::quoted(name) << " is not declared in scenario " + << std::quoted(scenario_file_path.string()) << ", so ignore it." << std::endl; + } } - } else { - throw common::Error("base scenario does not exist : " + base_scenario_path.string()); - } - std::cout << "base scenario is valid!" << std::endl; + const auto derived_scenario_path = + output_directory / + (scenario.path.stem().string() + "." + std::to_string(parameter_list.index()) + + scenario.path.extension().string()); + + derived_script.save_file(derived_scenario_path.c_str()); + + preprocessed_scenarios.emplace(derived_scenario_path, scenario.frame_rate); + } } else { - // normal scenario - preprocessed_scenarios.emplace_back(scenario); + std::stringstream what; + what << "Scenario file " << std::quoted(scenario_file_path.string()) + << " described in ParameterDistributionDefinition file " + << std::quoted(scenario.path.string()) << " does not exist"; + throw std::runtime_error(what.str()); } } else { - throw common::Error("the scenario file is not valid. Please check your scenario"); + preprocessed_scenarios.push(scenario); // normal scenario } } } // namespace openscenario_preprocessor - -RCLCPP_COMPONENTS_REGISTER_NODE(openscenario_preprocessor::Preprocessor) diff --git a/openscenario/openscenario_preprocessor/src/openscenario_preprocessor_node.cpp b/openscenario/openscenario_preprocessor/src/openscenario_preprocessor_node.cpp index b85e871aab4..dd2f847fa95 100644 --- a/openscenario/openscenario_preprocessor/src/openscenario_preprocessor_node.cpp +++ b/openscenario/openscenario_preprocessor/src/openscenario_preprocessor_node.cpp @@ -12,24 +12,84 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include +#include +#include +#include +#include -int main(const int argc, char const * const * const argv) +class PreprocessorNode : public rclcpp::Node, public openscenario_preprocessor::Preprocessor { - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); +public: + explicit PreprocessorNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("openscenario_preprocessor", options), + openscenario_preprocessor::Preprocessor([this] { + declare_parameter("output_directory", "/tmp/openscenario_preprocessor"); + return get_parameter("output_directory").as_string(); + }()), + load_server(create_service( + "~/load", + [this]( + const openscenario_preprocessor_msgs::srv::Load::Request::SharedPtr request, + openscenario_preprocessor_msgs::srv::Load::Response::SharedPtr response) -> void { + auto lock = std::lock_guard(preprocessed_scenarios_mutex); + try { + preprocessScenario( + openscenario_preprocessor::Scenario(request->path, request->frame_rate)); + response->has_succeeded = true; + response->message = "success"; + } catch (std::exception & e) { + response->has_succeeded = false; + response->message = e.what(); + std::queue().swap(preprocessed_scenarios); + } + })), + derive_server(create_service( + "~/derive", + [this]( + const openscenario_preprocessor_msgs::srv::Derive::Request::SharedPtr, + openscenario_preprocessor_msgs::srv::Derive::Response::SharedPtr response) -> void { + auto lock = std::lock_guard(preprocessed_scenarios_mutex); + if (preprocessed_scenarios.empty()) { + response->path = "no output"; + } else { + response->path = preprocessed_scenarios.front().path.string(); + response->frame_rate = preprocessed_scenarios.front().frame_rate; + preprocessed_scenarios.pop(); + } + })), + check_server(create_service( + "~/check", + [this]( + const openscenario_preprocessor_msgs::srv::CheckDerivativeRemained::Request::SharedPtr, + openscenario_preprocessor_msgs::srv::CheckDerivativeRemained::Response::SharedPtr response) + -> void { + auto lock = std::lock_guard(preprocessed_scenarios_mutex); + response->derivative_remained = not preprocessed_scenarios.empty(); + })) + { + } + +private: + rclcpp::Service::SharedPtr load_server; + + rclcpp::Service::SharedPtr derive_server; + rclcpp::Service::SharedPtr + check_server; +}; + +int main(const int argc, char const * const * const argv) +{ rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor{}; rclcpp::NodeOptions options{}; - auto node = std::make_shared(options); + auto node = std::make_shared(options); executor.add_node((*node).get_node_base_interface()); diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 4da3715979e..49212eee37f 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -8,10 +8,13 @@ Apache License 2.0 ament_cmake ament_cmake_python + python3-numpy python3-xmlschema python3-yaml + rclcpp rclpy + ament_cmake