diff --git a/openscenario/openscenario_interpreter/CMakeLists.txt b/openscenario/openscenario_interpreter/CMakeLists.txt index 19df4b2251a..a1afbcc7105 100644 --- a/openscenario/openscenario_interpreter/CMakeLists.txt +++ b/openscenario/openscenario_interpreter/CMakeLists.txt @@ -11,6 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_auto REQUIRED) +find_package(Boost REQUIRED COMPONENTS filesystem) ament_auto_find_build_dependencies() @@ -97,6 +98,73 @@ install( DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/test DESTINATION share/${PROJECT_NAME}) +# ------------------------------------------------------------------------------ +# parameter value distribution without ROS +# ------------------------------------------------------------------------------ +add_library(parameter_value_distribution_without_ros STATIC + src/syntax/open_scenario.cpp + src/syntax/file_header.cpp + src/syntax/license.cpp + src/syntax/properties.cpp + src/syntax/property.cpp + src/syntax/file.cpp + src/syntax/open_scenario_category.cpp + src/syntax/parameter_value_distribution_definition.cpp + src/syntax/parameter_value_distribution.cpp + src/syntax/distribution_definition.cpp + src/syntax/deterministic.cpp + src/syntax/deterministic_parameter_distribution.cpp + src/syntax/deterministic_multi_parameter_distribution.cpp + src/syntax/deterministic_multi_parameter_distribution_type.cpp + src/syntax/value_set_distribution.cpp + src/syntax/deterministic_single_parameter_distribution.cpp + src/syntax/deterministic_single_parameter_distribution_type.cpp + src/syntax/distribution_range.cpp + src/syntax/range.cpp + src/syntax/distribution_set.cpp + src/syntax/distribution_set_element.cpp + src/syntax/stochastic.cpp + src/syntax/stochastic_distribution.cpp + src/syntax/stochastic_distribution_type.cpp + src/syntax/histogram.cpp + src/syntax/histogram_bin.cpp + src/syntax/normal_distribution.cpp + src/syntax/log_normal_distribution.cpp + src/syntax/poisson_distribution.cpp + src/syntax/probability_distribution_set.cpp + src/syntax/probability_distribution_set_element.cpp + src/syntax/uniform_distribution.cpp + + src/syntax/boolean.cpp + src/syntax/double.cpp + src/syntax/integer.cpp + src/syntax/unsigned_integer.cpp + src/syntax/unsigned_short.cpp + + src/syntax/parameter_value_set.cpp + + src/utility/demangle.cpp + + src/evaluate.cpp + src/object.cpp + src/parameter_distribution.cpp + src/scope.cpp) + +add_definitions("-DBOOST_ALLOW_DEPRECATED_HEADERS") +# cspell: ignore DPARAMETER +target_compile_definitions(parameter_value_distribution_without_ros PUBLIC -DPARAMETER_VALUE_DISTRIBUTION_ONLY) +target_include_directories(parameter_value_distribution_without_ros PUBLIC include) +target_include_directories(parameter_value_distribution_without_ros PUBLIC ${boost_json_SOURCE_DIR}/include) +target_include_directories(parameter_value_distribution_without_ros PUBLIC ${scenario_simulator_exception_INCLUDE_DIRS}) +target_link_libraries(parameter_value_distribution_without_ros + Boost::filesystem + Boost::json + pugixml) + +install( + TARGETS parameter_value_distribution_without_ros + LIBRARY DESTINATION lib/${PROJECT_NAME}) + # ------------------------------------------------------------------------------ # test # ------------------------------------------------------------------------------ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp index 945e1a346a3..d1d1578d7b7 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp @@ -15,9 +15,12 @@ #ifndef OPENSCENARIO_INTERPRETER__READER__ATTRIBUTE_HPP_ #define OPENSCENARIO_INTERPRETER__READER__ATTRIBUTE_HPP_ +#ifndef PARAMETER_VALUE_DISTRIBUTION_ONLY #include -#include #include +#endif // PARAMETER_VALUE_DISTRIBUTION_ONLY + +#include #include #include #include @@ -38,6 +41,7 @@ auto substitute(std::string attribute, Scope & scope) { auto dirname = [](auto &&, auto && scope) { return scope.dirname(); }; +#ifndef PARAMETER_VALUE_DISTRIBUTION_ONLY auto find_pkg_share = [](auto && package_name, auto &&) { return ament_index_cpp::get_package_share_directory(package_name); }; @@ -61,9 +65,11 @@ auto substitute(std::string attribute, Scope & scope) return result; } }; +#endif // PARAMETER_VALUE_DISTRIBUTION_ONLY auto var = [](auto && name, auto && scope) { - // TODO: Return the value of the launch configuration variable instead of the OpenSCENARIO parameter. + // TODO: Return the value of the launch configuration variable instead of the OpenSCENARIO + // parameter. if (const auto found = scope.ref(name); found) { return boost::lexical_cast(found); } else { @@ -76,14 +82,16 @@ auto substitute(std::string attribute, Scope & scope) std::string, std::function > substitutions{ {"dirname", dirname}, - // TODO {"env", env}, - // TODO {"eval", eval}, - // TODO {"exec-in-package", exec_in_package}, - // TODO {"find-exec", find_exec}, - // TODO {"find-pkg-prefix", find_pkg_prefix}, + // TODO {"env", env}, + // TODO {"eval", eval}, + // TODO {"exec-in-package", exec_in_package}, + // TODO {"find-exec", find_exec}, + // TODO {"find-pkg-prefix", find_pkg_prefix}, +#ifndef PARAMETER_VALUE_DISTRIBUTION_ONLY {"find-pkg-share", find_pkg_share}, {"ros2", ros2}, // NOTE: TIER IV extension (Not included in the ROS 2 Launch XML Substitution) +#endif // PARAMETER_VALUE_DISTRIBUTION_ONLY {"var", var}, }; @@ -136,7 +144,8 @@ auto readAttribute(const std::string & name, const Node & node, const Scope & sc } }; - // NOTE: https://www.asam.net/index.php?eID=dumpFile&t=f&f=4092&token=d3b6a55e911b22179e3c0895fe2caae8f5492467#_parameters + // NOTE: + // https://www.asam.net/index.php?eID=dumpFile&t=f&f=4092&token=d3b6a55e911b22179e3c0895fe2caae8f5492467#_parameters if (const auto & attribute = node.attribute(name.c_str())) { // NOTE: `substitute` is TIER IV extension (Non-OpenSCENARIO standard) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp index 14109565e6b..552221691be 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp @@ -22,7 +22,11 @@ #include #include #include + +#ifndef PARAMETER_VALUE_DISTRIBUTION_ONLY #include +#endif // PARAMETER_VALUE_DISTRIBUTION_ONLY + #include #include #include @@ -191,7 +195,9 @@ class Scope public: const std::string name; +#ifndef PARAMETER_VALUE_DISTRIBUTION_ONLY std::list actors; +#endif // PARAMETER_VALUE_DISTRIBUTION_ONLY // NOTE: `random_engine` is used only for sharing random number generator in Stochastic now std::default_random_engine random_engine; 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 868c9884526..682d6ea81de 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 @@ -19,7 +19,6 @@ #include #include #include -#include #include namespace openscenario_interpreter 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 6bae5544653..61b664b49fe 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 @@ -22,7 +22,6 @@ #include #include #include -#include #include namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/scope.cpp b/openscenario/openscenario_interpreter/src/scope.cpp index 8407b8898b8..c5d5ff8dc16 100644 --- a/openscenario/openscenario_interpreter/src/scope.cpp +++ b/openscenario/openscenario_interpreter/src/scope.cpp @@ -18,9 +18,12 @@ #include #include #include -#include #include +#ifndef PARAMETER_VALUE_DISTRIBUTION_ONLY +#include +#endif // PARAMETER_VALUE_DISTRIBUTION_ONLY + namespace openscenario_interpreter { EnvironmentFrame::EnvironmentFrame(EnvironmentFrame & outer_frame, const std::string & name) @@ -99,8 +102,11 @@ Scope::Scope(const std::string & name, const Scope & outer) : open_scenario(outer.open_scenario), frame(std::shared_ptr(new EnvironmentFrame(*outer.frame, name))), scenario_definition(outer.scenario_definition), - name(name), + name(name) +#ifndef PARAMETER_VALUE_DISTRIBUTION_ONLY + , actors(outer.actors) +#endif // PARAMETER_VALUE_DISTRIBUTION_ONLY { } 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 cce7e14edac..c2cb7d147ef 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 @@ -25,8 +25,8 @@ DeterministicSingleParameterDistributionType::DeterministicSingleParameterDistri : Group( 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);}))) + std::make_pair("DistributionRange", [&](auto && node){ return make(node, scope);}))) +// std::make_pair("UserDefinedDistribution", [&](auto && node){ return make(node, scope);}))) // clang-format on { } diff --git a/openscenario/openscenario_interpreter/src/syntax/open_scenario.cpp b/openscenario/openscenario_interpreter/src/syntax/open_scenario.cpp index 255fff1acc0..fa086d4f17c 100644 --- a/openscenario/openscenario_interpreter/src/syntax/open_scenario.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/open_scenario.cpp @@ -15,7 +15,10 @@ #include #include #include + +#ifndef PARAMETER_VALUE_DISTRIBUTION_ONLY #include +#endif // PARAMETER_VALUE_DISTRIBUTION_ONLY namespace openscenario_interpreter { @@ -48,6 +51,7 @@ auto operator<<(boost::json::object & json, const OpenScenario & datum) -> boost { json["version"] = "1.0"; +#ifndef PARAMETER_VALUE_DISTRIBUTION_ONLY json["frame"] = datum.frame; json["CurrentStates"].emplace_object(); @@ -63,6 +67,7 @@ auto operator<<(boost::json::object & json, const OpenScenario & datum) -> boost if (datum.category.is()) { json["OpenSCENARIO"].emplace_object() << datum.category.as(); } +#endif // PARAMETER_VALUE_DISTRIBUTION_ONLY return json; } diff --git a/openscenario/openscenario_interpreter/src/syntax/open_scenario_category.cpp b/openscenario/openscenario_interpreter/src/syntax/open_scenario_category.cpp index f9c083ea25d..e4af4dc7c4e 100644 --- a/openscenario/openscenario_interpreter/src/syntax/open_scenario_category.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/open_scenario_category.cpp @@ -12,10 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef PARAMETER_VALUE_DISTRIBUTION_ONLY #include +#include +#endif // PARAMETER_VALUE_DISTRIBUTION_ONLY + +#include #include #include -#include namespace openscenario_interpreter { @@ -25,8 +29,10 @@ OpenScenarioCategory::OpenScenarioCategory(const pugi::xml_node & tree, Scope & : Group( // clang-format off choice(tree, +#ifndef PARAMETER_VALUE_DISTRIBUTION_ONLY std::make_pair("Storyboard", [&](auto &&) { return make(tree, scope);}), // DIRTY HACK!!! std::make_pair("Catalog", [&](auto &&) { return make(tree, scope);}), +#endif // PARAMETER_VALUE_DISTRIBUTION_ONLY std::make_pair("ParameterValueDistribution",[&](auto &&) { return make(tree, scope);}))) // clang-format on { diff --git a/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution_type.cpp b/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution_type.cpp index 62f068c5289..3c33b2ff747 100644 --- a/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution_type.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/stochastic_distribution_type.cpp @@ -28,8 +28,8 @@ StochasticDistributionType::StochasticDistributionType(const pugi::xml_node & no 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);}), - std::make_pair("Histogram", [&](auto && node){return make(node, scope);}), - std::make_pair("UserDefinedDistribution", [&](auto && node){return make(node, scope);}))) + std::make_pair("Histogram", [&](auto && node){return make(node, scope);}))) +// std::make_pair("UserDefinedDistribution", [&](auto && node){return make(node, scope);}))) // clang-format on { } diff --git a/openscenario/openscenario_preprocessor/CMakeLists.txt b/openscenario/openscenario_preprocessor/CMakeLists.txt index bf2502786b6..cc3fdece02a 100644 --- a/openscenario/openscenario_preprocessor/CMakeLists.txt +++ b/openscenario/openscenario_preprocessor/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.16.3) # Ubuntu 20.04 default CMake version project(openscenario_preprocessor) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -11,16 +11,57 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_auto REQUIRED) +find_package(XercesC REQUIRED) +find_package(Boost REQUIRED COMPONENTS filesystem program_options regex) +find_package(pugixml REQUIRED) + ament_auto_find_build_dependencies() ament_auto_add_executable(${PROJECT_NAME}_node + src/schema.cpp src/${PROJECT_NAME}.cpp src/${PROJECT_NAME}_node.cpp) +target_link_libraries(${PROJECT_NAME}_node XercesC::XercesC) +target_link_libraries(${PROJECT_NAME}_node yaml-cpp) + +find_library(parameter_value_distribution_LIBRARIES parameter_value_distribution_without_ros) + +file(READ ${openscenario_validator_DIR}/../../openscenario_validator/schema/OpenSCENARIO-1.3.xsd ${PROJECT_NAME}_OPENSCENARIO_1_3_XSD) +configure_file(configure/schema.cpp src/schema.cpp) + +add_executable(${PROJECT_NAME}_command + src/t4v2.cpp + src/schema.cpp + src/${PROJECT_NAME}.cpp + src/${PROJECT_NAME}_command.cpp) + +# cspell: ignore DPARAMETER +target_compile_definitions(${PROJECT_NAME}_command PUBLIC -DPARAMETER_VALUE_DISTRIBUTION_ONLY) + +target_include_directories(${PROJECT_NAME}_command PUBLIC include) +target_include_directories(${PROJECT_NAME}_command PUBLIC ${XercesC_INCLUDE_DIRS}) +target_include_directories(${PROJECT_NAME}_command PUBLIC ${YAML_CPP_INCLUDE_DIRS}) +target_include_directories(${PROJECT_NAME}_command PUBLIC ${YAML_CPP_INCLUDE_DIRS}) +target_include_directories(${PROJECT_NAME}_command PUBLIC ${openscenario_interpreter_INCLUDE_DIRS}) +target_include_directories(${PROJECT_NAME}_command PUBLIC ${openscenario_validator_INCLUDE_DIRS}) + +target_link_libraries(${PROJECT_NAME}_command Boost::filesystem Boost::program_options Boost::regex) +target_link_libraries(${PROJECT_NAME}_command XercesC::XercesC) +target_link_libraries(${PROJECT_NAME}_command pugixml) +target_link_libraries(${PROJECT_NAME}_command yaml-cpp) + +target_link_libraries(${PROJECT_NAME}_command ${parameter_value_distribution_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_command + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + ament_add_pytest_test(test_${PROJECT_NAME} "test/test_main.py") endif() ament_auto_package() diff --git a/openscenario/openscenario_preprocessor/configure/schema.cpp b/openscenario/openscenario_preprocessor/configure/schema.cpp new file mode 100644 index 00000000000..10ea52a163b --- /dev/null +++ b/openscenario/openscenario_preprocessor/configure/schema.cpp @@ -0,0 +1,20 @@ +// 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_preprocessor +{ +const char schema[] = R"###(${${PROJECT_NAME}_OPENSCENARIO_1_3_XSD})###"; +} // namespace openscenario_preprocessor diff --git a/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/deriver.hpp b/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/deriver.hpp new file mode 100644 index 00000000000..8fd360309c5 --- /dev/null +++ b/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/deriver.hpp @@ -0,0 +1,77 @@ +// 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_PREPROCESSOR__DERIVER_HPP_ +#define OPENSCENARIO_PREPROCESSOR__DERIVER_HPP_ + +#include +#include +#include +#include +#include + +namespace openscenario_preprocessor +{ + +class Deriver +{ +public: + explicit Deriver(std::string schema_path) : validate(schema_path) {} + auto operator()(boost::filesystem::path path, bool check_scenario_path = true) + -> openscenario_interpreter::ParameterDistribution + { + using openscenario_interpreter::OpenScenario; + using openscenario_interpreter::ParameterValueDistribution; + using openscenario_interpreter::ParameterValueDistributionDefinition; + + validate(path); + + if (OpenScenario script{path}; script.category.is()) { + auto & parameter_value_distribution = script.category.as(); + scenario_file_path = parameter_value_distribution.scenario_file.filepath; + + if (not check_scenario_path || boost::filesystem::exists(scenario_file_path)) { + if (check_scenario_path) { + validate(scenario_file_path); + } + + scenario_file_doc.load_file(scenario_file_path.c_str()); + + return parameter_value_distribution.derive(); + } else { + std::stringstream what; + what << "Scenario file " << std::quoted(scenario_file_path.string()) + << " described in ParameterDistributionDefinition file " << std::quoted(path.string()) + << " does not exist"; + throw std::runtime_error(what.str()); + } + } else { + return {}; + } + } + + auto get_doc() -> pugi::xml_document & { return scenario_file_doc; } + + auto get_scenario_path() -> boost::filesystem::path { return scenario_file_path; } + +private: + pugi::xml_document scenario_file_doc; + + openscenario_validator::OpenSCENARIOValidator validate; + + boost::filesystem::path scenario_file_path; +}; +} // namespace openscenario_preprocessor + +#endif //OPENSCENARIO_PREPROCESSOR__DERIVER_HPP_ diff --git a/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/openscenario_preprocessor.hpp b/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/openscenario_preprocessor.hpp index 5f6d1cf226f..6a7ddd3e81a 100644 --- a/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/openscenario_preprocessor.hpp +++ b/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/openscenario_preprocessor.hpp @@ -15,8 +15,12 @@ #ifndef OPENSCENARIO_PREPROCESSOR__OPENSCENARIO_PREPROCESSOR_HPP_ #define OPENSCENARIO_PREPROCESSOR__OPENSCENARIO_PREPROCESSOR_HPP_ +#include + #include #include +#include +#include #include #include @@ -36,25 +40,50 @@ struct Scenario float frame_rate; }; +enum class ScenarioFormat { + t4v2, + xosc, +}; + +std::istream & operator>>(std::istream & is, ScenarioFormat & format); + class Preprocessor { public: explicit Preprocessor(const boost::filesystem::path & output_directory) - : validate(), output_directory(output_directory) + : output_directory(output_directory), derive([]() -> std::string { + auto file = std::ofstream("/tmp/openscenario_preprocessor/schema.xsd", std::ios::trunc); + file << openscenario_preprocessor::schema; + file.close(); + return "/tmp/openscenario_preprocessor/schema.xsd"; + }()) { if (not boost::filesystem::exists(output_directory)) { boost::filesystem::create_directories(output_directory); } } -protected: - void preprocessScenario(const Scenario &); + void preprocessScenario( + const boost::filesystem::path & scenario_path, + ScenarioFormat output_format = ScenarioFormat::xosc); + + void generateDerivedScenarioFromDistribution( + openscenario_interpreter::ParameterDistribution & distribution, + const boost::filesystem::path & path, ScenarioFormat output_format); - std::queue preprocessed_scenarios; + void convertXMLtoYAML(const pugi::xml_node & xml, YAML::Emitter & emitter); + + const std::queue & getPreprocessedScenarios() const + { + return preprocessed_scenarios; + } + +protected: + std::queue preprocessed_scenarios; std::mutex preprocessed_scenarios_mutex; - openscenario_validator::OpenSCENARIOValidator validate; + Deriver derive; boost::filesystem::path output_directory; }; diff --git a/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/schema.hpp b/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/schema.hpp new file mode 100644 index 00000000000..5ef5ea7c39a --- /dev/null +++ b/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/schema.hpp @@ -0,0 +1,18 @@ +// 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. + +namespace openscenario_preprocessor +{ +extern const char schema[]; +} // namespace openscenario_preprocessor diff --git a/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/t4v2.hpp b/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/t4v2.hpp new file mode 100644 index 00000000000..859a540987c --- /dev/null +++ b/openscenario/openscenario_preprocessor/include/openscenario_preprocessor/t4v2.hpp @@ -0,0 +1,45 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPENSCENARIO_PREPROCESSOR_T4V2_HPP +#define OPENSCENARIO_PREPROCESSOR_T4V2_HPP + +#include +#include +#include + +namespace openscenario_preprocessor +{ +class T4V2 +{ +public: + auto deriveScenarioWithScenarioModifiers( + const pugi::xml_document & base_scenario_doc, + const openscenario_interpreter::ParameterDistribution & scenario_modifier_distribution) + -> std::vector; + + auto deriveToXoscStringScenarios( + boost::filesystem::path modifiers_path, boost::filesystem::path scenario_path = {}) + -> std::vector; + + auto generateParameterValueDistributionFromScenarioModifiers(std::string scenario_modifiers_str) + -> pugi::xml_document; + + auto loadScenarioFile(boost::filesystem::path path) -> pugi::xml_document; + + std::pair splitScenarioModifiers( + boost::filesystem::path scenario_path); +}; +} // namespace openscenario_preprocessor +#endif //OPENSCENARIO_PREPROCESSOR_T4V2_HPP diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 65e87998cdf..c1688860b56 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -13,6 +13,9 @@ openscenario_preprocessor_msgs openscenario_validator rclcpp + pugixml-dev + yaml-cpp + nlohmann-json-dev ament_cmake_clang_format ament_cmake_copyright @@ -20,6 +23,9 @@ ament_cmake_pep257 ament_cmake_xmllint ament_lint_auto + ament_cmake_pytest + openscenario_utility + ament_index_python ament_cmake diff --git a/openscenario/openscenario_preprocessor/src/openscenario_preprocessor.cpp b/openscenario/openscenario_preprocessor/src/openscenario_preprocessor.cpp index 9fd9d390d07..234a8672e68 100644 --- a/openscenario/openscenario_preprocessor/src/openscenario_preprocessor.cpp +++ b/openscenario/openscenario_preprocessor/src/openscenario_preprocessor.cpp @@ -18,69 +18,166 @@ #include #include #include +#include namespace openscenario_preprocessor { -void Preprocessor::preprocessScenario(const Scenario & scenario) +void Preprocessor::preprocessScenario( + const boost::filesystem::path & scenario_path, ScenarioFormat output_format) { 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; + auto distribution = derive(scenario_path); + if (distribution.empty()) { + preprocessed_scenarios.push(scenario_path); // normal scenario + } else { + auto base_scenario_path = derive.get_scenario_path(); + generateDerivedScenarioFromDistribution(distribution, base_scenario_path, output_format); + } +} - if (boost::filesystem::exists(scenario_file_path)) { - validate(scenario_file_path); +void Preprocessor::generateDerivedScenarioFromDistribution( + openscenario_interpreter::ParameterDistribution & distribution, + const boost::filesystem::path & path, ScenarioFormat output_format) +{ + for (const auto & parameter_list : distribution | boost::adaptors::indexed()) { + pugi::xml_document derived_script; - OpenScenario scenario_file{scenario_file_path}; + derived_script.reset(derive.get_doc()); // deep copy - for (const auto & parameter_list : - parameter_value_distribution.derive() | boost::adaptors::indexed()) { - pugi::xml_document derived_script; + auto parameter_declarations = + derived_script.document_element() + .select_node(pugi::xpath_query{"/OpenSCENARIO/ParameterDeclarations"}) + .node(); - derived_script.reset(scenario_file.script); // deep copy + // 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(derive.get_scenario_path().string()) << ", so ignore it." + << std::endl; + } + } - auto parameter_declarations = - derived_script.document_element() - .select_node(pugi::xpath_query{"/OpenSCENARIO/ParameterDeclarations"}) - .node(); + const auto derived_scenario_path_xosc = + output_directory / (path.stem().string() + "." + std::to_string(parameter_list.index()) + + path.extension().string()); - // 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; - } - } + auto derived_scenario_path = [&]() { + if (output_format == ScenarioFormat::t4v2) { + YAML::Emitter yaml_emitter; + convertXMLtoYAML(derived_script, yaml_emitter); - const auto derived_scenario_path = + const auto derived_scenario_path_t4v2 = output_directory / - (scenario.path.stem().string() + "." + std::to_string(parameter_list.index()) + - scenario.path.extension().string()); + (path.stem().string() + "." + std::to_string(parameter_list.index()) + ".yaml"); - derived_script.save_file(derived_scenario_path.c_str()); + std::ofstream derived_scenario_yaml{derived_scenario_path_t4v2}; + derived_scenario_yaml << yaml_emitter.c_str(); + derived_scenario_yaml.close(); - preprocessed_scenarios.emplace(derived_scenario_path, scenario.frame_rate); + // NOTE: for debug, write out yaml to + // "/tmp/openscenario_preprocessor/preprocessed_scenario.yaml" + std::ofstream debug_yaml{"/tmp/openscenario_preprocessor/preprocessed_scenario.yaml"}; + debug_yaml << yaml_emitter.c_str(); + debug_yaml.close(); + + return derived_scenario_path_t4v2; + } else { + derived_script.save_file(derived_scenario_path_xosc.c_str()); + return derived_scenario_path_xosc; } + }(); + preprocessed_scenarios.emplace(derived_scenario_path); + } +} + +void Preprocessor::convertXMLtoYAML(const pugi::xml_node & xml, YAML::Emitter & emitter) +{ + if (xml.attributes().empty() && xml.children().empty()) { + emitter << ""; + return; + } + + // the map of node name and {total count, used count} + std::map> count; + for (const auto & child : xml.children()) { + if (count.find(child.name()) == count.end()) { + count[child.name()] = std::make_pair(1, 0); } else { - 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()); + count[child.name()].first++; } + } + + // iterate attributes + if (not xml.attributes().empty()) { + emitter << YAML::BeginMap; + + for (const auto & attr : xml.attributes()) { + emitter << YAML::Key << attr.name(); + emitter << YAML::Value << YAML::SingleQuoted << attr.as_string(); + } + if (xml.children().empty()) { + emitter << YAML::EndMap; + } + } + + // iterate child elements + if (not xml.children().empty()) { + if (xml.attributes().empty()) { + emitter << YAML::BeginMap; + } + for (const auto & child : xml.children()) { + // total count == 1, make single map in yaml + if (count[child.name()].first == 1) { + emitter << YAML::Key << child.name(); + emitter << YAML::Value; + convertXMLtoYAML(child, emitter); + } else { + // total count > 1, make sequence in yaml + if (count[child.name()].second == 0) { + // first node + emitter << YAML::Key << child.name(); + emitter << YAML::Value; + emitter << YAML::BeginSeq; + convertXMLtoYAML(child, emitter); + // count up used count + count[child.name()].second++; + } else if (count[child.name()].second == count[child.name()].first - 1) { + // last node + convertXMLtoYAML(child, emitter); + emitter << YAML::EndSeq; + } else { + // middle node + convertXMLtoYAML(child, emitter); + count[child.name()].second++; + } + } + } + emitter << YAML::EndMap; + } +} + +std::istream & operator>>(std::istream & is, ScenarioFormat & format) +{ + std::string token; + is >> token; + if (token == "t4v2") { + format = ScenarioFormat::t4v2; + } else if (token == "xosc") { + format = ScenarioFormat::xosc; } else { - preprocessed_scenarios.push(scenario); // normal scenario + std::stringstream what; + what << token << " is invalid scenario format. Please specify t4v2 or xosc as scenario format"; + throw std::runtime_error(what.str()); } + return is; } } // namespace openscenario_preprocessor diff --git a/openscenario/openscenario_preprocessor/src/openscenario_preprocessor_command.cpp b/openscenario/openscenario_preprocessor/src/openscenario_preprocessor_command.cpp new file mode 100644 index 00000000000..83b00b0b183 --- /dev/null +++ b/openscenario/openscenario_preprocessor/src/openscenario_preprocessor_command.cpp @@ -0,0 +1,230 @@ +// 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 + +const std::string_view template_scenario = R"###( + + + + + + + + + + + + + +)###"; + +auto create_parameter_value_distribution_from_json( + const boost::filesystem::path & scenario_path, const nlohmann::json & json_parameters) +{ + pugi::xml_document script; + script.load_string(template_scenario.data()); + + script.document_element() + .select_node(pugi::xpath_query{"/OpenSCENARIO/ParameterValueDistribution/ScenarioFile"}) + .node() + .attribute("filepath") + .set_value(scenario_path.c_str()); + + auto value_set_node = + script.document_element() + .select_node(pugi::xpath_query{ + "/OpenSCENARIO/ParameterValueDistribution/Deterministic/" + "DeterministicMultiParameterDistribution/ValueSetDistribution/ParameterValueSet"}) + .node(); + + for (auto const & json_parameter : json_parameters.items()) { + auto parameter_assignment_node = value_set_node.append_child("ParameterAssignment"); + parameter_assignment_node.append_attribute("parameterRef") = json_parameter.key().c_str(); + parameter_assignment_node.append_attribute("value") = + json_parameter.value().get().c_str(); + } + return script; +} + +int main(const int argc, char const * const * const argv) +try { + using namespace boost::program_options; + + options_description description("openscenario_preprocessor_command"); + + // cspell: ignore multitoken + description.add_options()( + "output-directory,o", + value()->default_value("/tmp/openscenario_preprocessor/derived"), + "path of output directory")( + "format,f", value()->multitoken(), + "output scenario format (t4v2 / xosc)")( + "parameters,p", value()->default_value("null"), "parameters in json format")( + "scenario,s", value(), "path of scenario file")("skip-full-derivation", "")( + "help,H", "help"); + + variables_map vm; + store(parse_command_line(argc, argv, description), vm); + notify(vm); + + auto output_directory_option = boost::filesystem::path(vm["output-directory"].as()); + auto format_option = vm["format"].as(); + auto parameters_option = boost::filesystem::path(vm["parameters"].as()); + auto scenario_option = boost::filesystem::path(vm["scenario"].as()); + bool skip_full_derivation_option = (vm.count("skip-full-derivation") > 0); + + auto scenario_path = boost::filesystem::path(scenario_option); + + boost::filesystem::path tmp_output_directory = "/tmp/openscenario_preprocessor"; + if (not boost::filesystem::exists(tmp_output_directory)) { + boost::filesystem::create_directories(tmp_output_directory); + } + + std::vector xosc_scenario_paths; + + boost::filesystem::path scenario_modifiers_path{}; + + // preprocess t4v2 format and convert to xosc scenarios + if (scenario_path.extension() == ".yaml" or scenario_path.extension() == ".yml") { + openscenario_preprocessor::T4V2 t4v2; + + auto [modifiers_path, base_scenario_path] = t4v2.splitScenarioModifiers(scenario_path); + scenario_modifiers_path = modifiers_path; + + auto xosc_string_scenarios = [&]() { + if (skip_full_derivation_option) { + return t4v2.deriveToXoscStringScenarios(base_scenario_path); + } else { + return t4v2.deriveToXoscStringScenarios(base_scenario_path, modifiers_path); + } + }(); + + boost::filesystem::path t4v2_output_directory = "/tmp/openscenario_preprocessor/t4v2_derived"; + if (not boost::filesystem::exists(t4v2_output_directory)) { + boost::filesystem::create_directories(t4v2_output_directory); + } + for (const auto & xosc_string_scenario : xosc_string_scenarios | boost::adaptors::indexed()) { + boost::filesystem::path xosc_scenario_path = + t4v2_output_directory / (std::to_string(xosc_string_scenario.index()) + ".xosc"); + std::ofstream ofs(xosc_scenario_path.c_str()); + ofs << xosc_string_scenario.value().c_str(); + ofs.close(); + xosc_scenario_paths.push_back(xosc_scenario_path); + } + } else { + xosc_scenario_paths.push_back(scenario_path); + } + + // derive for given parameters + if (parameters_option != "null") { + for (auto scenario_path : xosc_scenario_paths) { + auto parameters_json = nlohmann::json::parse(parameters_option.c_str()); + // convert value to string, because current implementation is not support other types + for (auto & json_item : parameters_json.items()) { + if (not json_item.value().is_string()) { + try { + json_item.value() = boost::lexical_cast(json_item.value()); + } catch (std::exception & e) { + std::stringstream what; + what << "Cannot convert parameter value, type " << json_item.value().type_name() + << " to string : " << e.what(); + throw std::runtime_error(what.str()); + } + } + } + + auto parameter_value_distribution = + create_parameter_value_distribution_from_json(scenario_path, parameters_json); + + parameter_value_distribution.save_file( + "/tmp/openscenario_preprocessor/parameter_value_distribution.xosc"); + + openscenario_preprocessor::Preprocessor preprocessor(output_directory_option); + + preprocessor.preprocessScenario( + "/tmp/openscenario_preprocessor/parameter_value_distribution.xosc", format_option); + + // merge scenario modifiers if skip_full_derivation_option is ON + if ( + format_option == openscenario_preprocessor::ScenarioFormat::t4v2 && + skip_full_derivation_option) { + auto derived_scenario_paths = preprocessor.getPreprocessedScenarios(); + while (not derived_scenario_paths.empty()) { + // 1. copy scenario content to stringstream + std::stringstream scenario_ss; + { + std::ifstream input_scenario_file{derived_scenario_paths.front().string()}; + if (not input_scenario_file) { + std::stringstream what; + what << "Cannot open scenario file : " << derived_scenario_paths.front().string(); + throw std::runtime_error(what.str()); + } else { + std::cout << "opened scenario file : " << derived_scenario_paths.front().string() + << std::endl; + } + scenario_ss << input_scenario_file.rdbuf(); + input_scenario_file.close(); + } + + // 2. write scenario modifiers to scenario file + std::cout << "2. write scenario modifiers to scenario file" << std::endl; + std::stringstream merged_scenario_ss; + + // load scenario modifiers + { + std::ifstream modifiers_file{scenario_modifiers_path.string()}; + if (not modifiers_file) { + std::stringstream what; + what << "Cannot open scenario modifiers file : " << scenario_modifiers_path.string(); + throw std::runtime_error(what.str()); + } + + merged_scenario_ss << modifiers_file.rdbuf(); + modifiers_file.close(); + } + + // load scenario content + { + merged_scenario_ss << scenario_ss.str(); + } + + // write-out merged scenario + { + std::ofstream scenario_file(derived_scenario_paths.front().string()); + if (not scenario_file) { + std::stringstream what; + what << "Cannot open scenario file : " << derived_scenario_paths.front().string(); + throw std::runtime_error(what.str()); + } + scenario_file << merged_scenario_ss.str(); + scenario_file.close(); + } + + derived_scenario_paths.pop(); + } + } + } + } else { + throw std::runtime_error("parameters option is required"); + } + return 0; +} catch (std::exception & e) { + std::cerr << "Caught an exception : " << e.what() << std::endl; + return 1; +} diff --git a/openscenario/openscenario_preprocessor/src/openscenario_preprocessor_node.cpp b/openscenario/openscenario_preprocessor/src/openscenario_preprocessor_node.cpp index dd2f847fa95..5b5118d256d 100644 --- a/openscenario/openscenario_preprocessor/src/openscenario_preprocessor_node.cpp +++ b/openscenario/openscenario_preprocessor/src/openscenario_preprocessor_node.cpp @@ -36,14 +36,13 @@ class PreprocessorNode : public rclcpp::Node, public openscenario_preprocessor:: 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)); + preprocessScenario(request->path); response->has_succeeded = true; response->message = "success"; } catch (std::exception & e) { response->has_succeeded = false; response->message = e.what(); - std::queue().swap(preprocessed_scenarios); + // std::queue().swap(preprocessed_scenarios); } })), derive_server(create_service( @@ -55,8 +54,8 @@ class PreprocessorNode : public rclcpp::Node, public openscenario_preprocessor:: if (preprocessed_scenarios.empty()) { response->path = "no output"; } else { - response->path = preprocessed_scenarios.front().path.string(); - response->frame_rate = preprocessed_scenarios.front().frame_rate; + response->path = preprocessed_scenarios.front().string(); + response->frame_rate = 30.0; preprocessed_scenarios.pop(); } })), diff --git a/openscenario/openscenario_preprocessor/src/t4v2.cpp b/openscenario/openscenario_preprocessor/src/t4v2.cpp new file mode 100644 index 00000000000..f3414a1fcff --- /dev/null +++ b/openscenario/openscenario_preprocessor/src/t4v2.cpp @@ -0,0 +1,379 @@ +// 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 + +namespace openscenario_preprocessor +{ + +// cspell: ignore isinstance etree + +const std::string_view scenario_modifiers_distribution_base = R"###( + + + + + + + +)###"; + +const std::string_view load_yaml_to_xosc_with_encode_python_script = R"###( +import xmlschema +import yaml +import os +import sys +import xml.etree.ElementTree as ET + + +def from_yaml(keyword, node): + + if isinstance(node, dict): + # + # ???: { ... } + # + result = {} + + for tag, value in node.items(): + + if isinstance(value, list) and len(value) == 0: + # + # Tag: [] + # + # => REMOVE + # + continue + + if str.islower(tag[0]): + # + # tag: { ... } + # + # => @tag: { ... } + # + result["@" + tag] = str(value) + else: + # + # Tag: { ... } + # + # => NO CHANGES + # + result[tag] = from_yaml(tag, value) + + return result + + elif isinstance(node, list): + # + # ???: [ ... ] + # + result = [] + + for index, item in enumerate(node): + result.append(from_yaml(keyword, item)) + + return result + + elif isinstance(node, str): + return node + + else: + return None + + +if __name__ == "__main__": + # parse arguments here (input yaml path, output xosc path) + input_yaml_path = sys.argv[1] + output_xosc_path = sys.argv[2] + + xsd = open("/tmp/openscenario_preprocessor/schema.xsd") + schema = xmlschema.XMLSchema(xsd) + + if os.path.exists(input_yaml_path): + with open(input_yaml_path, "r") as file: + openscenario_yaml = from_yaml("OpenSCENARIO", yaml.safe_load(file)) + openscenario_yaml.pop("ScenarioModifiers", None) + xosc, errors = schema.encode( + openscenario_yaml, + indent=2, + preserve_root=True, + unordered=True, # Reorder elements + # The "strict" mode is too strict than we would like. + validation="lax", + ) + xosc = xmlschema.XMLResource(xosc).tostring().replace("True", "true").replace("False", "false") + with open(output_xosc_path, "w") as file: + file.write(xosc) +)###"; + +auto T4V2::deriveToXoscStringScenarios( + boost::filesystem::path scenario_path, boost::filesystem::path modifiers_path) + -> std::vector +{ + auto openscenario_doc = loadScenarioFile(scenario_path.string()); + + std::vector derived_scenarios; + if (not modifiers_path.empty()) { + std::fstream file{modifiers_path.string()}; + std::stringstream modifiers_ss; + modifiers_ss << file.rdbuf(); + file.close(); + auto parameter_value_distribution = + generateParameterValueDistributionFromScenarioModifiers(modifiers_ss.str()); + boost::filesystem::path parameter_value_distribution_path = + "/tmp/openscenario_preprocessor/scenario_modifiers_distribution.xosc"; + parameter_value_distribution.save_file(parameter_value_distribution_path.string().c_str()); + + Deriver derive("/tmp/openscenario_preprocessor/schema.xsd"); + auto distribution = derive(parameter_value_distribution_path, false); + + if (not distribution.empty()) { + auto derived_scenario_docs = + deriveScenarioWithScenarioModifiers(openscenario_doc, distribution); + for (auto & derived_scenario_doc : derived_scenario_docs) { + std::stringstream derived_scenario_stream; + derived_scenario_doc.save(derived_scenario_stream); + derived_scenarios.push_back(derived_scenario_stream.str()); + } + } else { + // no scenario modifiers + std::stringstream scenario_stream; + openscenario_doc.save(scenario_stream); + derived_scenarios.push_back(scenario_stream.str()); + } + } else { + // no scenario modifiers + std::stringstream scenario_stream; + openscenario_doc.save(scenario_stream); + derived_scenarios.push_back(scenario_stream.str()); + } + return derived_scenarios; +} + +std::pair T4V2::splitScenarioModifiers( + boost::filesystem::path scenario_path) +{ + std::fstream file{scenario_path.string()}; + std::stringstream scenario_ss; + scenario_ss << file.rdbuf(); + file.close(); + + auto scenario_string = scenario_ss.str(); + + ; + if (auto modifiers_pos = scenario_string.find("ScenarioModifiers"); + modifiers_pos != std::string::npos) { + auto openscenario_pos = scenario_string.find("OpenSCENARIO"); + + if (openscenario_pos == std::string::npos) { + throw std::runtime_error( + "No OpenSCENARIO element found in TIER IV 2.0 Format Scenario. Please check your " + "scenario."); + } + + std::ofstream modifiers_ofs("/tmp/openscenario_preprocessor/t4v2_modifiers.yaml"); + std::ofstream base_scenario_ofs("/tmp/openscenario_preprocessor/t4v2_openscenario.yaml"); + + if (openscenario_pos > modifiers_pos) { + std::regex re("(.*\n|^)(OpenSCENARIO.*)"); + std::smatch match; + std::regex_search(scenario_string, match, re); + modifiers_ofs << scenario_string.substr(0, match.position(2)); + base_scenario_ofs << scenario_string.substr(match.position(2)); + } else { + std::regex re("(.*\n|^)(ScenarioModifiers.*)"); + std::smatch match; + std::regex_search(scenario_string, match, re); + base_scenario_ofs << scenario_string.substr(0, match.position(2)); + modifiers_ofs << scenario_string.substr(match.position(2)); + } + modifiers_ofs.close(); + base_scenario_ofs.close(); + return std::make_pair( + "/tmp/openscenario_preprocessor/t4v2_modifiers.yaml", + "/tmp/openscenario_preprocessor/t4v2_openscenario.yaml"); + } else { + std::ofstream base_scenario_ofs("/tmp/openscenario_preprocessor/t4v2_openscenario.yaml"); + base_scenario_ofs << scenario_string; + base_scenario_ofs.close(); + return std::make_pair( + "", "/tmp/openscenario_preprocessor/t4v2_openscenario.yaml"); + } +} + +auto T4V2::generateParameterValueDistributionFromScenarioModifiers( + std::string scenario_modifiers_str) -> pugi::xml_document +{ + pugi::xml_document doc; + doc.load_string(scenario_modifiers_distribution_base.data()); + auto deterministic = + doc.select_node(pugi::xpath_query{"OpenSCENARIO/ParameterValueDistribution/Deterministic"}) + .node(); + + auto scenario_modifiers = + YAML::Load(scenario_modifiers_str)["ScenarioModifiers"]["ScenarioModifier"]; + for (const auto & scenario_modifier : scenario_modifiers) { + auto distribution = deterministic.append_child("DeterministicSingleParameterDistribution"); + + struct + { + bool list_enabled = false; + std::string name; + double start; + double step; + double stop; + std::vector list; + } data; + + for (auto modifier_element : scenario_modifier) { + auto key = modifier_element.first.as(); + + if (key == "name") { + data.name = modifier_element.second.as(); + } else if (key == "list") { + data.list_enabled = true; + for (const auto & value : modifier_element.second.as>()) { + data.list.push_back(value); + } + } else if (key == "start") { + data.start = modifier_element.second.as(); + } else if (key == "step") { + data.step = modifier_element.second.as(); + } else if (key == "stop") { + data.stop = modifier_element.second.as(); + } else { + std::cout << "unknown key: " << key << std::endl; + } + } + + distribution.append_attribute("parameterName") = data.name.c_str(); + + if (data.list_enabled) { + // add distribution set + auto distribution_set = distribution.append_child("DistributionSet"); + for (const auto & value : data.list) { + auto distribution_set_element = distribution_set.append_child("Element"); + distribution_set_element.append_attribute("value") = value.c_str(); + } + } else { + // add distribution range + auto range = distribution.append_child("DistributionRange"); + range.append_attribute("stepWidth") = data.step; + auto range_range = range.append_child("Range"); + range_range.append_attribute("upperLimit") = data.stop; + range_range.append_attribute("lowerLimit") = data.start; + } + } + return doc; +} + +auto T4V2::deriveScenarioWithScenarioModifiers( + const pugi::xml_document & base_scenario_doc, + const openscenario_interpreter::ParameterDistribution & scenario_modifier_distribution) + -> std::vector +{ + std::vector derived_scripts; + + struct replace_walker : pugi::xml_tree_walker + { + openscenario_interpreter::ParameterSetSharedPtr parameter_set; + + virtual bool for_each(pugi::xml_node & node) + { + for (pugi::xml_attribute_iterator attribute_iter = node.attributes_begin(); + attribute_iter != node.attributes_end(); ++attribute_iter) { + for (const auto & parameter : *parameter_set) { + attribute_iter->set_value(std::regex_replace( + attribute_iter->as_string(), std::regex(parameter.first), + parameter.second.as()) + .c_str()); + } + } + return true; + } + }; + + replace_walker walker; + + for (const auto & parameter_set : scenario_modifier_distribution | boost::adaptors::indexed()) { + pugi::xml_document derived_script; + + derived_script.reset(base_scenario_doc); // deep copy + + // overwrite with scenario modifiers + walker.parameter_set = parameter_set.value(); + derived_script.traverse(walker); + + derived_scripts.push_back(std::forward(derived_script)); + } + + return derived_scripts; +} + +//template +//void convertYAMLtoXML(const YAML::Node & yaml, XMLClass & xml) +//{ +// switch (yaml.Type()) { +// case YAML::NodeType::Scalar: +// break; +// case YAML::NodeType::Sequence: +// break; +// case YAML::NodeType::Map: +// for (const auto & element : yaml) { +// auto key = element.first.as(); +// if (element.second.IsScalar()) { +// xml.append_attribute(key.c_str()).set_value(element.second.as().c_str()); +// } else if (element.second.IsSequence()) { +// for (const auto & sequence_element : element.second) { +// auto child = xml.append_child(key.c_str()); +// convertYAMLtoXML(sequence_element, child); +// } +// } else { +// auto child = xml.append_child(key.c_str()); +// convertYAMLtoXML(element.second, child); +// } +// } +// break; +// case YAML::NodeType::Null: +// break; +// default: +// throw std::runtime_error("Unknown YAML node type"); +// } +//} + +auto T4V2::loadScenarioFile(boost::filesystem::path path) -> pugi::xml_document +{ + std::string script_path = "/tmp/openscenario_preprocessor/load_yaml_to_xosc_with_encode.py"; + std::ofstream ofs(script_path); + ofs << load_yaml_to_xosc_with_encode_python_script; + ofs.close(); + + std::stringstream command_ss; + command_ss << "python3 " << script_path << " " << path.string() + << " /tmp/openscenario_preprocessor/normalized.xosc"; + + if (system(command_ss.str().c_str()) != 0) { + throw std::runtime_error("failed to execute python script : " + command_ss.str()); + } + pugi::xml_document doc; + doc.load_file("/tmp/openscenario_preprocessor/normalized.xosc"); + return doc; +} +} // namespace openscenario_preprocessor diff --git a/openscenario/openscenario_preprocessor/test/test_main.py b/openscenario/openscenario_preprocessor/test/test_main.py new file mode 100644 index 00000000000..7aecfd6142d --- /dev/null +++ b/openscenario/openscenario_preprocessor/test/test_main.py @@ -0,0 +1,60 @@ +# 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. + +from time import sleep + +import pytest +import os +from openscenario_utility.conversion import convert +from ament_index_python.packages import get_package_share_directory +from pathlib import Path +import difflib + + +@pytest.mark.skip +def test(scenario_name, desired_diff_num): + # share_dir = get_package_share_directory("openscenario_preprocessor") + preprocessor_share_dir = get_package_share_directory("openscenario_preprocessor") + test_runner_share_dir = preprocessor_share_dir + "/../../../scenario_test_runner/share/scenario_test_runner" + # sample_scenario_path = Path(share_dir + "/test/scenarios/" + scenario_name + ".yaml") + sample_scenario_path = Path(test_runner_share_dir + "/scenario/" + scenario_name + ".yaml") + output_dir = Path("/tmp/openscenario_preprocessor/test") + os.makedirs(output_dir, exist_ok=True) + convert(sample_scenario_path, output_dir) + input_xosc = "/tmp/openscenario_preprocessor/test/" + scenario_name + "_0.xosc" + + command_path = preprocessor_share_dir + "/../../lib/openscenario_preprocessor/openscenario_preprocessor_command" + preprocessor_command = command_path + " -s " + str(sample_scenario_path) + " -o /tmp/openscenario_preprocessor/test" \ + + " --parameters '{\"random_offset\": true}'" + " -f t4v2" + " --skip-full-derivation" + print(preprocessor_command) + os.system(preprocessor_command) + sleep(1) + + file1 = open(input_xosc) + file2 = open("/tmp/openscenario_preprocessor/normalized.xosc") + diff = difflib.Differ() + output_diff = diff.compare(file1.readlines(), file2.readlines()) + diff_num = 0 + for line in output_diff: + if line[0:1] in ['+', '-']: + diff_num += 1 + assert diff_num == desired_diff_num + + +def test_sample(): + test("sample", 2) + + +def test_autoware_simple(): + test("autoware-simple", 2) diff --git a/openscenario/openscenario_validator/CMakeLists.txt b/openscenario/openscenario_validator/CMakeLists.txt index 75e8f4eab1d..12084f2ad06 100644 --- a/openscenario/openscenario_validator/CMakeLists.txt +++ b/openscenario/openscenario_validator/CMakeLists.txt @@ -15,7 +15,7 @@ target_include_directories(validate PUBLIC $) ament_export_include_directories(include) -ament_export_dependencies(XercesC ament_index_cpp) +ament_export_dependencies(XercesC) install(DIRECTORY include/ DESTINATION include) diff --git a/openscenario/openscenario_validator/include/openscenario_validator/validator.hpp b/openscenario/openscenario_validator/include/openscenario_validator/validator.hpp index 8d197cb292b..0a685933bde 100644 --- a/openscenario/openscenario_validator/include/openscenario_validator/validator.hpp +++ b/openscenario/openscenario_validator/include/openscenario_validator/validator.hpp @@ -15,7 +15,6 @@ #ifndef OPENSCENARIO_VALIDATOR__VALIDATOR_HPP_ #define OPENSCENARIO_VALIDATOR__VALIDATOR_HPP_ -#include #include #include #include @@ -74,17 +73,15 @@ class OpenSCENARIOValidator static inline XMLPlatformLifecycleHandler xml_platform_lifecycle_handler; public: - OpenSCENARIOValidator() : parser(std::make_unique()) + explicit OpenSCENARIOValidator(std::string schema_path) + : parser(std::make_unique()) { parser->setDoNamespaces(true); parser->setDoSchema(true); parser->setErrorHandler(&error_handler); parser->setValidationSchemaFullChecking(true); parser->setValidationScheme(xercesc::XercesDOMParser::Val_Always); - parser->setExternalNoNamespaceSchemaLocation( - (ament_index_cpp::get_package_share_directory("openscenario_validator") + - "/schema/OpenSCENARIO-1.3.xsd") - .c_str()); + parser->setExternalNoNamespaceSchemaLocation(schema_path.c_str()); } auto validate(const boost::filesystem::path & xml_file) -> void diff --git a/openscenario/openscenario_validator/src/validator_command.cpp b/openscenario/openscenario_validator/src/validator_command.cpp index 147de8d6387..de2e35cc27e 100644 --- a/openscenario/openscenario_validator/src/validator_command.cpp +++ b/openscenario/openscenario_validator/src/validator_command.cpp @@ -12,13 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include int main(int argc, char * argv[]) { assert(argc == 2); std::string file_path = argv[1]; - openscenario_validator::OpenSCENARIOValidator validate; + std::string schema_path = ament_index_cpp::get_package_share_directory("openscenario_validator") + + "/schema/OpenSCENARIO-1.3.xsd"; + openscenario_validator::OpenSCENARIOValidator validate(schema_path); try { validate(file_path); return 0;