diff --git a/.gitignore b/.gitignore index fc6f72e2355847d30f07b61dddbd8fcd335bc342..a31023a573a4fd9eb4f6e8a44785d036d3137f25 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,7 @@ __pycache__ generator.log *.orig *.bkp -.devcontainer \ No newline at end of file +.devcontainer +compile_commands.json +build +deps diff --git a/engine/CMakeLists.txt b/engine/CMakeLists.txt index 2e937ef779581ffd71c7eb0d2398719dcb566dcc..c54e100df079dba194e8da4a1acd2c0ac84ba246 100644 --- a/engine/CMakeLists.txt +++ b/engine/CMakeLists.txt @@ -48,6 +48,8 @@ find_package(Stochastics REQUIRED) # see https://stackoverflow.com/a/58495612 set(CMAKE_INSTALL_RPATH $ORIGIN) +find_package(Stochastics REQUIRED CONFIG) + add_library(${PROJECT_NAME} SHARED "") target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_17) @@ -70,12 +72,12 @@ target_include_directories( $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}/include> ) -target_link_libraries(${PROJECT_NAME} PUBLIC openscenario_api::shared - antlr4_runtime::shared - Yase::agnostic_behavior_tree - units::units +target_link_libraries(${PROJECT_NAME} PUBLIC antlr4_runtime::shared MantleAPI::MantleAPI - PRIVATE Stochastics::Stochastics) + openscenario_api::shared + Stochastics::Stochastics + units::units + Yase::agnostic_behavior_tree) if(USE_CCACHE) find_program(CCACHE_FOUND ccache) @@ -205,6 +207,7 @@ target_sources( ${CMAKE_CURRENT_LIST_DIR}/tests/Storyboard/GenericAction/EnvironmentActionTest.cpp ${CMAKE_CURRENT_LIST_DIR}/tests/Storyboard/GenericAction/LightStateActionTests.cpp ${CMAKE_CURRENT_LIST_DIR}/tests/Storyboard/GenericAction/TeleportActionTest.cpp + ${CMAKE_CURRENT_LIST_DIR}/tests/Storyboard/GenericAction/TrafficAreaActionTest.cpp ${CMAKE_CURRENT_LIST_DIR}/tests/Storyboard/GenericAction/TrafficSignalStateActionTest.cpp ${CMAKE_CURRENT_LIST_DIR}/tests/Storyboard/GenericAction/TrafficSinkActionTest.cpp ${CMAKE_CURRENT_LIST_DIR}/tests/Storyboard/GenericAction/TrafficSwarmActionTest.cpp diff --git a/engine/bazel/deps.bzl b/engine/bazel/deps.bzl new file mode 100644 index 0000000000000000000000000000000000000000..cc30aa0182755091841423be732f260d460b9097 --- /dev/null +++ b/engine/bazel/deps.bzl @@ -0,0 +1,46 @@ +load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") +load("@bazel_tools//tools/build_defs/repo:utils.bzl", "maybe") + +ECLIPSE_GITLAB = "https://gitlab.eclipse.org/eclipse" + +MANTLE_API_TAG = "v6.0.0" +UNITS_TAG = "2.3.3" +YASE_TAG = "v0.0.1" + +MANTLE_API_TAG = "15bf75e4e3238afa5f1aa85c4054f37804375453" + +def osc_engine_deps(): + """Load dependencies""" + maybe( + http_archive, + name = "mantle_api", + url = ECLIPSE_GITLAB + "/openpass/mantle-api/-/archive/{tag}/mantle-api-{tag}.tar.gz".format(tag = MANTLE_API_TAG), + sha256 = "9f898c45b0a0541a553e6d1a7721215d76516daf9ad70658aab40d669bd2364d", + strip_prefix = "mantle-api-{tag}".format(tag = MANTLE_API_TAG), + type = "tar.gz", + ) + + maybe( + http_archive, + name = "units_nhh", + url = "https://github.com/nholthaus/units/archive/refs/tags/v{tag}.tar.gz".format(tag = UNITS_TAG), + sha256 = "b1f3c1dd11afa2710a179563845ce79f13ebf0c8c090d6aa68465b18bd8bd5fc", + strip_prefix = "./units-{tag}".format(tag = UNITS_TAG), + build_file = "//bazel:units.BUILD", + ) + + maybe( + http_archive, + name = "yase", + url = ECLIPSE_GITLAB + "/openpass/yase/-/archive/{tag}/yase-{tag}.tar.gz".format(tag = YASE_TAG), + sha256 = "243d710172dff4c4e7ab95e5bd68a6d8335cf8a3fdd1efa87d118250e3b85ee3", + strip_prefix = "yase-{tag}".format(tag = YASE_TAG), + ) + + http_archive( + name = "open_scenario_parser", + build_file = "//bazel:openscenario_api.BUILD", + url = "https://github.com/RA-Consulting-GmbH/openscenario.api.test/releases/download/v1.4.0/OpenSCENARIO_API_LinuxSharedRelease_2024.11.18.tgz", + sha256 = "7a4cdb82ccaaeed5fccf12efd94cf4f9c9d3ac0fc7d7feedd4c0babe2404f853", + strip_prefix = "OpenSCENARIO_API_LinuxSharedRelease", + ) diff --git a/engine/cmake/generated_files.cmake b/engine/cmake/generated_files.cmake index a1d1a4030c1e87582072b5c494c56f3bba8ef6da..fe7d985531d2aca935fd459567fe85a51f8c1b48 100644 --- a/engine/cmake/generated_files.cmake +++ b/engine/cmake/generated_files.cmake @@ -133,7 +133,6 @@ list(APPEND ${PROJECT_NAME}_SOURCES gen/Storyboard/GenericAction/RandomRouteAction_impl.cpp gen/Storyboard/GenericAction/SetMonitorAction_impl.cpp gen/Storyboard/GenericAction/SpeedProfileAction_impl.cpp - gen/Storyboard/GenericAction/TrafficAreaAction_impl.cpp gen/Storyboard/GenericAction/TrafficSignalControllerAction_impl.cpp gen/Storyboard/GenericAction/TrafficSourceAction_impl.cpp gen/Storyboard/GenericAction/TrafficStopAction_impl.cpp @@ -163,7 +162,9 @@ list(APPEND ${PROJECT_NAME}_SOURCES src/Conversion/OscToMantle/ConvertScenarioSpeedActionTarget.cpp src/Conversion/OscToMantle/ConvertScenarioTimeReference.cpp src/Conversion/OscToMantle/ConvertScenarioTimeToCollisionConditionTarget.cpp + src/Conversion/OscToMantle/ConvertScenarioTrafficArea.cpp src/Conversion/OscToMantle/ConvertScenarioTrafficDefinition.cpp + src/Conversion/OscToMantle/ConvertScenarioTrafficDistribution.cpp src/Conversion/OscToMantle/ConvertScenarioTrafficSignalController.cpp src/Conversion/OscToMantle/ConvertScenarioTrajectoryRef.cpp src/Conversion/OscToMantle/ConvertScenarioTransitionDynamics.cpp @@ -211,13 +212,13 @@ list(APPEND ${PROJECT_NAME}_SOURCES src/Storyboard/ByValueCondition/UserDefinedValueCondition_impl.cpp src/Storyboard/GenericAction/AcquirePositionAction_impl.cpp src/Storyboard/GenericAction/ActivateControllerAction.cpp - src/Storyboard/GenericAction/ActivateControllerAction_impl.cpp src/Storyboard/GenericAction/AssignControllerAction_impl.cpp src/Storyboard/GenericAction/AssignRouteAction_impl.cpp src/Storyboard/GenericAction/CustomCommandAction_impl.cpp src/Storyboard/GenericAction/EnvironmentAction_impl.cpp - src/Storyboard/GenericAction/LightStateAction_impl.cpp + src/Storyboard/GenericAction/LightStateAction.cpp src/Storyboard/GenericAction/TeleportAction_impl.cpp + src/Storyboard/GenericAction/TrafficAreaAction.cpp src/Storyboard/GenericAction/TrafficSignalStateAction_impl.cpp src/Storyboard/GenericAction/TrafficSinkAction_base.h src/Storyboard/GenericAction/TrafficSinkAction_impl.cpp @@ -231,6 +232,15 @@ list(APPEND ${PROJECT_NAME}_SOURCES src/Storyboard/MotionControlAction/LateralDistanceAction_impl.cpp src/Storyboard/MotionControlAction/LongitudinalDistanceAction_impl.cpp src/Storyboard/MotionControlAction/SpeedAction_impl.cpp + src/Utils/Spawning/Interval.cpp + src/Utils/Spawning/Length.cpp + src/Utils/Spawning/Obstacle.cpp + src/Utils/Spawning/SpawnSpace.cpp + src/Utils/Spawning/SpawnZone.cpp + src/Utils/Spawning/ToEntity.cpp + src/Utils/Spawning/VelocityRange.cpp + src/Utils/Spawning/Weight.cpp + src/Utils/Spawning/Weighted.cpp src/Utils/ConditionEdgeEvaluator.cpp src/Utils/ControllerCreator.cpp src/Utils/ControllerService.cpp @@ -458,9 +468,6 @@ list(APPEND ${PROJECT_NAME}_HEADERS gen/Storyboard/GenericAction/TeleportAction.h gen/Storyboard/GenericAction/TeleportAction_base.h gen/Storyboard/GenericAction/TeleportAction_impl.h - gen/Storyboard/GenericAction/TrafficAreaAction.h - gen/Storyboard/GenericAction/TrafficAreaAction_base.h - gen/Storyboard/GenericAction/TrafficAreaAction_impl.h gen/Storyboard/GenericAction/TrafficSignalControllerAction.h gen/Storyboard/GenericAction/TrafficSignalControllerAction_base.h gen/Storyboard/GenericAction/TrafficSignalControllerAction_impl.h @@ -573,12 +580,9 @@ list(APPEND ${PROJECT_NAME}_HEADERS src/Storyboard/ByEntityCondition/TraveledDistanceCondition_impl.h src/Storyboard/ByValueCondition/UserDefinedValueCondition_impl.h src/Storyboard/GenericAction/ActivateControllerAction.h - src/Storyboard/GenericAction/ActivateControllerAction_base.h - src/Storyboard/GenericAction/ActivateControllerAction_impl.h src/Storyboard/GenericAction/CustomCommandAction_impl.h src/Storyboard/GenericAction/LightStateAction.h - src/Storyboard/GenericAction/LightStateAction_base.h - src/Storyboard/GenericAction/LightStateAction_impl.h + src/Storyboard/GenericAction/TrafficAreaAction.h src/Storyboard/GenericAction/TrafficSignalAction.h src/Storyboard/GenericAction/TrafficSwarmAction.h src/Storyboard/GenericAction/TrafficSwarmAction_base.h @@ -592,6 +596,12 @@ list(APPEND ${PROJECT_NAME}_HEADERS src/Storyboard/MotionControlAction/LongitudinalDistanceAction_base.h src/Storyboard/MotionControlAction/MotionControlAction.h src/Storyboard/MotionControlAction/SpeedAction_impl.h + src/Utils/Spawning/Common.h + src/Utils/Spawning/Interval.h + src/Utils/Spawning/Length.h + src/Utils/Spawning/SpawnZone.h + src/Utils/Spawning/Transform.h + src/Utils/Spawning/Weight.h src/Utils/ConditionEdgeEvaluator.h src/Utils/Constants.h src/Utils/ControllerCreator.h diff --git a/engine/gen/Storyboard/GenericAction/TrafficAreaAction.h b/engine/gen/Storyboard/GenericAction/TrafficAreaAction.h deleted file mode 100644 index 0a92698db452b6b413bedfa6e421066307886398..0000000000000000000000000000000000000000 --- a/engine/gen/Storyboard/GenericAction/TrafficAreaAction.h +++ /dev/null @@ -1,61 +0,0 @@ -/******************************************************************************** - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) - * - * This program and the accompanying materials are made available under the - * terms of the Eclipse Public License 2.0 which is available at - * http://www.eclipse.org/legal/epl-2.0. - * - * SPDX-License-Identifier: EPL-2.0 - ********************************************************************************/ - -#pragma once - -#include <MantleAPI/Execution/i_environment.h> -#include <agnostic_behavior_tree/action_node.h> -#include <openScenarioLib/generated/v1_3/api/ApiClassInterfacesV1_3.h> - -#include <cassert> -#include <utility> - -#include "Storyboard/GenericAction/TrafficAreaAction_impl.h" - -namespace OpenScenarioEngine::v1_3::Node -{ -class TrafficAreaAction : public yase::ActionNode -{ -public: - TrafficAreaAction(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrafficAreaAction> trafficAreaAction) - : yase::ActionNode{"TrafficAreaAction"}, - trafficAreaAction_{trafficAreaAction} - { - } - - void onInit() override{}; - -private: - yase::NodeStatus tick() override - { - assert(impl_); - const auto is_finished = impl_->Step(); - return is_finished ? yase::NodeStatus::kSuccess : yase::NodeStatus::kRunning; - }; - - void lookupAndRegisterData(yase::Blackboard& blackboard) final - { - std::shared_ptr<mantle_api::IEnvironment> environment = blackboard.get<std::shared_ptr<mantle_api::IEnvironment>>("Environment"); - - impl_ = std::make_unique<OpenScenarioEngine::v1_3::TrafficAreaAction>( - OpenScenarioEngine::v1_3::TrafficAreaAction::Values{ - trafficAreaAction_->GetContinuous(), - trafficAreaAction_->GetNumberOfEntities(), - ConvertScenarioTrafficDistribution(trafficAreaAction_->GetTrafficDistribution()), - ConvertScenarioTrafficArea(trafficAreaAction_->GetTrafficArea())}, - OpenScenarioEngine::v1_3::TrafficAreaAction::Interfaces{ - environment}); - } - - std::unique_ptr<OpenScenarioEngine::v1_3::TrafficAreaAction> impl_{nullptr}; - std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrafficAreaAction> trafficAreaAction_; -}; - -} // namespace OpenScenarioEngine::v1_3::Node diff --git a/engine/gen/Storyboard/GenericAction/TrafficAreaAction_base.h b/engine/gen/Storyboard/GenericAction/TrafficAreaAction_base.h deleted file mode 100644 index 4768c8e8ab473fc5a39fac997ffa1f86820fa89f..0000000000000000000000000000000000000000 --- a/engine/gen/Storyboard/GenericAction/TrafficAreaAction_base.h +++ /dev/null @@ -1,47 +0,0 @@ -/******************************************************************************** - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) - * - * This program and the accompanying materials are made available under the - * terms of the Eclipse Public License 2.0 which is available at - * http://www.eclipse.org/legal/epl-2.0. - * - * SPDX-License-Identifier: EPL-2.0 - ********************************************************************************/ - -#pragma once - -#include <MantleAPI/Execution/i_environment.h> - -#include "Conversion/OscToMantle/ConvertScenarioTrafficArea.h" -#include "Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h" - -namespace OpenScenarioEngine::v1_3 -{ - -class TrafficAreaActionBase -{ -public: - struct Values - { - bool continuous; - unsigned int numberOfEntities; - TrafficDistribution trafficDistribution; - TrafficArea trafficArea; - }; - struct Interfaces - { - std::shared_ptr<mantle_api::IEnvironment> environment; - }; - - TrafficAreaActionBase(Values values, Interfaces interfaces) - : values{std::move(values)}, - mantle{std::move(interfaces)} {}; - virtual ~TrafficAreaActionBase() = default; - virtual bool Step() = 0; - -protected: - Values values; - Interfaces mantle; -}; - -} // namespace OpenScenarioEngine::v1_3 \ No newline at end of file diff --git a/engine/gen/Storyboard/GenericAction/TrafficAreaAction_impl.cpp b/engine/gen/Storyboard/GenericAction/TrafficAreaAction_impl.cpp deleted file mode 100644 index 2c7b6a75d4fc578e667497f4f1133a5a0201761a..0000000000000000000000000000000000000000 --- a/engine/gen/Storyboard/GenericAction/TrafficAreaAction_impl.cpp +++ /dev/null @@ -1,27 +0,0 @@ -/******************************************************************************** - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) - * - * This program and the accompanying materials are made available under the - * terms of the Eclipse Public License 2.0 which is available at - * http://www.eclipse.org/legal/epl-2.0. - * - * SPDX-License-Identifier: EPL-2.0 - ********************************************************************************/ - -#include "Storyboard/GenericAction/TrafficAreaAction_impl.h" - -#include "Utils/Logger.h" - -namespace OpenScenarioEngine::v1_3 -{ - -bool TrafficAreaAction::Step() -{ - // Note: - // - Access to values parse to mantle/ose datatypes: this->values.xxx - // - Access to mantle interfaces: this->mantle.xxx - Logger::Error("Method TrafficAreaAction::Step() not implemented yet (returning \"true\" by default)"); - return true; -} - -} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/gen/Storyboard/GenericAction/TrafficAreaAction_impl.h b/engine/gen/Storyboard/GenericAction/TrafficAreaAction_impl.h deleted file mode 100644 index 60371a16a64ceea4ad9e252ac8d0a0fe857d7892..0000000000000000000000000000000000000000 --- a/engine/gen/Storyboard/GenericAction/TrafficAreaAction_impl.h +++ /dev/null @@ -1,27 +0,0 @@ -/******************************************************************************** - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) - * - * This program and the accompanying materials are made available under the - * terms of the Eclipse Public License 2.0 which is available at - * http://www.eclipse.org/legal/epl-2.0. - * - * SPDX-License-Identifier: EPL-2.0 - ********************************************************************************/ - -#pragma once - -#include "Storyboard/GenericAction/TrafficAreaAction_base.h" - -namespace OpenScenarioEngine::v1_3 -{ - -class TrafficAreaAction : public TrafficAreaActionBase -{ -public: - TrafficAreaAction(Values values, Interfaces interfaces) - : TrafficAreaActionBase{std::move(values), std::move(interfaces)} {}; - - bool Step() override; -}; - -} // namespace OpenScenarioEngine::v1_3 \ No newline at end of file diff --git a/engine/gen/Storyboard/GenericAction/TrafficSourceAction.h b/engine/gen/Storyboard/GenericAction/TrafficSourceAction.h index 2a582a7ac88973d56fd5e97447f9ad14467778cb..dfbcfdf489a4ba5cab03b7e0d2e083e2f169ab50 100644 --- a/engine/gen/Storyboard/GenericAction/TrafficSourceAction.h +++ b/engine/gen/Storyboard/GenericAction/TrafficSourceAction.h @@ -55,9 +55,9 @@ private: [=]() { return ConvertScenarioPosition(environment, trafficSourceAction_->GetPosition()); }, ConvertScenarioTrafficDefinition(trafficSourceAction_->GetTrafficDefinition()), - ConvertScenarioTrafficDistribution(trafficSourceAction_->GetTrafficDistribution())}, - OpenScenarioEngine::v1_3::TrafficSourceAction::Interfaces{ - environment}); + ConvertScenarioTrafficDistribution(trafficSourceAction_->GetTrafficDistribution()) // + }, + OpenScenarioEngine::v1_3::TrafficSourceAction::Interfaces{environment}); } std::unique_ptr<OpenScenarioEngine::v1_3::TrafficSourceAction> impl_{nullptr}; diff --git a/engine/gen/Storyboard/GenericAction/TrafficSourceAction_base.h b/engine/gen/Storyboard/GenericAction/TrafficSourceAction_base.h index afffc0b23721d6e7c6eefbdecbe7821739d761ad..45bce20616e6dd8f29c74ac82d91fd1bcc2005fb 100644 --- a/engine/gen/Storyboard/GenericAction/TrafficSourceAction_base.h +++ b/engine/gen/Storyboard/GenericAction/TrafficSourceAction_base.h @@ -32,7 +32,7 @@ public: double velocity; std::function<std::optional<mantle_api::Pose>()> GetPosition; TrafficDefinition trafficDefinition; - TrafficDistribution trafficDistribution; + TrafficDistributions trafficDistributions; }; struct Interfaces { diff --git a/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficArea.cpp b/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficArea.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1c13fcfdcaa57973c74be51189c899137c7dd419 --- /dev/null +++ b/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficArea.cpp @@ -0,0 +1,53 @@ + +/******************************************************************************** + * Copyright (c) 2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#include "ConvertScenarioTrafficArea.h" + +#include "Utils/Spawning/Transform.h" + +namespace OpenScenarioEngine::v1_3 +{ +struct ToLaneRestriction +{ + mantle_api::RoadCursor::LaneRestriction operator()(const NET_ASAM_OPENSCENARIO::v1_3::ILane& lane) const + { + return {lane.GetId()}; + } +}; + +struct ToRoadCursor +{ + mantle_api::RoadCursor operator()(const NET_ASAM_OPENSCENARIO::v1_3::IRoadCursor& cursor) const + { + return {units::length::meter_t{cursor.GetS()}, cursor.GetRoadId(), TransformSharedPointers(cursor.GetLane(), ToLaneRestriction{})}; + } +}; + +struct ToRoadRange +{ + mantle_api::RoadRange operator()(const NET_ASAM_OPENSCENARIO::v1_3::IRoadRange& range) const + { + // An IRoadRange always has a length, but a mantle_api::RoadRange can have a std::nullopt length. Thus, if + // the IRoadRange has a length 0 despite having more than one cursor, treat that length as representing std::nullopt: + return {(range.GetRoadCursorSize() > 1 && range.GetLength() == .0) ? std::nullopt : std::optional<units::length::meter_t>{range.GetLength()}, + TransformSharedPointers(range.GetRoadCursor(), ToRoadCursor{})}; + } +}; + +TrafficArea ConvertScenarioTrafficArea(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrafficArea>& trafficArea) +{ + if (!trafficArea->GetRoadRange().empty()) + { + return TransformSharedPointers(trafficArea->GetRoadRange(), ToRoadRange{}); + } + throw std::runtime_error("ConvertScenarioTrafficArea: Conversion of polygon not yet supported"); +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficArea.h b/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficArea.h index 6e615fd808ac97b57c372cead9084ba0ebc3b36a..cbc3b3be284004a412844ba731f8f0c33cdb894d 100644 --- a/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficArea.h +++ b/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficArea.h @@ -11,20 +11,15 @@ #pragma once +#include <MantleAPI/Traffic/road_range.h> #include <openScenarioLib/generated/v1_3/api/ApiClassInterfacesV1_3.h> #include <memory> -#include <string> +#include <vector> namespace OpenScenarioEngine::v1_3 { -struct TrafficArea -{ -}; - -inline TrafficArea ConvertScenarioTrafficArea(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrafficArea>& /*trafficArea*/) -{ - return {}; -} +using TrafficArea = std::vector<mantle_api::RoadRange>; -} // namespace OpenScenarioEngine::v1_3 \ No newline at end of file +TrafficArea ConvertScenarioTrafficArea(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrafficArea>&); +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficDistribution.cpp b/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficDistribution.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8f2cdb17781bf6201a6a2cf358a918b3e6d948fa --- /dev/null +++ b/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficDistribution.cpp @@ -0,0 +1,47 @@ +/******************************************************************************** + * Copyright (c) 2024-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#include "ConvertScenarioTrafficDistribution.h" + +#include "Utils/Spawning/Transform.h" +#include "Utils/Spawning/Weight.h" + +namespace OpenScenarioEngine::v1_3 +{ +struct ToEntities +{ + std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IEntityObject> operator()(const NET_ASAM_OPENSCENARIO::v1_3::IEntityDistributionEntry& entry) const + { + return entry.GetScenarioObjectTemplate()->GetEntitiyObject(); // https://github.com/RA-Consulting-GmbH/openscenario.api.test/issues/222 + } + + std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IEntityObject>> operator()(const NET_ASAM_OPENSCENARIO::v1_3::ITrafficDistributionEntry& entry) const + { + return {TransformSharedPointers(entry.GetEntityDistribution()->GetEntityDistributionEntry(), *this)}; + } +}; + +struct ToTrafficDistribution +{ + TrafficDistribution operator()(const NET_ASAM_OPENSCENARIO::v1_3::ITrafficDistributionEntry& entry) const + { + return {TransformSharedPointers(entry.GetEntityDistribution()->GetEntityDistributionEntry(), [](const NET_ASAM_OPENSCENARIO::v1_3::IEntityDistributionEntry& entry) + { return entry.GetScenarioObjectTemplate(); }), // https://github.com/RA-Consulting-GmbH/openscenario.api.test/issues/222 + entry.GetWeight()}; + } +}; + +TrafficDistributions ConvertScenarioTrafficDistribution(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrafficDistribution>& distribution) +{ + decltype(auto) entries{distribution->GetTrafficDistributionEntry()}; + return {TransformSharedPointers(entries, ToTrafficDistribution{}), + TransformSharedPointers(entries, ToAccumulatedWeight{})}; +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h b/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h index e3c7abcfcc654ff1a9cd4cd70f4e1de1d3710ff2..2149edf16c85f328a4040de970be97d6eb937133 100644 --- a/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h +++ b/engine/src/Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h @@ -1,6 +1,5 @@ - /******************************************************************************** - * Copyright (c) 2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * Copyright (c) 2024-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) * * This program and the accompanying materials are made available under the * terms of the Eclipse Public License 2.0 which is available at @@ -12,19 +11,24 @@ #pragma once #include <openScenarioLib/generated/v1_3/api/ApiClassInterfacesV1_3.h> +#include <units.h> #include <memory> -#include <string> +#include <vector> + +#include "Utils/Spawning/Iterable.h" namespace OpenScenarioEngine::v1_3 { -struct TrafficDistribution +struct TrafficDistribution : Iterable<std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate>>> { + units::dimensionless::scalar_t weight; }; -inline TrafficDistribution ConvertScenarioTrafficDistribution(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrafficDistribution>& /*trafficDistribution*/) +struct TrafficDistributions : Iterable<std::vector<TrafficDistribution>> { - return {}; -} + std::vector<units::dimensionless::scalar_t> weights; +}; -} // namespace OpenScenarioEngine::v1_3 \ No newline at end of file +TrafficDistributions ConvertScenarioTrafficDistribution(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrafficDistribution>&); +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/Action.h b/engine/src/Storyboard/GenericAction/Action.h new file mode 100644 index 0000000000000000000000000000000000000000..d97f656d8ad841b7ab3437dbe414fb9aa3231c3d --- /dev/null +++ b/engine/src/Storyboard/GenericAction/Action.h @@ -0,0 +1,119 @@ +/******************************************************************************** + * Copyright (c) 2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include <agnostic_behavior_tree/action_node.h> + +#include <string> +#include <string_view> + +namespace OpenScenarioEngine::v1_3 +{ +template <typename> +struct Interfaces +{ + std::shared_ptr<mantle_api::IEnvironment> environment; +}; + +template <typename> +struct Values +{ +}; + +template <typename Instance> // CRTP +class Action +{ +public: + Action(Values<Instance> values, Interfaces<Instance> interfaces) + : values{std::move(values)}, + mantle{std::move(interfaces)} {}; + +protected: + Values<Instance> values; + Interfaces<Instance> mantle; +}; + +namespace Node +{ +namespace detail +{ +template <typename> +struct Identifier +{ + static constexpr std::string_view value = "UNASSIGNED_IDENTIFIER"; +}; +} // namespace detail + +template <typename Type> +constexpr std::string_view identifier = detail::Identifier<Type>::value; + +#define SET_NODE_IDENTIFIER(ACTION) \ + namespace detail \ + { \ + template <> \ + struct Identifier<ACTION> \ + { \ + static constexpr std::string_view value = #ACTION; \ + }; \ + } + +namespace detail +{ +template <typename> +struct Implementation +{ + using type = void; +}; +} // namespace detail + +template <typename Type> +using Implementation = typename detail::Implementation<Type>::type; + +#define SPECIALIZE_NODE_TYPE_TRAIT(TRAIT, ACTION, TYPE) \ + namespace detail \ + { \ + template <> \ + struct TRAIT<ACTION> \ + { \ + using type = TYPE; \ + }; \ + } + +#define SET_NODE_IMPLEMENTATION(ACTION, TYPE) SPECIALIZE_NODE_TYPE_TRAIT(Implementation, ACTION, TYPE) + +namespace detail +{ +template <typename> +struct Interface +{ + using type = void; +}; +} // namespace detail + +template <typename Type> +using Interface = typename detail::Interface<Type>::type; + +#define SET_NODE_INTERFACE(ACTION, TYPE) SPECIALIZE_NODE_TYPE_TRAIT(Interface, ACTION, TYPE) + +template <typename Instance> +class ActionNode : public yase::ActionNode +{ +public: + ActionNode(std::shared_ptr<Interface<Instance>> action) + : yase::ActionNode{std::string{identifier<Instance>}}, + action_{action} {} + +protected: + std::unique_ptr<Implementation<Instance>> impl_{nullptr}; + std::shared_ptr<Interface<Instance>> action_; +}; +} // namespace Node +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/ActivateControllerAction.cpp b/engine/src/Storyboard/GenericAction/ActivateControllerAction.cpp index 646c8cfd9a2501bf95c313d863c1381cca7fb22d..aa2ccf2af35a3231e10e0da89a6b8caae6d6cb7f 100644 --- a/engine/src/Storyboard/GenericAction/ActivateControllerAction.cpp +++ b/engine/src/Storyboard/GenericAction/ActivateControllerAction.cpp @@ -15,12 +15,43 @@ #include <cassert> #include <utility> +#include "Storyboard/GenericAction/ActivateControllerAction.h" #include "Utils/EntityBroker.h" +#include "Utils/EntityUtils.h" #include "Utils/IControllerService.h" +#include "Utils/Logger.h" -namespace OpenScenarioEngine::v1_3::Node +namespace OpenScenarioEngine::v1_3 { +bool ActivateControllerAction::Step() +{ + const auto& entity_repository = mantle.environment->GetEntityRepository(); + + for (const auto& entity_name : values.entities) + { + // entities might have been despawned, leading to a dangling reference to the controllers + // so we check if the entity is still part of the simulation, before we access the controllers + if (auto entity_ref = entity_repository.Get(entity_name)) + { + const auto entity_id = entity_ref->get().GetUniqueId(); + if (const auto& entity_controllers = ose_services.controllerService->GetControllers(entity_id)) + { + auto controller_id = ose_services.controllerService->GetControllerId( + values.controllerRef, *entity_controllers); + ose_services.controllerService->ChangeState( + entity_id, controller_id, values.lateral_state, values.longitudinal_state); + } + else + { + Logger::Error("ActivateControllerAction: No user defined controller available for " + entity_name); + } + } + } + return true; +} +namespace Node +{ namespace detail { template <typename TargetState> @@ -34,13 +65,6 @@ auto convert_to(bool is_set, bool value) } } // namespace detail -ActivateControllerAction::ActivateControllerAction( - std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IActivateControllerAction> activateControllerAction) - : yase::ActionNode{"ActivateControllerAction"}, - activateControllerAction_{activateControllerAction} -{ -} - void ActivateControllerAction::onInit() {} yase::NodeStatus ActivateControllerAction::tick() @@ -57,30 +81,22 @@ void ActivateControllerAction::lookupAndRegisterData(yase::Blackboard& blackboar auto controllerService = blackboard.get<ControllerServicePtr>("ControllerService"); impl_ = std::make_unique<OpenScenarioEngine::v1_3::ActivateControllerAction>( - OpenScenarioEngine::v1_3::ActivateControllerAction::Values{ + Values<OpenScenarioEngine::v1_3::ActivateControllerAction>{ entityBroker->GetEntities(), - - activateControllerAction_->IsSetAnimation() ? std::make_optional<bool>(activateControllerAction_->GetAnimation()) : std::nullopt, - - ConvertScenarioControllerRef(activateControllerAction_->GetControllerRef()), - + action_->IsSetAnimation() ? std::make_optional<bool>(action_->GetAnimation()) : std::nullopt, + ConvertScenarioControllerRef(action_->GetControllerRef()), detail::convert_to<mantle_api::IController::LateralState>( - activateControllerAction_->IsSetLateral(), - activateControllerAction_->GetLateral()), - - activateControllerAction_->IsSetLighting() - ? std::make_optional<bool>(activateControllerAction_->GetLighting()) + action_->IsSetLateral(), + action_->GetLateral()), + action_->IsSetLighting() + ? std::make_optional<bool>(action_->GetLighting()) : std::nullopt, - detail::convert_to<mantle_api::IController::LongitudinalState>( - activateControllerAction_->IsSetLongitudinal(), - activateControllerAction_->GetLongitudinal()) - - }, // Values - OpenScenarioEngine::v1_3::ActivateControllerAction::Interfaces{ - environment}, - OpenScenarioEngine::v1_3::ActivateControllerAction::OseServices{ - controllerService}); + action_->IsSetLongitudinal(), + action_->GetLongitudinal()) // + }, + Interfaces<OpenScenarioEngine::v1_3::ActivateControllerAction>{environment}, + OseServices{controllerService}); } - -} // namespace OpenScenarioEngine::v1_3::Node +} // namespace Node +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/ActivateControllerAction.h b/engine/src/Storyboard/GenericAction/ActivateControllerAction.h index 601a3d3b2fe6ef2c8e95adfbf2e37e1540ba4840..a154b36002378653b5b3eb45d6efc1906fa24144 100644 --- a/engine/src/Storyboard/GenericAction/ActivateControllerAction.h +++ b/engine/src/Storyboard/GenericAction/ActivateControllerAction.h @@ -10,25 +10,65 @@ #pragma once -#include <agnostic_behavior_tree/action_node.h> -#include <openScenarioLib/generated/v1_3/api/ApiClassInterfacesV1_3.h> +#include <MantleAPI/Execution/i_environment.h> +#include <MantleAPI/Traffic/i_controller.h> -#include "Storyboard/GenericAction/ActivateControllerAction_impl.h" +#include "Action.h" +#include "Conversion/OscToMantle/ConvertScenarioController.h" +#include "Utils/EntityBroker.h" +#include "Utils/IControllerService.h" -namespace OpenScenarioEngine::v1_3::Node +namespace OpenScenarioEngine::v1_3 { -class ActivateControllerAction : public yase::ActionNode +struct OseServices +{ + ControllerServicePtr controllerService; +}; + +class ActivateControllerAction; + +template <> +struct Values<ActivateControllerAction> +{ + Entities entities; + std::optional<bool> animation; + std::optional<std::string> controllerRef; + mantle_api::IController::LateralState lateral_state; + std::optional<bool> lighting; + mantle_api::IController::LongitudinalState longitudinal_state; +}; + +class ActivateControllerAction : Action<ActivateControllerAction> { public: - ActivateControllerAction(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IActivateControllerAction> activateControllerAction); + ActivateControllerAction(Values<ActivateControllerAction> values, Interfaces<ActivateControllerAction> interfaces, OseServices ose_services) + : Action<ActivateControllerAction>{std::move(values), std::move(interfaces)}, + ose_services{std::move(ose_services)} {} + + bool Step(); + +protected: + OseServices ose_services; +}; + +namespace Node +{ +class ActivateControllerAction; +SET_NODE_IDENTIFIER(ActivateControllerAction) +SET_NODE_IMPLEMENTATION(ActivateControllerAction, OpenScenarioEngine::v1_3::ActivateControllerAction) +SET_NODE_INTERFACE(ActivateControllerAction, NET_ASAM_OPENSCENARIO::v1_3::IActivateControllerAction) + +class ActivateControllerAction : public ActionNode<ActivateControllerAction> +{ +public: + using ActionNode<ActivateControllerAction>::ActionNode; + void onInit() override; private: yase::NodeStatus tick() override; - void lookupAndRegisterData(yase::Blackboard& blackboard) final; - std::unique_ptr<OpenScenarioEngine::v1_3::ActivateControllerAction> impl_{nullptr}; - std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IActivateControllerAction> activateControllerAction_; + void lookupAndRegisterData(yase::Blackboard& blackboard) final; }; - -} // namespace OpenScenarioEngine::v1_3::Node \ No newline at end of file +} // namespace Node +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/ActivateControllerAction_base.h b/engine/src/Storyboard/GenericAction/ActivateControllerAction_base.h deleted file mode 100644 index bbdd2091b57aacf46ebbf8d66414d55224e5e8e3..0000000000000000000000000000000000000000 --- a/engine/src/Storyboard/GenericAction/ActivateControllerAction_base.h +++ /dev/null @@ -1,57 +0,0 @@ -/******************************************************************************** - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) - * - * This program and the accompanying materials are made available under the - * terms of the Eclipse Public License 2.0 which is available at - * http://www.eclipse.org/legal/epl-2.0. - * - * SPDX-License-Identifier: EPL-2.0 - ********************************************************************************/ - -#pragma once - -#include <MantleAPI/Execution/i_environment.h> -#include <MantleAPI/Traffic/i_controller.h> - -#include "Conversion/OscToMantle/ConvertScenarioController.h" -#include "Utils/EntityBroker.h" -#include "Utils/IControllerService.h" - -namespace OpenScenarioEngine::v1_3 -{ -class ActivateControllerActionBase -{ -public: - struct Values - { - Entities entities; - std::optional<bool> animation; - std::optional<std::string> controllerRef; - mantle_api::IController::LateralState lateral_state; - std::optional<bool> lighting; - mantle_api::IController::LongitudinalState longitudinal_state; - }; - struct Interfaces - { - std::shared_ptr<mantle_api::IEnvironment> environment; - }; - - struct OseServices - { - ControllerServicePtr controllerService; - }; - - ActivateControllerActionBase(Values values, Interfaces interfaces, OseServices ose_services) - : values{std::move(values)}, - mantle{std::move(interfaces)}, - ose_services{std::move(ose_services)} {}; - virtual ~ActivateControllerActionBase() = default; - virtual bool Step() = 0; - -protected: - Values values; - Interfaces mantle; - OseServices ose_services; -}; - -} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/ActivateControllerAction_impl.cpp b/engine/src/Storyboard/GenericAction/ActivateControllerAction_impl.cpp deleted file mode 100644 index 92d6399a9f5f17f99a4141f4230f75d68fea1c25..0000000000000000000000000000000000000000 --- a/engine/src/Storyboard/GenericAction/ActivateControllerAction_impl.cpp +++ /dev/null @@ -1,47 +0,0 @@ -/******************************************************************************** - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) - * - * This program and the accompanying materials are made available under the - * terms of the Eclipse Public License 2.0 which is available at - * http://www.eclipse.org/legal/epl-2.0. - * - * SPDX-License-Identifier: EPL-2.0 - ********************************************************************************/ - -#include "Storyboard/GenericAction/ActivateControllerAction_impl.h" - -#include "Utils/EntityUtils.h" -#include "Utils/Logger.h" - -namespace OpenScenarioEngine::v1_3 -{ - -bool ActivateControllerAction::Step() -{ - const auto& entity_repository = mantle.environment->GetEntityRepository(); - - for (const auto& entity_name : values.entities) - { - // entities might have been despawned, leading to a dangling reference to the controllers - // so we check if the entity is still part of the simulation, before we access the controllers - if (auto entity_ref = entity_repository.Get(entity_name)) - { - const auto entity_id = entity_ref->get().GetUniqueId(); - if (const auto& entity_controllers = ose_services.controllerService->GetControllers(entity_id)) - { - auto controller_id = ose_services.controllerService->GetControllerId( - values.controllerRef, *entity_controllers); - ose_services.controllerService->ChangeState( - entity_id, controller_id, values.lateral_state, values.longitudinal_state); - } - else - { - Logger::Error("ActivateControllerAction: No user defined controller available for " + entity_name); - } - } - } - - return true; -} - -} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/ActivateControllerAction_impl.h b/engine/src/Storyboard/GenericAction/ActivateControllerAction_impl.h deleted file mode 100644 index 701d514fdc7ade8213f172c109a1c68fec187b9e..0000000000000000000000000000000000000000 --- a/engine/src/Storyboard/GenericAction/ActivateControllerAction_impl.h +++ /dev/null @@ -1,29 +0,0 @@ -/******************************************************************************** - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) - * - * This program and the accompanying materials are made available under the - * terms of the Eclipse Public License 2.0 which is available at - * http://www.eclipse.org/legal/epl-2.0. - * - * SPDX-License-Identifier: EPL-2.0 - ********************************************************************************/ - -#pragma once - -#include "Storyboard/GenericAction/ActivateControllerAction_base.h" - -namespace OpenScenarioEngine::v1_3 -{ -class ActivateControllerAction : public ActivateControllerActionBase -{ -public: - ActivateControllerAction(Values values, Interfaces interfaces, OseServices ose_services) - : ActivateControllerActionBase{ - std::move(values), - std::move(interfaces), - std::move(ose_services)} {}; - - bool Step() override; -}; - -} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/LightStateAction.cpp b/engine/src/Storyboard/GenericAction/LightStateAction.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7f225d0a004ac2626fb8042b712f3a9cc4f16dcb --- /dev/null +++ b/engine/src/Storyboard/GenericAction/LightStateAction.cpp @@ -0,0 +1,65 @@ +/******************************************************************************** + * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#include "Storyboard/GenericAction/LightStateAction.h" + +#include <MantleAPI/Execution/i_environment.h> + +#include "Conversion/OscToMantle/ConvertScenarioLightState.h" +#include "Conversion/OscToMantle/ConvertScenarioLightType.h" +#include "Utils/EntityUtils.h" +#include "Utils/Logger.h" + +namespace OpenScenarioEngine::v1_3 +{ + +bool LightStateAction::Step() +{ + SetControlStrategy(); + return true; +} + +void LightStateAction::SetControlStrategy() +{ + control_strategy_->light_type = values.lightType; + control_strategy_->light_state = values.lightState; + for (const auto& actor : values.entities) + { + auto& entity = EntityUtils::GetEntityByName(mantle.environment, actor); + mantle.environment->UpdateControlStrategies( + entity.GetUniqueId(), + {control_strategy_}); + } +} +namespace Node +{ +yase::NodeStatus LightStateAction::tick() +{ + assert(impl_); + const auto is_finished = impl_->Step(); + return is_finished ? yase::NodeStatus::kSuccess : yase::NodeStatus::kRunning; +}; + +void LightStateAction::lookupAndRegisterData(yase::Blackboard& blackboard) +{ + std::shared_ptr<mantle_api::IEnvironment> environment = blackboard.get<std::shared_ptr<mantle_api::IEnvironment>>("Environment"); + const EntityBroker::Ptr entityBroker = blackboard.get<EntityBroker::Ptr>("EntityBroker"); + + impl_ = std::make_unique<OpenScenarioEngine::v1_3::LightStateAction>( + Values<OpenScenarioEngine::v1_3::LightStateAction>{ + entityBroker->GetEntities(), + action_->GetTransitionTime(), + ConvertScenarioLightType(action_->GetLightType()), + ConvertScenarioLightState(action_->GetLightState()) // + }, + Interfaces<OpenScenarioEngine::v1_3::LightStateAction>{environment}); +} +} // namespace Node +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/LightStateAction.h b/engine/src/Storyboard/GenericAction/LightStateAction.h index 3311528aa09a6859c407a4b5236bbbdb33c04468..982fb916528ebca79a9d2d02f44e33710c920cb4 100644 --- a/engine/src/Storyboard/GenericAction/LightStateAction.h +++ b/engine/src/Storyboard/GenericAction/LightStateAction.h @@ -17,47 +17,60 @@ #include <cassert> #include <utility> -#include "Storyboard/GenericAction/LightStateAction_impl.h" +#include "Action.h" #include "Utils/EntityBroker.h" -namespace OpenScenarioEngine::v1_3::Node +namespace OpenScenarioEngine::v1_3 { -class LightStateAction : public yase::ActionNode +class LightStateAction; + +template <> +struct Values<LightStateAction> +{ + Entities entities; + double transitionTime; + mantle_api::LightType lightType; + mantle_api::LightState lightState; +}; + +template <> +struct Interfaces<LightStateAction> +{ + std::shared_ptr<mantle_api::IEnvironment> environment; +}; + +class LightStateAction : Action<LightStateAction> { public: - LightStateAction(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ILightStateAction> lightStateAction) - : yase::ActionNode{"LightStateAction"}, - lightStateAction_{lightStateAction} - { - } + using Action<LightStateAction>::Action; - void onInit() override{}; + bool Step(); private: - yase::NodeStatus tick() override - { - assert(impl_); - const auto is_finished = impl_->Step(); - return is_finished ? yase::NodeStatus::kSuccess : yase::NodeStatus::kRunning; - }; - - void lookupAndRegisterData(yase::Blackboard& blackboard) final - { - std::shared_ptr<mantle_api::IEnvironment> environment = blackboard.get<std::shared_ptr<mantle_api::IEnvironment>>("Environment"); - const EntityBroker::Ptr entityBroker = blackboard.get<EntityBroker::Ptr>("EntityBroker"); - - impl_ = std::make_unique<OpenScenarioEngine::v1_3::LightStateAction>( - OpenScenarioEngine::v1_3::LightStateAction::Values{ - entityBroker->GetEntities(), - lightStateAction_->GetTransitionTime(), - ConvertScenarioLightType(lightStateAction_->GetLightType()), - ConvertScenarioLightState(lightStateAction_->GetLightState())}, - OpenScenarioEngine::v1_3::LightStateAction::Interfaces{ - environment}); - } - - std::unique_ptr<OpenScenarioEngine::v1_3::LightStateAction> impl_{nullptr}; - std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ILightStateAction> lightStateAction_; + void SetControlStrategy(); + + std::shared_ptr<mantle_api::VehicleLightStatesControlStrategy> control_strategy_{ + std::make_shared<mantle_api::VehicleLightStatesControlStrategy>()}; }; -} // namespace OpenScenarioEngine::v1_3::Node +namespace Node +{ +class LightStateAction; +SET_NODE_IDENTIFIER(LightStateAction) +SET_NODE_IMPLEMENTATION(LightStateAction, OpenScenarioEngine::v1_3::LightStateAction) +SET_NODE_INTERFACE(LightStateAction, NET_ASAM_OPENSCENARIO::v1_3::ILightStateAction) + +class LightStateAction : public ActionNode<LightStateAction> +{ +public: + using ActionNode<LightStateAction>::ActionNode; + + void onInit() override{}; + +private: + yase::NodeStatus tick() override; + + void lookupAndRegisterData(yase::Blackboard& blackboard) final; +}; +} // namespace Node +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/LightStateAction_base.h b/engine/src/Storyboard/GenericAction/LightStateAction_base.h deleted file mode 100644 index efcd12a39e299f3d27ed3f7b33c782d88e5a5031..0000000000000000000000000000000000000000 --- a/engine/src/Storyboard/GenericAction/LightStateAction_base.h +++ /dev/null @@ -1,48 +0,0 @@ -/******************************************************************************** - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) - * - * This program and the accompanying materials are made available under the - * terms of the Eclipse Public License 2.0 which is available at - * http://www.eclipse.org/legal/epl-2.0. - * - * SPDX-License-Identifier: EPL-2.0 - ********************************************************************************/ - -#pragma once - -#include <MantleAPI/Execution/i_environment.h> - -#include "Conversion/OscToMantle/ConvertScenarioLightState.h" -#include "Conversion/OscToMantle/ConvertScenarioLightType.h" -#include "Utils/EntityBroker.h" - -namespace OpenScenarioEngine::v1_3 -{ - -class LightStateActionBase -{ -public: - struct Values - { - Entities entities; - double transitionTime; - mantle_api::LightType lightType; - mantle_api::LightState lightState; - }; - struct Interfaces - { - std::shared_ptr<mantle_api::IEnvironment> environment; - }; - - LightStateActionBase(Values values, Interfaces interfaces) - : values{std::move(values)}, - mantle{std::move(interfaces)} {}; - virtual ~LightStateActionBase() = default; - virtual bool Step() = 0; - -protected: - Values values; - Interfaces mantle; -}; - -} // namespace OpenScenarioEngine::v1_3 \ No newline at end of file diff --git a/engine/src/Storyboard/GenericAction/LightStateAction_impl.cpp b/engine/src/Storyboard/GenericAction/LightStateAction_impl.cpp deleted file mode 100644 index e993a670c377e3961a1b5fe6266f3baf529650b1..0000000000000000000000000000000000000000 --- a/engine/src/Storyboard/GenericAction/LightStateAction_impl.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/******************************************************************************** - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) - * - * This program and the accompanying materials are made available under the - * terms of the Eclipse Public License 2.0 which is available at - * http://www.eclipse.org/legal/epl-2.0. - * - * SPDX-License-Identifier: EPL-2.0 - ********************************************************************************/ - -#include "Storyboard/GenericAction/LightStateAction_impl.h" - -#include "Utils/EntityUtils.h" -#include "Utils/Logger.h" - -namespace OpenScenarioEngine::v1_3 -{ - -bool LightStateAction::Step() -{ - SetControlStrategy(); - return true; -} - -void LightStateAction::SetControlStrategy() -{ - control_strategy_->light_type = values.lightType; - control_strategy_->light_state = values.lightState; - for (const auto& actor : values.entities) - { - auto& entity = EntityUtils::GetEntityByName(mantle.environment, actor); - mantle.environment->UpdateControlStrategies( - entity.GetUniqueId(), - {control_strategy_}); - } -} - -} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/LightStateAction_impl.h b/engine/src/Storyboard/GenericAction/LightStateAction_impl.h deleted file mode 100644 index 2ea1c6fbe2b90d8a77deeee5c09504aac946c5a8..0000000000000000000000000000000000000000 --- a/engine/src/Storyboard/GenericAction/LightStateAction_impl.h +++ /dev/null @@ -1,33 +0,0 @@ -/******************************************************************************** - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) - * - * This program and the accompanying materials are made available under the - * terms of the Eclipse Public License 2.0 which is available at - * http://www.eclipse.org/legal/epl-2.0. - * - * SPDX-License-Identifier: EPL-2.0 - ********************************************************************************/ - -#pragma once - -#include "Storyboard/GenericAction/LightStateAction_base.h" - -namespace OpenScenarioEngine::v1_3 -{ - -class LightStateAction : public LightStateActionBase -{ -public: - LightStateAction(Values values, Interfaces interfaces) - : LightStateActionBase{std::move(values), std::move(interfaces)} {}; - - bool Step() override; - -private: - void SetControlStrategy(); - - std::shared_ptr<mantle_api::VehicleLightStatesControlStrategy> control_strategy_{ - std::make_shared<mantle_api::VehicleLightStatesControlStrategy>()}; -}; - -} // namespace OpenScenarioEngine::v1_3 \ No newline at end of file diff --git a/engine/src/Storyboard/GenericAction/TrafficAreaAction.cpp b/engine/src/Storyboard/GenericAction/TrafficAreaAction.cpp new file mode 100644 index 0000000000000000000000000000000000000000..797aa3eb7f6358a220b95c10211f299fac968a59 --- /dev/null +++ b/engine/src/Storyboard/GenericAction/TrafficAreaAction.cpp @@ -0,0 +1,98 @@ +/******************************************************************************** + * Copyright (c) 2021-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#include "Storyboard/GenericAction/TrafficAreaAction.h" + +#include "Conversion/OscToMantle/ConvertScenarioTrafficArea.h" +#include "Utils/ControllerCreator.h" +#include "Utils/EntityCreator.h" +#include "Utils/Logger.h" +#include "Utils/Spawning/Length.h" +#include "Utils/Spawning/SpawnSpace.h" +#include "Utils/Spawning/Transform.h" + +namespace OpenScenarioEngine::v1_3 +{ +mantle_api::IController& Interfaces<TrafficAreaAction>::operator()(mantle_api::IEntity& entity, const std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>>& controllers) +{ + ControllerRegistrar registrar{entity, entity.GetName(), controllers, *environment, environment->GetControllerRepository(), controller_service}; + auto& controller{registrar.CreateDefaultController()}; + registrar.CreateUserDefinedControllers(false); + return controller; +} + +void TrafficAreaAction::UpdateTrafficAreas() +{ + trafficAreas = Transform(values.trafficArea, [this](const mantle_api::RoadRange& range) + { return mantle.environment->GetTrafficAreaService().CreateTrafficArea(range); }); + lengths = Transform(trafficAreas, ToAccumulatedLength{}); +} + +bool TrafficAreaAction::Step() +{ + if (values.numberOfEntities == 0) // No entities left to spawn + { + return true; + } + if (values.trafficDistributions.empty()) // No spawnable entities + { + return false; + } + if (trafficAreas.empty()) + { + UpdateTrafficAreas(); + if (trafficAreas.empty()) // No areas to spawn entities in + { + return false; + } + } + EntityCreator spawner{mantle.environment}; + SequentialSpawnSpace spawnSpace{trafficAreas, values.trafficDistributions}; + while (values.numberOfEntities) + { + if (!spawnSpace.Spawn(spawner, mantle, rng)) + { + return false; + } + --values.numberOfEntities; + } + return true; +} + +namespace Node +{ +void TrafficAreaAction::onInit(){}; + +yase::NodeStatus TrafficAreaAction::tick() +{ + assert(impl_); + const auto is_finished = impl_->Step(); + return is_finished ? yase::NodeStatus::kSuccess : yase::NodeStatus::kRunning; +}; + +void TrafficAreaAction::lookupAndRegisterData(yase::Blackboard& blackboard) +{ + auto environment = blackboard.get<std::shared_ptr<mantle_api::IEnvironment>>("Environment"); + auto controller_service = std::dynamic_pointer_cast<ControllerService>(blackboard.get<std::shared_ptr<IControllerService>>("ControllerService")); + if (!controller_service) + { + throw std::runtime_error("TrafficAreaAction::lookupAndRegisterData: IControllerService not supported"); + } + + impl_ = std::make_unique<OpenScenarioEngine::v1_3::TrafficAreaAction>( + Values<OpenScenarioEngine::v1_3::TrafficAreaAction>{ + action_->GetContinuous(), + action_->GetNumberOfEntities(), + ConvertScenarioTrafficDistribution(action_->GetTrafficDistribution()), + ConvertScenarioTrafficArea(action_->GetTrafficArea())}, + Interfaces<OpenScenarioEngine::v1_3::TrafficAreaAction>{environment, controller_service}); +} +} // namespace Node +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/TrafficAreaAction.h b/engine/src/Storyboard/GenericAction/TrafficAreaAction.h new file mode 100644 index 0000000000000000000000000000000000000000..ddf6db219a26271c5942443ed1252952c3c2ba70 --- /dev/null +++ b/engine/src/Storyboard/GenericAction/TrafficAreaAction.h @@ -0,0 +1,89 @@ +/******************************************************************************** + * Copyright (c) 2021-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include <MantleAPI/Execution/i_environment.h> +#include <MantleAPI/Traffic/i_traffic_area_service.h> +#include <Stochastics/Stochastics.h> +#include <Stochastics/StochasticsInterface.h> +#include <agnostic_behavior_tree/action_node.h> +#include <openScenarioLib/generated/v1_3/api/ApiClassInterfacesV1_3.h> +#include <units.h> + +#include <cassert> +#include <utility> +#include <vector> + +#include "Action.h" +#include "Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h" +#include "Utils/ControllerService.h" + +namespace OpenScenarioEngine::v1_3 +{ +class TrafficAreaAction; + +template <> +struct Values<TrafficAreaAction> +{ + bool continuous; + unsigned int numberOfEntities; + TrafficDistributions trafficDistributions; + std::vector<mantle_api::RoadRange> trafficArea; +}; + +template <> +struct Interfaces<TrafficAreaAction> +{ + std::shared_ptr<mantle_api::IEnvironment> environment; + std::shared_ptr<ControllerService> controller_service; + + mantle_api::IController& operator()(mantle_api::IEntity& entity, const std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>>& controllers); +}; + +class TrafficAreaAction : public Action<TrafficAreaAction> +{ +public: + using Action<TrafficAreaAction>::Action; + + bool Step(); + +private: + void UpdateTrafficAreas(); + + std::vector<mantle_api::TrafficArea> trafficAreas; + + /// The accumulated size of all traffic areas up to and including the index of the respective element. + /// Does not account for overlapping areas. + std::vector<units::length::meter_t> lengths; + Stochastics rng; +}; + +namespace Node +{ +class TrafficAreaAction; +SET_NODE_IDENTIFIER(TrafficAreaAction) +SET_NODE_IMPLEMENTATION(TrafficAreaAction, OpenScenarioEngine::v1_3::TrafficAreaAction) +SET_NODE_INTERFACE(TrafficAreaAction, NET_ASAM_OPENSCENARIO::v1_3::ITrafficAreaAction) + +class TrafficAreaAction : public ActionNode<TrafficAreaAction> +{ +public: + using ActionNode<TrafficAreaAction>::ActionNode; + + void onInit() override; + +private: + yase::NodeStatus tick() override; + + void lookupAndRegisterData(yase::Blackboard& blackboard) final; +}; +} // namespace Node +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Storyboard/GenericAction/TrafficSwarmAction_impl.cpp b/engine/src/Storyboard/GenericAction/TrafficSwarmAction_impl.cpp index 16deb16a337f49cc38b86a9824d2d0ffee0ee849..228cb5a3992686fd657bca4cc2749d3058b232d9 100644 --- a/engine/src/Storyboard/GenericAction/TrafficSwarmAction_impl.cpp +++ b/engine/src/Storyboard/GenericAction/TrafficSwarmAction_impl.cpp @@ -15,21 +15,11 @@ #include "MantleAPI/Traffic/i_traffic_swarm_service.h" #include "Utils/Ellipse.h" #include "Utils/EntityUtils.h" +#include "Utils/Geometry.h" namespace detail { constexpr units::length::meter_t ELLIPSE_DETECTION_TOLERANCE{1.0}; - -mantle_api::Vec3<units::velocity::meters_per_second_t> GetVelocityVector( - units::velocity::meters_per_second_t speed, - const mantle_api::Orientation3<units::angle::radian_t>& orientation) -{ - auto cos_elevation{units::math::cos(orientation.pitch)}; - return {speed * units::math::cos(orientation.yaw) * cos_elevation, - speed * units::math::sin(orientation.yaw) * cos_elevation, - speed * -units::math::sin(orientation.pitch)}; -} - } // namespace detail namespace OpenScenarioEngine::v1_3 @@ -238,7 +228,7 @@ void TrafficSwarmAction::SpawnEntities() auto& entity{mantle_.environment->GetEntityRepository().Create("traffic-swarm-entity-" + std::to_string(spawned_entities_count_ + 1), vehicle_properties)}; entity.SetPosition(spawning_pose.position); entity.SetOrientation(spawning_pose.orientation); - entity.SetVelocity(detail::GetVelocityVector(vehicle_speed, spawning_pose.orientation)); + entity.SetVelocity(util::Rotate(vehicle_speed, spawning_pose.orientation)); const auto controller_configuration{GetControllerConfiguration()}; auto external_controller_config{std::make_unique<mantle_api::ExternalControllerConfig>(controller_configuration)}; diff --git a/engine/src/Utils/ControllerCreator.cpp b/engine/src/Utils/ControllerCreator.cpp index 669a8875ef48bd39ca7c89fc0204f4021326f8f7..96de78e223eee08980ae500204da6086ef46370b 100644 --- a/engine/src/Utils/ControllerCreator.cpp +++ b/engine/src/Utils/ControllerCreator.cpp @@ -1,5 +1,5 @@ /******************************************************************************* - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * Copyright (c) 2021-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) * * This program and the accompanying materials are made * available under the terms of the Eclipse Public License 2.0 @@ -162,7 +162,7 @@ void ControllerCreator::CreateControllers(const std::vector<std::shared_ptr<NET_ ControllerRegistrar::ControllerRegistrar( mantle_api::IEntity& entity, const std::string& entity_name, - std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>>&& object_controllers, + std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>> object_controllers, mantle_api::IEnvironment& environment, mantle_api::IControllerRepository& controller_repository, std::shared_ptr<ControllerService>& controller_service) @@ -175,7 +175,7 @@ ControllerRegistrar::ControllerRegistrar( { } -void ControllerRegistrar::CreateDefaultController() +mantle_api::IController& ControllerRegistrar::CreateDefaultController() { Logger::Info("ControllerCreator: Setting up internal controller for entity \"" + entity_name_ + "\""); @@ -186,6 +186,7 @@ void ControllerRegistrar::CreateDefaultController() auto& controller = controller_repository_.Create(std::move(default_config)); controller.ChangeState(mantle_api::IController::LateralState::kActivate, mantle_api::IController::LongitudinalState::kActivate); RegisterDefaultController(controller); + return controller; } void ControllerRegistrar::CreateUserDefinedControllers(bool control_override) diff --git a/engine/src/Utils/ControllerCreator.h b/engine/src/Utils/ControllerCreator.h index abdbff2a01e43302494789e4911d4060af88388f..4542cadd243e6cee9e1d7b703f92b49d39bc8c04 100644 --- a/engine/src/Utils/ControllerCreator.h +++ b/engine/src/Utils/ControllerCreator.h @@ -1,5 +1,5 @@ /******************************************************************************** - * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * Copyright (c) 2021-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) * * This program and the accompanying materials are made available under the * terms of the Eclipse Public License 2.0 which is available at @@ -50,12 +50,12 @@ public: explicit ControllerRegistrar( mantle_api::IEntity& entity, const std::string& entity_name, - std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>>&& object_controllers, + std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>> object_controllers, mantle_api::IEnvironment& environment, mantle_api::IControllerRepository& controller_repository, std::shared_ptr<ControllerService>& controller_service); - void CreateDefaultController(); + mantle_api::IController& CreateDefaultController(); void CreateUserDefinedControllers(bool control_override); private: @@ -68,7 +68,7 @@ private: const std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>> object_controllers_; mantle_api::IEnvironment& environment_; mantle_api::IControllerRepository& controller_repository_; - std::shared_ptr<ControllerService>& controller_service_; + std::shared_ptr<ControllerService> controller_service_; }; } // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/EntityCreator.cpp b/engine/src/Utils/EntityCreator.cpp index 0042a031930b199b38da083b660b1e576511ed42..52bf14470965c9a7ee64a3b326c058376e233806 100644 --- a/engine/src/Utils/EntityCreator.cpp +++ b/engine/src/Utils/EntityCreator.cpp @@ -13,6 +13,7 @@ #include <MantleAPI/Traffic/entity_properties.h> #include <openScenarioLib/generated/v1_3/catalog/CatalogHelperV1_3.h> +#include <units.h> #include "Utils/Constants.h" @@ -63,35 +64,41 @@ std::map<NET_ASAM_OPENSCENARIO::v1_3::MiscObjectCategory::MiscObjectCategoryEnum EntityCreator::EntityCreator(std::shared_ptr<mantle_api::IEnvironment> environment) : environment_(environment) {} -void EntityCreator::CreateEntity(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObject> scenario_object) +mantle_api::IEntity& EntityCreator::CreateEntity(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IEntityObject>& entity_object, const std::string& name) { - auto entity_object = scenario_object->GetEntityObject(); - - if (!entity_object) + if (entity_object->IsSetVehicle()) { - throw std::runtime_error(std::string("entityObject missing in ScenarioObject " + scenario_object->GetName())); + return CreateVehicle(entity_object->GetVehicle(), name); } - - if (entity_object->GetVehicle() != nullptr) + else if (entity_object->IsSetPedestrian()) { - CreateVehicle(entity_object->GetVehicle(), scenario_object->GetName()); + return CreatePedestrian(entity_object->GetPedestrian(), name); } - else if (entity_object->GetPedestrian() != nullptr) + else if (entity_object->IsSetMiscObject()) { - CreatePedestrian(entity_object->GetPedestrian(), scenario_object->GetName()); + return CreateMiscObject(entity_object->GetMiscObject(), name); } - else if (entity_object->GetMiscObject() != nullptr) + else if (entity_object->IsSetCatalogReference()) + { + return CreateCatalogReferenceEntity(entity_object->GetCatalogReference(), name); + } + else { - CreateMiscObject(entity_object->GetMiscObject(), scenario_object->GetName()); + throw std::runtime_error("EntityCreator::CreateEntity: Entity has no valid object type (neither vehicle, pedestrian, miscellaneous nor catalog reference)"); } - else if (entity_object->GetCatalogReference() != nullptr) +} + +mantle_api::IEntity& EntityCreator::CreateEntity(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObject>& scenario_object) +{ + if (!scenario_object->IsSetEntityObject()) { - CreateCatalogReferenceEntity(entity_object->GetCatalogReference(), scenario_object->GetName()); + throw std::runtime_error("EntityObject missing in ScenarioObject " + scenario_object->GetName()); } + return CreateEntity(scenario_object->GetEntityObject(), scenario_object->GetName()); } -void EntityCreator::CreateVehicle(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IVehicle> vehicle, - const std::string& name) +mantle_api::IVehicle& EntityCreator::CreateVehicle(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IVehicle>& vehicle, + const std::string& name) { mantle_api::VehicleProperties properties; properties.type = mantle_api::EntityType::kVehicle; @@ -129,30 +136,46 @@ void EntityCreator::CreateVehicle(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::I properties.is_host = true; } - environment_->GetEntityRepository().Create(name, properties); + return environment_->GetEntityRepository().Create(name, properties); +} + +inline bool IsInWheelchair(const NET_ASAM_OPENSCENARIO::v1_3::IPedestrian& pedestrian) +{ + return GetEnumValue(pedestrian.GetPedestrianCategory()) == NET_ASAM_OPENSCENARIO::v1_3::PedestrianCategory::PedestrianCategoryEnum::WHEELCHAIR; } -void EntityCreator::CreatePedestrian(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPedestrian> pedestrian, - const std::string& name) +mantle_api::IEntity& EntityCreator::CreatePedestrian(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPedestrian>& pedestrian, + const std::string& name) { - if (pedestrian->GetPedestrianCategory().GetFromLiteral(pedestrian->GetPedestrianCategory().GetLiteral()) == - NET_ASAM_OPENSCENARIO::v1_3::PedestrianCategory::PedestrianCategoryEnum::WHEELCHAIR) + if (IsInWheelchair(*pedestrian)) { - mantle_api::VehicleProperties vehicle_properties; - vehicle_properties.classification = mantle_api::VehicleClass::kWheelchair; - FillEntityPropertiesForPedestrian(pedestrian, vehicle_properties); - environment_->GetEntityRepository().Create(name, vehicle_properties); + return CreateWheelchairPedestrian(pedestrian, name); } else { - mantle_api::PedestrianProperties pedestrian_properties; - FillEntityPropertiesForPedestrian(pedestrian, pedestrian_properties); - environment_->GetEntityRepository().Create(name, pedestrian_properties); + return CreateRegularPedestrian(pedestrian, name); } } -void EntityCreator::CreateMiscObject(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IMiscObject> misc_object, - const std::string& name) +mantle_api::IVehicle& EntityCreator::CreateWheelchairPedestrian(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPedestrian>& pedestrian, + const std::string& name) +{ + mantle_api::VehicleProperties vehicle_properties; + vehicle_properties.classification = mantle_api::VehicleClass::kWheelchair; + FillEntityPropertiesForPedestrian(pedestrian, vehicle_properties); + return environment_->GetEntityRepository().Create(name, vehicle_properties); +} + +mantle_api::IPedestrian& EntityCreator::CreateRegularPedestrian(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPedestrian>& pedestrian, + const std::string& name) +{ + mantle_api::PedestrianProperties pedestrian_properties; + FillEntityPropertiesForPedestrian(pedestrian, pedestrian_properties); + return environment_->GetEntityRepository().Create(name, pedestrian_properties); +} + +mantle_api::IStaticObject& EntityCreator::CreateMiscObject(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IMiscObject>& misc_object, + const std::string& name) { mantle_api::StaticObjectProperties properties; properties.type = mantle_api::EntityType::kStatic; @@ -161,32 +184,31 @@ void EntityCreator::CreateMiscObject(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3 FillBoundingBoxProperties(misc_object->GetBoundingBox(), misc_object->GetName(), properties); FillGenericProperties(*misc_object, properties); SetVerticalOffset(misc_object, properties); - environment_->GetEntityRepository().Create(name, properties); + return environment_->GetEntityRepository().Create(name, properties); } -void EntityCreator::CreateCatalogReferenceEntity( - std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ICatalogReference> catalog_reference, +mantle_api::IEntity& EntityCreator::CreateCatalogReferenceEntity( + const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ICatalogReference>& catalog_reference, const std::string& name) { - auto catalog_element = catalog_reference->GetRef(); - if (catalog_element == nullptr) + if (!catalog_reference->IsSetRef()) { - throw std::runtime_error( - std::string("CatalogReference " + catalog_reference->GetEntryName() + " without ref.")); + throw std::runtime_error("CatalogReference " + catalog_reference->GetEntryName() + " without ref."); } - + auto catalog_element = catalog_reference->GetRef(); if (NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::IsVehicle(catalog_element)) { - CreateVehicle(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsVehicle(catalog_element), name); + return CreateVehicle(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsVehicle(catalog_element), name); } - else if (NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::IsPedestrian(catalog_element)) + if (NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::IsPedestrian(catalog_element)) { - CreatePedestrian(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsPedestrian(catalog_element), name); + return CreatePedestrian(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsPedestrian(catalog_element), name); } - else if (NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::IsMiscObject(catalog_element)) + if (NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::IsMiscObject(catalog_element)) { - CreateMiscObject(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsMiscObject(catalog_element), name); + return CreateMiscObject(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsMiscObject(catalog_element), name); } + throw std::runtime_error("Catalog reference does not describe a valid entity"); } void EntityCreator::FillBoundingBoxProperties(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IBoundingBox> bounding_box, @@ -195,12 +217,12 @@ void EntityCreator::FillBoundingBoxProperties(std::shared_ptr<NET_ASAM_OPENSCENA { if (!bounding_box) { - throw std::runtime_error(std::string("Entity " + name + " without bounding box.")); + throw std::runtime_error("Entity " + name + " without bounding box."); } auto dimensions = bounding_box->GetDimensions(); if (!dimensions) { - throw std::runtime_error(std::string("Bounding box of entity " + name + " without Dimensions.")); + throw std::runtime_error("Bounding box of entity " + name + " without Dimensions."); } properties.bounding_box.dimension.length = units::length::meter_t(dimensions->GetLength()); @@ -210,7 +232,7 @@ void EntityCreator::FillBoundingBoxProperties(std::shared_ptr<NET_ASAM_OPENSCENA auto center = bounding_box->GetCenter(); if (!center) { - throw std::runtime_error(std::string("Bounding box of entity " + name + " without Center.")); + throw std::runtime_error("Bounding box of entity " + name + " without Center."); } properties.bounding_box.geometric_center.x = units::length::meter_t(center->GetX()); @@ -225,7 +247,7 @@ void EntityCreator::FillAxleProperties(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1 { if (open_scenario_axle == nullptr) { - throw std::runtime_error(std::string("Entity " + name + " is missing axle information.")); + throw std::runtime_error("Entity " + name + " is missing axle information."); } axle.bb_center_to_axle_center = { @@ -255,7 +277,7 @@ mantle_api::VehicleClass EntityCreator::GetVehicleClass(NET_ASAM_OPENSCENARIO::v { return map_vehicle_class[vehicle_category.GetFromLiteral(vehicle_category.GetLiteral())]; } - throw std::runtime_error(std::string("No vehicle class for vehicle category " + vehicle_category.GetLiteral())); + throw std::runtime_error("No vehicle class for vehicle category " + vehicle_category.GetLiteral()); } mantle_api::StaticObjectType EntityCreator::GetStaticObjectType(NET_ASAM_OPENSCENARIO::v1_3::MiscObjectCategory misc_object_category) @@ -265,7 +287,7 @@ mantle_api::StaticObjectType EntityCreator::GetStaticObjectType(NET_ASAM_OPENSCE { return map_static_object_type[misc_object_category.GetFromLiteral(misc_object_category.GetLiteral())]; } - throw std::runtime_error(std::string("No static object type for misc object category " + misc_object_category.GetLiteral())); + throw std::runtime_error("No static object type for misc object category " + misc_object_category.GetLiteral()); } mantle_api::EntityType EntityCreator::GetEntityTypeFromPedestrianCategory( @@ -276,7 +298,7 @@ mantle_api::EntityType EntityCreator::GetEntityTypeFromPedestrianCategory( { return map_entity_type[pedestrian_category.GetFromLiteral(pedestrian_category.GetLiteral())]; } - throw std::runtime_error(std::string("No entity type for pedestrian category " + pedestrian_category.GetLiteral())); + throw std::runtime_error("No entity type for pedestrian category " + pedestrian_category.GetLiteral()); } void EntityCreator::SetVerticalOffset(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IMiscObject> misc_object, mantle_api::StaticObjectProperties& entity_properties) diff --git a/engine/src/Utils/EntityCreator.h b/engine/src/Utils/EntityCreator.h index 8dc19ed7fe0eede2699d0acee4dc0b7a49190fe9..03f976d82491b0ef4c1cb1adeacaf43ab56953ef 100644 --- a/engine/src/Utils/EntityCreator.h +++ b/engine/src/Utils/EntityCreator.h @@ -21,16 +21,27 @@ class EntityCreator public: explicit EntityCreator(std::shared_ptr<mantle_api::IEnvironment> environment); - void CreateEntity(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObject> scenario_object); + mantle_api::IEntity& CreateEntity(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObject>& scenario_object); + + mantle_api::IEntity& CreateEntity(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IEntityObject>& entity_object, const std::string& name); private: - void CreateVehicle(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IVehicle> vehicle, const std::string& name); - void CreatePedestrian(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPedestrian> pedestrian, - const std::string& name); - void CreateMiscObject(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IMiscObject> misc_object, - const std::string& name); - void CreateCatalogReferenceEntity(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ICatalogReference> catalog_reference, - const std::string& name); + mantle_api::IVehicle& CreateVehicle(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IVehicle>& vehicle, const std::string& name); + + mantle_api::IEntity& CreatePedestrian(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPedestrian>& pedestrian, + const std::string& name); + + mantle_api::IVehicle& CreateWheelchairPedestrian(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPedestrian>& pedestrian, + const std::string& name); + + mantle_api::IPedestrian& CreateRegularPedestrian(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPedestrian>& pedestrian, + const std::string& name); + + mantle_api::IStaticObject& CreateMiscObject(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IMiscObject>& misc_object, + const std::string& name); + + mantle_api::IEntity& CreateCatalogReferenceEntity(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ICatalogReference>& catalog_reference, + const std::string& name); /// Fills the bounding box properties of an entity /// @param[in] bounding_box The bounding box information to copy from @@ -86,4 +97,9 @@ private: std::shared_ptr<mantle_api::IEnvironment> environment_; }; +template <typename Category> +inline auto GetEnumValue(const Category& category) +{ + return category.GetFromLiteral(category.GetLiteral()); +} } // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Geometry.h b/engine/src/Utils/Geometry.h new file mode 100644 index 0000000000000000000000000000000000000000..5e3fd697a604d1e4b7524e0cd3c951306cc3dde3 --- /dev/null +++ b/engine/src/Utils/Geometry.h @@ -0,0 +1,28 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ +#pragma once + +#include <MantleAPI/Common/orientation.h> +#include <MantleAPI/Common/vector.h> +#include <units.h> + +namespace util +{ +template <typename Unit> +constexpr mantle_api::Vec3<Unit> Rotate( + Unit magnitude, + const mantle_api::Orientation3<units::angle::radian_t>& orientation) +{ + auto cos_elevation{units::math::cos(orientation.pitch)}; + return {magnitude * units::math::cos(orientation.yaw) * cos_elevation, + magnitude * units::math::sin(orientation.yaw) * cos_elevation, + magnitude * -units::math::sin(orientation.pitch)}; +} +} // namespace util diff --git a/engine/src/Utils/Spawning/Common.h b/engine/src/Utils/Spawning/Common.h new file mode 100644 index 0000000000000000000000000000000000000000..f424bc8abcc24754e6378b3cd2cb85384955bc35 --- /dev/null +++ b/engine/src/Utils/Spawning/Common.h @@ -0,0 +1,103 @@ +/******************************************************************************** + * Copyright (c) 2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include <units.h> + +#include <algorithm> +#include <cassert> +#include <functional> +#include <memory> +#include <type_traits> + +namespace OpenScenarioEngine::v1_3 +{ +template <typename T> +bool is_uninitialized(const std::weak_ptr<T>& pointer) +{ + return pointer.expired(); +} + +template <typename T> +bool is_uninitialized(const std::shared_ptr<T>& pointer) +{ + return pointer == nullptr; +} + +template <typename T> +bool is_uninitialized(const std::unique_ptr<T>& pointer) +{ + return pointer == nullptr; +} + +template <typename T> +constexpr bool is_uninitialized(const T* pointer) +{ + return pointer == nullptr; +} + +/// Functor returning the specified value when invoked. +template <auto Value> +struct Return +{ + template <typename... Input> + constexpr decltype(Value) operator()(Input&&...) const + { + return Value; + } +}; + +/// Functor returning the input as-is. +struct Forward +{ + template <typename Input> + constexpr decltype(auto) operator()(Input&& input) const + { + return std::forward<Input>(input); + } +}; + +template <typename Operation, typename Transformer = Forward> +struct BinaryOperation +{ + using is_transparent = bool; + + template <typename A, typename B> + constexpr decltype(auto) operator()(A&&, B&&) const; +}; + +template <typename Transform> +using Less = BinaryOperation<std::less<>, Transform>; + +template <typename Transform> +using Greater = BinaryOperation<std::greater<>, Transform>; + +template <typename Transform> +using Add = BinaryOperation<std::plus<>, Transform>; + +template <typename Operation, typename Transformer> +template <typename A, typename B> +constexpr decltype(auto) BinaryOperation<Operation, Transformer>::operator()(A&& a, B&& b) const +{ + static_assert(std::is_invocable_v<Transformer, A&&> && std::is_invocable_v<Transformer, B&&>); + return Operation{}(Transformer{}(std::forward<A>(a)), Transformer{}(std::forward<B>(b))); +} + +template <typename Elements, typename Thresholds> +typename Elements::const_reference Sample(const Elements& elements, const Thresholds& thresholds, typename Thresholds::const_reference value) +{ + assert(elements.size() <= thresholds.size()); + const size_t index_offset{thresholds.size() - elements.size()}; + const auto threshold{std::upper_bound(std::next(thresholds.begin(), index_offset), thresholds.end(), value)}; + const size_t index{static_cast<size_t>(std::distance(thresholds.begin(), threshold) - index_offset)}; + return elements[index]; +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Interval.cpp b/engine/src/Utils/Spawning/Interval.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9946ebe0136f60e3f0319c4bfe71570eaca40871 --- /dev/null +++ b/engine/src/Utils/Spawning/Interval.cpp @@ -0,0 +1,56 @@ +/******************************************************************************** + * Copyright (c) 2024-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#include "Interval.h" + +#include <algorithm> +#include <cassert> + +#include "Common.h" + +namespace OpenScenarioEngine::v1_3 +{ +void OffsetIntervals(std::vector<Interval>& intervals, units::length::meter_t frontOffset, units::length::meter_t rearOffset) +{ + const auto shrinkage{frontOffset - rearOffset}; + intervals.erase(std::remove_if(intervals.begin(), intervals.end(), [shrinkage](const Interval& interval) + { return interval.GetLength() < shrinkage; }), + intervals.end()); + std::for_each(intervals.begin(), intervals.end(), OffsetBy{frontOffset, rearOffset}); +} + +std::vector<Interval> Interval::SplitBy(Interval input) const +{ + assert(min <= input.min && input.max <= max); + if (min < input.min) + { + if (input.max < max) + { + return {{min, input.min}, {input.max, max}}; + } + return {{min, input.min}}; + } + if (input.max < max) + { + return {{input.max, max}}; + } + return {}; +} + +std::ostream& operator<<(std::ostream& os, const Padding& padding) +{ + return os << '[' << padding.rear.value() << ", " << padding.front.value() << ']'; +} + +std::ostream& operator<<(std::ostream& os, const Interval& interval) +{ + return os << '[' << interval.min.value() << ", " << interval.max.value() << ']'; +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Interval.h b/engine/src/Utils/Spawning/Interval.h new file mode 100644 index 0000000000000000000000000000000000000000..ca4494d2a607e8d35bf8cc946b19ca6b28ac47c5 --- /dev/null +++ b/engine/src/Utils/Spawning/Interval.h @@ -0,0 +1,232 @@ +/******************************************************************************** + * Copyright (c) 2024-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include <MantleAPI/Traffic/i_traffic_area_stream.h> +#include <units.h> + +#include <iostream> +#include <set> +#include <vector> + +#include "Common.h" + +namespace OpenScenarioEngine::v1_3 +{ +struct Padding +{ + constexpr units::length::meter_t GetLength() const; + + constexpr Padding operator-() const; + + units::length::meter_t rear; + units::length::meter_t front; +}; + +std::ostream& operator<<(std::ostream&, const Padding&); + +struct Interval +{ + constexpr Interval(units::length::meter_t min, units::length::meter_t max) + : min{min}, max{max} {} + + /// Returns the length of this interval + /// + /// \return Difference between min and max + constexpr units::length::meter_t GetLength() const; + + /// Returns an interval that matches this interval moved by the given offsets. + /// + /// \param Padding Rear and front padding to be added to this interval + /// \return Modified copy of this interval + constexpr Interval OffsetBy(Padding) const; + + /// Returns an interval that matches this interval moved by the given offsets. + /// + /// \param Interval Min and max offset to be added to this interval's respective limits + /// \return Modified copy of this interval + constexpr Interval OffsetBy(Interval) const; + + /// Returns an interval that matches this interval moved by the given offsets. + /// + /// \param min_offset The amount to be added to min + /// \param max_offset The amount to be added to max + /// \return Modified copy of this interval + constexpr Interval OffsetBy(units::length::meter_t min_offset, units::length::meter_t max_offset) const; + + /// Returns the resulting list of intervals if the given interval were cut out of this interval. + /// + /// \param Interval A subrange of this interval + /// \return 0, 1 or 2 elements sorted by ascending (min/max) value. + std::vector<Interval> SplitBy(Interval) const; + + /// Returns the interval representing the intersection of this interval and the given interval. + /// If the intervals do not intersect, the inverted negative space between the intervals is returned instead. + /// + /// \param other Other interval + /// \return The intersection interval of this and the given other interval + constexpr Interval GetIntersection(const Interval& other) const; + + units::length::meter_t min, max; +}; + +constexpr Interval operator+(const Interval&, units::length::meter_t); +constexpr Interval operator-(const Interval&, units::length::meter_t); + +std::ostream& operator<<(std::ostream&, const Interval&); + +struct OffsetBy +{ + constexpr Interval operator()(const Interval&) const; + + Padding offset; +}; + +void OffsetIntervals(std::vector<Interval>& intervals, units::length::meter_t frontOffset, units::length::meter_t rearOffset); + +/// Returns all pairs of stream distances between which no entity is placed on the given stream +std::vector<Interval> GetFreeIntervals(const mantle_api::ITrafficAreaStream&); + +struct ToMin +{ + constexpr units::length::meter_t operator()(units::length::meter_t) const; + + constexpr units::length::meter_t operator()(const Interval&) const; +}; + +struct ToMax +{ + constexpr units::length::meter_t operator()(units::length::meter_t) const; + + constexpr units::length::meter_t operator()(const Interval&) const; +}; +} // namespace OpenScenarioEngine::v1_3 + +namespace OpenScenarioEngine::v1_3 +{ +constexpr units::length::meter_t Padding::GetLength() const +{ + return rear + front; +} + +constexpr Padding Padding::operator-() const +{ + return {units::length::meter_t{-rear()}, units::length::meter_t{-front()}}; +} + +constexpr units::length::meter_t Interval::GetLength() const +{ + return max - min; +} + +constexpr Interval Interval::GetIntersection(const Interval& other) const +{ + return {std::max(min, other.min), std::min(max, other.max)}; +} + +constexpr Interval Interval::OffsetBy(Padding padding) const +{ + return OffsetBy(-units::length::meter_t{padding.rear()}, padding.front); +} + +constexpr Interval Interval::OffsetBy(Interval other) const +{ + return OffsetBy(other.min, other.max); +} + +constexpr Interval Interval::OffsetBy(units::length::meter_t min_offset, units::length::meter_t max_offset) const +{ + return {min + min_offset, max + max_offset}; +} + +constexpr Interval operator+(const Interval& interval, units::length::meter_t offset) +{ + return {interval.min + offset, interval.max + offset}; +} + +constexpr Interval operator-(const Interval& interval, units::length::meter_t offset) +{ + return {interval.min - offset, interval.max - offset}; +} + +constexpr Interval OffsetBy::operator()(const Interval& interval) const +{ + return interval.OffsetBy(offset); +} + +template <typename Filter = Return<true>> +void EmplaceInterval(std::vector<Interval>& output, Interval&& input, Filter&& filter = {}) +{ + if constexpr (std::is_same_v<Filter, Return<true>>) + { + output.push_back(std::move(input)); + } + else + { + if (filter(input)) + { + output.push_back(std::move(input)); + } + } +} + +template <typename Filter> +std::vector<Interval> GetFreeIntervals(const mantle_api::ITrafficAreaStream& stream, Filter&& filter) +{ + // TODO: The interface of the ITrafficAreaStream does not provide an efficient way to compute free intervals. One must retrieve + // all entities on the stream, meaning the implementation must have computed their local positions, but they are not available + // through the interface. ITrafficAreaStream should be expanded to provide this information without redundant computation. + + std::vector<Interval> result; + units::length::meter_t min; + const units::length::meter_t max{stream.GetLength()}; + while (min < max) + { + std::weak_ptr<mantle_api::IEntity> pointer{stream.GetEntity(mantle_api::ITrafficAreaStream::SearchDirection::kDownstream, min, max - min)}; + if (is_uninitialized(pointer)) + { + EmplaceInterval(result, {min, max}, filter); + break; + } + auto entity{pointer.lock()}; + const auto length{entity->GetProperties()->bounding_box.dimension.length}; + const auto offset{entity->GetProperties()->bounding_box.geometric_center.x}; + auto stream_pose{stream.Convert(mantle_api::Pose{entity->GetPosition(), entity->GetOrientation()}).value()}; + if (const auto entityStartDistance{stream_pose.s + offset - length / units::dimensionless::scalar_t{2.0}}; entityStartDistance > min) + { + EmplaceInterval(result, {min, entityStartDistance}, filter); + } + min = stream_pose.s + offset + length / units::dimensionless::scalar_t{2.0}; + // TODO: This would loop endlessly if the object has a length of 0 and MIGHT loop endlessly if its offset lies outside of its bounds. + } + return result; +} + +constexpr units::length::meter_t ToMin::operator()(units::length::meter_t value) const +{ + return value; +} + +constexpr units::length::meter_t ToMin::operator()(const Interval& interval) const +{ + return interval.min; +} + +constexpr units::length::meter_t ToMax::operator()(units::length::meter_t value) const +{ + return value; +} + +constexpr units::length::meter_t ToMax::operator()(const Interval& interval) const +{ + return interval.max; +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Iterable.h b/engine/src/Utils/Spawning/Iterable.h new file mode 100644 index 0000000000000000000000000000000000000000..00baa36fdba5c55acf6314183ae34e25cab01cd9 --- /dev/null +++ b/engine/src/Utils/Spawning/Iterable.h @@ -0,0 +1,207 @@ +/******************************************************************************** + * Copyright (c) 2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include <cstdint> + +namespace OpenScenarioEngine::v1_3 +{ +namespace detail +{ +template <typename Type> +struct is_vector +{ + static constexpr bool value{false}; +}; + +template <typename Type> +struct is_vector<std::vector<Type>> +{ + static constexpr bool value{true}; +}; +} // namespace detail + +template <typename Type> +constexpr bool is_vector{detail::is_vector<Type>::value}; + +template <typename Container> +struct Iterable +{ + using const_iterator = typename Container::const_iterator; + using iterator = typename Container::iterator; + using const_reverse_iterator = typename Container::const_reverse_iterator; + using reverse_iterator = typename Container::reverse_iterator; + using const_reference = typename Container::const_reference; + using reference = typename Container::reference; + using value_type = typename Container::value_type; + + constexpr Iterable(Container&&); + + constexpr const Container& GetContainer() const; + constexpr Container& GetContainer(); + + constexpr operator const Container&() const; + constexpr operator Container&(); + + constexpr const_iterator begin() const; + constexpr iterator begin(); + + constexpr const_iterator end() const; + constexpr iterator end(); + + constexpr const_reverse_iterator rbegin() const; + constexpr reverse_iterator rbegin(); + + constexpr const_reverse_iterator rend() const; + constexpr reverse_iterator rend(); + + constexpr size_t size() const; + constexpr bool empty() const; + + constexpr const_reference front() const; + constexpr reference front(); + + constexpr const_reference back() const; + constexpr reference back(); + + template <typename Type = Container, std::enable_if_t<is_vector<Type>, bool> = true> + constexpr const_reference operator[](size_t i) const + { + return container[i]; + } + + template <typename Type = Container, std::enable_if_t<is_vector<Type>, bool> = true> + constexpr reference operator[](size_t i) + { + return container[i]; + } + +protected: + Container container; +}; +} // namespace OpenScenarioEngine::v1_3 + +namespace OpenScenarioEngine::v1_3 +{ + +template <typename Container> +constexpr Iterable<Container>::Iterable(Container&& input) + : container{std::forward<Container>(input)} +{ +} + +template <typename Container> +constexpr const Container& Iterable<Container>::GetContainer() const +{ + return (container); +} + +template <typename Container> +constexpr Container& Iterable<Container>::GetContainer() +{ + return (container); +} + +template <typename Container> +constexpr Iterable<Container>::operator const Container&() const +{ + return GetContainer(); +} + +template <typename Container> +constexpr Iterable<Container>::operator Container&() +{ + return GetContainer(); +} + +template <typename Container> +constexpr typename Container::const_iterator Iterable<Container>::begin() const +{ + return container.begin(); +} + +template <typename Container> +constexpr typename Container::iterator Iterable<Container>::begin() +{ + return container.begin(); +} + +template <typename Container> +constexpr typename Container::const_iterator Iterable<Container>::end() const +{ + return container.end(); +} + +template <typename Container> +constexpr typename Container::iterator Iterable<Container>::end() +{ + return container.end(); +} + +template <typename Container> +constexpr typename Container::const_reverse_iterator Iterable<Container>::rbegin() const +{ + return container.rbegin(); +} + +template <typename Container> +constexpr typename Container::reverse_iterator Iterable<Container>::rbegin() +{ + return container.rbegin(); +} + +template <typename Container> +constexpr typename Container::const_reverse_iterator Iterable<Container>::rend() const +{ + return container.rend(); +} + +template <typename Container> +constexpr typename Container::reverse_iterator Iterable<Container>::rend() +{ + return container.rend(); +} + +template <typename Container> +constexpr size_t Iterable<Container>::size() const +{ + return container.size(); +} + +template <typename Container> +constexpr bool Iterable<Container>::empty() const +{ + return container.empty(); +} + +template <typename Container> +constexpr typename Container::const_reference Iterable<Container>::front() const +{ + return *begin(); +} +template <typename Container> +constexpr typename Container::reference Iterable<Container>::front() +{ + return *begin(); +} + +template <typename Container> +constexpr typename Container::const_reference Iterable<Container>::back() const +{ + return *rbegin(); +} + +template <typename Container> +constexpr typename Container::reference Iterable<Container>::back() +{ + return *rbegin(); +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Length.cpp b/engine/src/Utils/Spawning/Length.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1d2a41e6ac482ed450b183b089b94caa5630be61 --- /dev/null +++ b/engine/src/Utils/Spawning/Length.cpp @@ -0,0 +1,110 @@ +/******************************************************************************** + * Copyright (c) 2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#include "Length.h" + +#include <openScenarioLib/generated/v1_3/catalog/CatalogHelperV1_3.h> + +#include <cassert> +#include <numeric> + +#include "SpawnSpace.h" + +namespace OpenScenarioEngine::v1_3 +{ +units::length::meter_t ToLength::operator()(const mantle_api::ITrafficAreaStream& stream) const +{ + return stream.GetLength(); +} + +units::length::meter_t ToLength::operator()(const mantle_api::TrafficArea& area) const +{ + return std::transform_reduce(area.begin(), area.end(), units::length::meter_t{}, std::plus<>{}, *this); +} + +units::length::meter_t ToLength::operator()(const mantle_api::IEntity& entity) const +{ + return (*this)(entity.GetProperties()); +} + +units::length::meter_t ToLength::operator()(const mantle_api::EntityProperties& properties) const +{ + return properties.bounding_box.dimension.length; +} + +units::length::meter_t ToLength::operator()(const SpawnZone& zone) const +{ + return zone.GetLength() - std::transform_reduce(zone.obstacles.begin(), zone.obstacles.end(), units::length::meter_t{}, std::plus<>{}, *this); +} + +units::length::meter_t ToLength::operator()(const SpawnZones& zones) const +{ + return std::transform_reduce(zones.begin(), zones.end(), units::length::meter_t{}, std::plus<>{}, *this); +} + +units::length::meter_t ToLength::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IBoundingBox& bounding_box) const +{ + return units::length::meter_t{bounding_box.GetDimensions()->GetLength()}; +} + +units::length::meter_t ToLength::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IVehicle& vehicle) const +{ + return (*this)(vehicle.GetBoundingBox()); +} + +units::length::meter_t ToLength::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IPedestrian& pedestrian) const +{ + return (*this)(pedestrian.GetBoundingBox()); +} + +units::length::meter_t ToLength::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IMiscObject& misc_object) const +{ + return (*this)(misc_object.GetBoundingBox()); +} + +units::length::meter_t ToLength::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IEntityObject& object) const +{ + if (object.IsSetCatalogReference()) + { + const auto& catalog_reference{object.GetCatalogReference()->GetRef()}; + auto& element{const_cast<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ICatalogElement>&>(catalog_reference)}; + if (NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::IsVehicle(element)) + { + return (*this)(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsVehicle(element)); + } + if (NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::IsPedestrian(element)) + { + return (*this)(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsPedestrian(element)); + } + if (NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::IsMiscObject(element)) + { + return (*this)(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsMiscObject(element)); + } + } + if (object.IsSetVehicle()) + { + return (*this)(object.GetVehicle()); + } + if (object.IsSetPedestrian()) + { + return (*this)(object.GetPedestrian()); + } + if (object.IsSetMiscObject()) + { + return (*this)(object.GetMiscObject()); + } + throw std::runtime_error("ToLength: Entity is not a catalog reference, vehicle, pedestrian or misc. object."); +} + +units::length::meter_t ToLength::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate& input) const +{ + return (*this)(input.GetEntitiyObject()); +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Length.h b/engine/src/Utils/Spawning/Length.h new file mode 100644 index 0000000000000000000000000000000000000000..47d7de9cbca7e47c6bc2a8800a4a9947638a87b3 --- /dev/null +++ b/engine/src/Utils/Spawning/Length.h @@ -0,0 +1,165 @@ +/******************************************************************************** + * Copyright (c) 2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include <MantleAPI/Traffic/i_entity.h> +#include <MantleAPI/Traffic/i_traffic_area_service.h> +#include <MantleAPI/Traffic/i_traffic_area_stream.h> +#include <openScenarioLib/generated/v1_3/api/ApiClassInterfacesV1_3.h> +#include <units.h> + +#include <memory> + +#include "Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h" +#include "Interval.h" +#include "SpawnZone.h" +#include "Transform.h" +#include "Weighted.h" + +namespace OpenScenarioEngine::v1_3 +{ +struct ToLength +{ + constexpr units::length::meter_t operator()(units::length::meter_t) const; + + constexpr units::length::meter_t operator()(const Interval&) const; + + units::length::meter_t operator()(const mantle_api::ITrafficAreaStream&) const; + + template <typename Type> + units::length::meter_t operator()(const std::shared_ptr<Type>&) const; + + template <typename Type> + units::length::meter_t operator()(const Weighted<Type>&) const; + + units::length::meter_t operator()(const mantle_api::IEntity&) const; + + units::length::meter_t operator()(const mantle_api::EntityProperties&) const; + + units::length::meter_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IBoundingBox&) const; + + units::length::meter_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IVehicle&) const; + + units::length::meter_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IPedestrian&) const; + + units::length::meter_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IMiscObject&) const; + + template <typename Type> + units::length::meter_t operator()(const Type*) const; + + units::length::meter_t operator()(const mantle_api::TrafficArea&) const; + + units::length::meter_t operator()(const SpawnZone&) const; + + units::length::meter_t operator()(const SpawnZones&) const; + + units::length::meter_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IEntityObject&) const; + + units::length::meter_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate&) const; +}; + +struct ToSize +{ + constexpr size_t operator()(size_t) const; + + template <typename Type> + constexpr size_t operator()(const Iterable<Type>&) const; + + template <typename Type> + constexpr size_t operator()(const std::vector<Type>&) const; +}; + +template <typename Type> +units::length::meter_t GetLength(const Type&); + +struct ToAccumulatedLength +{ + template <typename Type> + units::length::meter_t operator()(const Type&); + + units::length::meter_t totalLength; +}; + +/// Returns the positive distance from the entity's center to its rear and the positive distance from the entity's center to its front. +/// +/// \tparam Type Type for which GetLength(const Type&) is implemented +/// \return Positive distance from the entity's center to its rear and the positive distance from the entity's center to its front +template <typename Type> +Padding GetLengthPadding(const Type&); +} // namespace OpenScenarioEngine::v1_3 + +namespace OpenScenarioEngine::v1_3 +{ +constexpr units::length::meter_t ToLength::operator()(units::length::meter_t length) const +{ + return length; +} + +constexpr units::length::meter_t ToLength::operator()(const Interval& interval) const +{ + return interval.GetLength(); +} + +template <typename Type> +units::length::meter_t ToLength::operator()(const std::shared_ptr<Type>& input) const +{ + return ToLength{}(*input); +} + +template <typename Type> +units::length::meter_t ToLength::operator()(const Weighted<Type>& input) const +{ + return ToLength{}(input.handle); +} + +template <typename Type> +units::length::meter_t ToLength::operator()(const Type* input) const +{ + return ToLength{}(*input); +} + +constexpr size_t ToSize::operator()(size_t size) const +{ + return size; +} + +template <typename Type> +constexpr size_t ToSize::operator()(const Iterable<Type>& container) const +{ + return container.size(); +} + +template <typename Type> +constexpr size_t ToSize::operator()(const std::vector<Type>& container) const +{ + return container.size(); +} + +template <typename Type> +units::length::meter_t GetLength(const Type& item) +{ + return ToLength{}(item); +} + +template <typename Type> +units::length::meter_t ToAccumulatedLength::operator()(const Type& input) +{ + totalLength += GetLength(input); + return totalLength; +} + +template <typename Input> +Padding GetLengthPadding(const Input& input) +{ // TODO: Consider the center offset of the entity's bounds + const units::length::meter_t half_length{GetLength(input) * units::dimensionless::scalar_t{.5}}; + return {half_length, half_length}; +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Obstacle.cpp b/engine/src/Utils/Spawning/Obstacle.cpp new file mode 100644 index 0000000000000000000000000000000000000000..84d750da7cb4d953c4aed14d9e6ea982e41ddc27 --- /dev/null +++ b/engine/src/Utils/Spawning/Obstacle.cpp @@ -0,0 +1,59 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#include "Obstacle.h" + +#include "ToEntity.h" + +namespace OpenScenarioEngine::v1_3 +{ +template <typename Entity> +Obstacle::Obstacle(Interval interval, Entity&& entity, units::velocity::meters_per_second_t velocity) + : Interval{std::move(interval)}, entity{&to_entity(std::forward<Entity>(entity))}, velocity{velocity} +{ +} + +Obstacle Obstacle::GetStartingSentinel(units::length::meter_t value) +{ + return {{value, value}, nullptr, units::velocity::meters_per_second_t{std::numeric_limits<double>::lowest()}}; +} + +Obstacle Obstacle::GetEndingSentinel(units::length::meter_t value) +{ + return {{value, value}, nullptr, units::velocity::meters_per_second_t{std::numeric_limits<double>::max()}}; +} + +Obstacle ToObstacle(const mantle_api::IEntity* entity, const mantle_api::ITrafficAreaStream& stream) +{ + const auto halfLength{entity->GetProperties()->bounding_box.dimension.length / units::dimensionless::scalar_t{2.0}}; + const auto offset{entity->GetProperties()->bounding_box.geometric_center.x}; + auto pose{stream.Convert(mantle_api::Pose{entity->GetPosition(), {}}).value()}; + const Interval interval{pose.s + offset - halfLength, pose.s + offset + halfLength}; + const auto velocity{detail::rotate_2d(entity->GetVelocity(), pose.hdg).x}; + return {interval, entity, velocity}; +} + +std::set<Obstacle, Less<ToMin>> GetObstacles(const mantle_api::ITrafficAreaStream& stream) +{ + std::set<Obstacle, Less<ToMin>> result; + units::length::meter_t min; + const units::length::meter_t max{stream.GetLength()}; + while (min < max) + { + const mantle_api::IEntity* entity{stream.GetEntity(mantle_api::ITrafficAreaStream::SearchDirection::kDownstream, min, max - min)}; + if (is_uninitialized(entity)) + { + break; + } + min = result.emplace(ToObstacle(entity, stream)).first->max; + } + return result; +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Obstacle.h b/engine/src/Utils/Spawning/Obstacle.h new file mode 100644 index 0000000000000000000000000000000000000000..df661464a34126b41d3353941045add765454558 --- /dev/null +++ b/engine/src/Utils/Spawning/Obstacle.h @@ -0,0 +1,51 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ +#pragma once + +#include <memory> + +#include "Common.h" +#include "Interval.h" + +namespace OpenScenarioEngine::v1_3 +{ +struct Obstacle : Interval +{ + template <typename Entity> + Obstacle(Interval, Entity&&, units::velocity::meters_per_second_t); + + static Obstacle GetStartingSentinel(units::length::meter_t); + + static Obstacle GetEndingSentinel(units::length::meter_t); + + const mantle_api::IEntity* entity; + + units::velocity::meters_per_second_t velocity; +}; + +std::set<Obstacle, Less<ToMin>> GetObstacles(const mantle_api::ITrafficAreaStream&); +} // namespace OpenScenarioEngine::v1_3 + +namespace OpenScenarioEngine::v1_3::detail +{ +template <typename Type> +struct Vec2 +{ + Type x, y; +}; + +template <typename Type> +inline Vec2<Type> rotate_2d(const mantle_api::Vec3<Type>& input, units::angle::radian_t angle) +{ + const auto cos{units::math::cos(angle)}; + const auto sin{units::math::sin(angle)}; + return {input.x * cos - input.y * sin, input.x * sin + input.y * cos}; +} +} // namespace OpenScenarioEngine::v1_3::detail diff --git a/engine/src/Utils/Spawning/SpawnSpace.cpp b/engine/src/Utils/Spawning/SpawnSpace.cpp new file mode 100644 index 0000000000000000000000000000000000000000..889f8f4441ed44a0b4c77e5d37ae286a5ed7a443 --- /dev/null +++ b/engine/src/Utils/Spawning/SpawnSpace.cpp @@ -0,0 +1,201 @@ +/******************************************************************************** + * Copyright (c) 2024-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#include "SpawnSpace.h" + +#include <algorithm> +#include <cassert> +#include <numeric> + +#include "Length.h" +#include "Weight.h" + +namespace OpenScenarioEngine::v1_3 +{ +SpawnSpace::SpawnSpace(const OpenScenarioEngine::v1_3::TrafficDistributions &distributions) + : weights{{}} +{ + // Add entities + { + const size_t numberOfEntities{std::transform_reduce(distributions.begin(), distributions.end(), size_t{}, std::plus<>{}, ToSize{})}; + entities.reserve(numberOfEntities); + } + for (auto &distribution : distributions) + { + std::transform(distribution.begin(), distribution.end(), std::back_inserter(entities), // + [weight = distribution.weight](const ObjectTemplate &entity) -> Weighted<ObjectTemplate> + { return {entity, weight}; }); + } + std::sort(entities.begin(), entities.end(), Less<ToLength>{}); + Transform(entities, weights, ToAccumulatedWeight{}); +} + +SequentialSpawnSpace::SequentialSpawnSpace(const std::vector<mantle_api::TrafficArea> &areas, const OpenScenarioEngine::v1_3::TrafficDistributions &distributions) + : SpawnSpace{distributions}, areas{areas} +{ + std::vector<SpawnZones> zones{Transform(areas, ToSpawnZones{})}; // clang-format off + Transform(zones, spots, [](const SpawnZones &zones) { + assert(!zones.empty()); + return Transform(zones, ToSpawnSpots{}); + }); // clang-format on +} + +ObjectTemplate SequentialSpawnSpace::SampleObjectTemplate(StochasticsInterface &rng) const +{ + if (entities.size() <= 1) + { + return entities.empty() ? ObjectTemplate{} : entities.front().handle; + } + assert(weights.size() == entities.size() + 1); + const units::dimensionless::scalar_t sampled_weight{rng.GetUniformDistributed(.0, weights.back().value())}; + return Sample(entities, weights, sampled_weight).handle; +} + +std::tuple<std::vector<std::vector<std::vector<SpawnSpot>>>::iterator, + std::vector<std::vector<SpawnSpot>>::iterator, + std::vector<SpawnSpot>::iterator, + Interval> +SequentialSpawnSpace::FindSpot(const Padding &padding, units::velocity::meters_per_second_t velocity, units::time::second_t timespan) +{ + assert(!spots.empty()); + for (auto area{spots.begin()}; area != spots.end(); ++area) + { + for (auto stream{area->begin()}; stream != area->end(); ++stream) + { + for (auto spot{stream->rbegin()}; spot != stream->rend(); ++spot) + { + Interval range{spot->OffsetBy(-padding)}; + if (range.min <= range.max) + { + range = range.GetIntersection(spot->OffsetBy(spot->velocity * timespan - velocity * timespan)); + if (range.min <= range.max) + { + return {area, stream, std::next(spot).base(), std::move(range)}; + } + } + } + } + } + return { + spots.end(), + spots.back().end(), + spots.back().back().end(), + Interval{ + units::length::meter_t{std::numeric_limits<double>::quiet_NaN()}, + units::length::meter_t{std::numeric_limits<double>::quiet_NaN()} // + } // + }; +} + +DistributedSpawnSpace::DistributedSpawnSpace(const OpenScenarioEngine::v1_3::TrafficDistributions &distributions, std::vector<SpawnZones> &zones) + : SpawnSpace{distributions} +{ + for (const SpawnZones ®ions : zones) + { + for (const SpawnZone &zone : regions) + { + assert(zone.obstacles.size() > 1); + for (auto obstacle{std::next(zone.obstacles.begin())}; obstacle != zone.obstacles.end(); ++obstacle) + { + spots.emplace(*std::prev(obstacle), *obstacle); + } + } + } +} + +ObjectTemplate DistributedSpawnSpace::SampleObjectTemplate(StochasticsInterface &rng) const +{ + const units::length::meter_t max_length{GetMaxIntervalLength()}; + const auto entity_end{FindSpawnableEntityEnd(max_length)}; + if (std::distance(entities.begin(), entity_end) <= 1) + { + return entity_end == entities.begin() ? ObjectTemplate{} : entities.front().handle; + } + assert(weights.size() == entities.size() + 1); + const units::dimensionless::scalar_t max_weight{weights[static_cast<size_t>(std::distance(entities.begin(), entity_end))]}; + const units::dimensionless::scalar_t sampled_weight{rng.GetUniformDistributed(.0, max_weight.value())}; + return Sample(entities, weights, sampled_weight).handle; +} + +units::length::meter_t DistributedSpawnSpace::GetMaxIntervalLength() const +{ + return spots.empty() ? units::length::meter_t{} : GetLength(*spots.rbegin()); +} + +std::vector<Weighted<ObjectTemplate>>::const_iterator DistributedSpawnSpace::FindSpawnableEntityEnd(units::length::meter_t threshold) const +{ + return std::upper_bound(entities.begin(), entities.end(), threshold, [](units::length::meter_t threshold, const Weighted<ObjectTemplate> &entity) + { return threshold < GetLength(entity); }); +} +std::vector<Weighted<ObjectTemplate>>::iterator DistributedSpawnSpace::FindSpawnableEntityEnd(units::length::meter_t threshold) +{ + return std::upper_bound(entities.begin(), entities.end(), threshold, [](units::length::meter_t threshold, const Weighted<ObjectTemplate> &entity) + { return threshold < GetLength(entity); }); +} + +std::set<SpawnSpot, Less<ToLength>>::const_iterator DistributedSpawnSpace::SampleSpot(units::length::meter_t minLength, VelocityRange velocity, StochasticsInterface &rng) const +{ + auto spot{spots.lower_bound(minLength)}; + if (spot == spots.end()) + { + return spots.end(); + } + std::vector<decltype(spot)> candidates; + for (; spot != spots.end(); ++spot) + { + const bool canKeepUp{spot->velocity.Contains(velocity)}; + const bool isEnoughRoomNow{GetLength(*spot) > minLength}; + const bool isEnoughRoomAfter2Seconds{GetLength(*spot) + spot->velocity.GetDifference() * units::time::second_t{2.0} > minLength}; + if (canKeepUp && isEnoughRoomNow && isEnoughRoomAfter2Seconds) + { + candidates.push_back(spot); + } + } + if (candidates.size() <= 1) + { + return candidates.empty() ? spots.end() : *candidates.begin(); + } + ToAccumulatedLength accumulator; + std::vector<units::length::meter_t> lengths{{}}; + Transform(candidates, lengths, [&accumulator](const auto iterator) { // + return accumulator(*iterator); + }); + const units::length::meter_t sampledLength{rng.GetUniformDistributed(.0, lengths.back().value())}; + return Sample(candidates, lengths, sampledLength); +} + +void UpdateSpot(std::vector<OpenScenarioEngine::v1_3::SpawnSpot> &stream, + std::vector<OpenScenarioEngine::v1_3::SpawnSpot>::iterator spot, + const Interval &occupied_space, + units::velocity::meters_per_second_t velocity) +{ + std::vector<OpenScenarioEngine::v1_3::Interval> new_intervals{spot->SplitBy(occupied_space)}; + if (new_intervals.empty()) + { + stream.erase(spot); + } + else + { + std::vector<SpawnSpot> new_spots{Transform(std::move(new_intervals), [spot](Interval &&interval) -> SpawnSpot { // + return {interval, spot->velocity}; + })}; + if (new_spots.front().min < occupied_space.min) + { + new_spots.front().velocity.max = velocity; + } + if (new_spots.back().max > occupied_space.max) + { + new_spots.back().velocity.min = velocity; + } + stream.insert(stream.erase(spot), std::make_move_iterator(new_spots.begin()), std::make_move_iterator(new_spots.end())); + } +} + +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/SpawnSpace.h b/engine/src/Utils/Spawning/SpawnSpace.h new file mode 100644 index 0000000000000000000000000000000000000000..ade5d2826e14777ddcd92841a9d43e860eadc3f4 --- /dev/null +++ b/engine/src/Utils/Spawning/SpawnSpace.h @@ -0,0 +1,189 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include <Stochastics/StochasticsInterface.h> +#include <units.h> + +#include <memory> +#include <set> +#include <utility> +#include <vector> + +#include "Conversion/OscToMantle/ConvertScenarioTrafficDistribution.h" +#include "Iterable.h" +#include "Length.h" +#include "SpawnSpot.h" +#include "SpawnZone.h" +#include "Utils/Geometry.h" +#include "VelocityRange.h" +#include "Weighted.h" + +namespace OpenScenarioEngine::v1_3 +{ +struct SpawnSpace +{ + SpawnSpace(const OpenScenarioEngine::v1_3::TrafficDistributions &); + + std::vector<Weighted<ObjectTemplate>> entities; // Object templates (entity + controllers) sorted by ascending length + std::vector<units::dimensionless::scalar_t> weights; // Used to sample an entity +}; + +/// Space that spawns entities naively & greedily by design as defined here: +/// https://eclipse.dev/openpass/content/html/user_guide/sim_user_guide/components/spawner.html#spawner +/// The algorithm is greedy because it does not restrict what entities it samples from based on the available space. Instead, it +/// samples a vehicle & velocity, then finds the first area it can be spawned in. If no such area exists, spawning is aborted. The +/// algorithm is naive because no memoization of area sizes occurs, meaning all areas are tested anew when another entity is being spawned. +struct SequentialSpawnSpace : SpawnSpace +{ + SequentialSpawnSpace(const std::vector<mantle_api::TrafficArea> &, const OpenScenarioEngine::v1_3::TrafficDistributions &); + + /// Samples and returns a random entity without considering the remaining space. + /// + /// \return Random entity out of this space's entities. If it has none, a nullptr is returned. + ObjectTemplate SampleObjectTemplate(StochasticsInterface &) const; + + /// Tries to spawn an entity and returns whether spawning was successful. + /// If successful, also adjusts the spawn space's intervals. + /// + /// \tparam EntityCreator Provides CreateEntity(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObject> &) + /// \tparam AttachControllers Invocable taking (mantle_api::IEntity&, std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>>) + /// used to attach controllers to the spawned entity + /// \param StochasticsInterface Used to sample the entity and its velocity + template <typename EntityCreator, typename AttachControllers> + bool Spawn(EntityCreator &&, AttachControllers &&, StochasticsInterface &); + + std::tuple<std::vector<std::vector<std::vector<SpawnSpot>>>::iterator, + std::vector<std::vector<SpawnSpot>>::iterator, + std::vector<SpawnSpot>::iterator, + Interval> + FindSpot(const Padding &, units::velocity::meters_per_second_t velocity, units::time::second_t timespan); + + // Stream = vector<SpawnSpot>, thus Area = vector<vector<SpawnSpot>>, thus vector<Area> = vector<vector<vector<SpawnSpot>>> + std::vector<std::vector<std::vector<SpawnSpot>>> spots; + const std::vector<mantle_api::TrafficArea> &areas; +}; + +/// A DistributedSpawnSpace is essentially a self-managing, depleting list of TrafficAreas and weighted entities optimized +/// for the use-case of spawning entities at the rear end of a TrafficArea (with offsets). It restructures the +/// data to be sorted by interval and entity length, allowing efficient filtering of remaining spawnable entities +/// and intervals as the space is filled. +struct DistributedSpawnSpace : SpawnSpace +{ + DistributedSpawnSpace(const OpenScenarioEngine::v1_3::TrafficDistributions &, std::vector<SpawnZones> &); + + /// Returns the entity out of the set of entities in this space based on the given sample + /// + /// \param sample Value within the accumulated weights of each entity of this space + /// \return Blueprint for spawning an entity alongside controllers for that entity + ObjectTemplate GetObjectTemplate(units::dimensionless::scalar_t sample); + + //! Returns a random object template (entity + controllers) using the given number generator + //! + //! \param StochasticsInterface Random number generator used to sample a weight + //! \return Randomly selected object template + ObjectTemplate SampleObjectTemplate(StochasticsInterface &) const; + + std::set<SpawnSpot, Less<ToLength>>::const_iterator SampleSpot(units::length::meter_t min_length, VelocityRange, StochasticsInterface &) const; + + /// Tries to spawn an entity and returns whether spawning was successful. + /// If successful, also adjusts the spawn space's intervals. + /// + /// \tparam EntityCreator Provides CreateEntity(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObject> &) + /// \param StochasticsInterface Used to sample the entity, its position and its velocity + template <typename EntityCreator> + bool Spawn(EntityCreator &&, StochasticsInterface &); + + units::length::meter_t GetMaxIntervalLength() const; + + typename std::vector<Weighted<ObjectTemplate>>::const_iterator FindSpawnableEntityEnd(units::length::meter_t upper_bound) const; + typename std::vector<Weighted<ObjectTemplate>>::iterator FindSpawnableEntityEnd(units::length::meter_t upper_bound); + + std::set<SpawnSpot, Less<ToLength>> spots; +}; + +//! Receives a vector of spawn spots, an iterator to a spot within that vector, an interval contained by that spot and a velocity. +//! Replaces the spot pointed to by the iterator with the disjunction of the spot's interval and the given interval. +//! The velocities of the new spots match the given velocity at the end where they touch the given interval. +//! +//! @param stream Container of spots in which a specific spot will be erased and possibly others inserted +//! @param spot Iterator to a spot that will be replaced +//! @param occupied_space Interval to be cut out of the given spot +//! @param velocity Velocity of the ends of the new inserted spots at the given interval +void UpdateSpot(std::vector<OpenScenarioEngine::v1_3::SpawnSpot> &stream, + std::vector<OpenScenarioEngine::v1_3::SpawnSpot>::iterator spot, + const Interval &occupied_space, + units::velocity::meters_per_second_t velocity); +} // namespace OpenScenarioEngine::v1_3 + +namespace OpenScenarioEngine::v1_3 +{ +template <typename EntityCreator, typename AttachControllers> +bool SequentialSpawnSpace::Spawn(EntityCreator &&spawner, AttachControllers &&attach_controllers, StochasticsInterface &rng) +{ + using namespace units::literals; + + static size_t i{0}; + + if (spots.empty()) + { + return false; + } + ObjectTemplate blueprint{SampleObjectTemplate(rng)}; + if (blueprint == nullptr) + { + return false; + } + const auto velocity_range{GetSpawnVelocityRange(blueprint)}; + auto spawn_velocity{SampleVelocity(velocity_range, rng)}; + const units::time::second_t minimum_time_to_collision{2_s}; + const Padding padding{GetLengthPadding(blueprint)}; + if (auto [spot_area, spot_stream, spot, interval]{FindSpot(padding, spawn_velocity, minimum_time_to_collision)}; spot_area != spots.end()) + { + const auto area{std::next(areas.begin(), std::distance(spots.begin(), spot_area))}; + const auto &stream{*std::next(area->begin(), std::distance(spot_area->begin(), spot_stream))}; + const mantle_api::Pose pose{stream->Convert(mantle_api::ITrafficAreaStream::StreamPose{{interval.max, {}}, {}}).value()}; + std::string name{"Common" + std::to_string(++i)}; + mantle_api::IEntity &object{spawner.CreateEntity(GetEntity(blueprint), name)}; + object.SetName(name); + object.SetPosition(pose.position); + object.SetOrientation(pose.orientation); + object.SetVelocity(util::Rotate(spawn_velocity, pose.orientation)); + attach_controllers(object, blueprint->GetObjectController()); + const auto occupied_space{Interval{interval.max, interval.max}.OffsetBy(-padding.rear, padding.front)}; + UpdateSpot(*spot_stream, spot, occupied_space, spawn_velocity); + return true; + } + return false; +} + +template <typename EntityCreator> +bool DistributedSpawnSpace::Spawn(EntityCreator &&spawner, StochasticsInterface &rng) +{ + ObjectTemplate blueprint{SampleObjectTemplate(rng)}; + if (blueprint == nullptr) + { + return false; + } + const auto velocity_range{GetSpawnVelocityRange(blueprint)}; + const auto spot{SampleSpot(GetLength(blueprint), velocity_range, rng)}; + if (spot == spots.end()) + { + return false; + } + auto spawn_velocity{SampleVelocity(velocity_range, rng)}; + // TODO: Find last spawnable spot on the zone of the sampled interval + // spawner.CreateEntity(*entity, place.zone->GetPosition(distance)); + // TODO: Remove/replace interval + // spot->zone->RemoveInterval(*interval->interval, sampler(restricted_velocity.min, restricted_velocity.max)); + return true; +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/SpawnSpot.h b/engine/src/Utils/Spawning/SpawnSpot.h new file mode 100644 index 0000000000000000000000000000000000000000..b476ba56d9b276e8f3ad35322627e409ed80a520 --- /dev/null +++ b/engine/src/Utils/Spawning/SpawnSpot.h @@ -0,0 +1,39 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include "Interval.h" +#include "Obstacle.h" +#include "VelocityRange.h" + +namespace OpenScenarioEngine::v1_3 +{ +/// A SpawnSpot is an interval that holds necessary information for a SpawnSpace to determine whether an +/// entity can and should be spawned within it. This includes the velocities of the obstacle before and after it +/// and a pointer to the spot's zone so that the SpawnSpace can look for the further spots within that zone. +struct SpawnSpot : Interval +{ + constexpr SpawnSpot(Interval, VelocityRange); + + constexpr SpawnSpot(const Obstacle &before, const Obstacle &after); + + VelocityRange velocity; +}; +} // namespace OpenScenarioEngine::v1_3 + +namespace OpenScenarioEngine::v1_3 +{ +constexpr SpawnSpot::SpawnSpot(Interval interval, VelocityRange velocity) + : Interval{std::move(interval)}, velocity{std::move(velocity)} {} + +constexpr SpawnSpot::SpawnSpot(const Obstacle &before, const Obstacle &after) + : Interval{before.max, after.min}, velocity{before.velocity, after.velocity} {} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/SpawnZone.cpp b/engine/src/Utils/Spawning/SpawnZone.cpp new file mode 100644 index 0000000000000000000000000000000000000000..cba3f7b87b0340aee11f4af772b01bc2132207a1 --- /dev/null +++ b/engine/src/Utils/Spawning/SpawnZone.cpp @@ -0,0 +1,76 @@ +/******************************************************************************** + * Copyright (c) 2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#include "SpawnZone.h" + +#include <algorithm> +#include <cassert> + +#include "Length.h" + +namespace OpenScenarioEngine::v1_3 +{ +SpawnZone::SpawnZone(Interval interval, std::set<Obstacle, Less<ToMin>> obstacles) + : Interval{std::move(interval)}, obstacles{std::move(obstacles)} +{ + this->obstacles.emplace_hint(this->obstacles.begin(), Obstacle::GetStartingSentinel(min)); + this->obstacles.emplace_hint(this->obstacles.end(), Obstacle::GetEndingSentinel(max)); +} + +SpawnZone::SpawnZone(const mantle_api::ITrafficAreaStream& stream) + : SpawnZone{Interval{{}, ToLength{}(stream)}, GetObstacles(stream)} +{ +} + +std::vector<SpawnSpot> SpawnZone::GetSpawnSpots() const +{ + std::vector<SpawnSpot> result; + assert(obstacles.size() > 1); + for (auto obstacle_after{std::next(obstacles.begin())}; obstacle_after != obstacles.end(); ++obstacle_after) + { + const auto obstacle_before{std::prev(obstacle_after)}; + if (obstacle_before->max < obstacle_after->min) + { + result.emplace_back(*obstacle_before, *obstacle_after); + } + } + return result; +} + +SpawnZones::SpawnZones(std::vector<SpawnZone> input) + : Iterable<std::vector<SpawnZone>>{std::move(input)} +{ +} + +SpawnZone ToSpawnZone::operator()(const mantle_api::ITrafficAreaStream& stream) const +{ + return {stream}; +} + +SpawnZone ToSpawnZone::operator()(const std::shared_ptr<mantle_api::ITrafficAreaStream>& stream) const +{ + return (*this)(*stream); +} + +SpawnZones::SpawnZones(const mantle_api::TrafficArea& area) + : SpawnZones{Transform(area, ToSpawnZone{})} +{ +} + +SpawnZones ToSpawnZones::operator()(const mantle_api::TrafficArea& area) const +{ + return {area}; +} + +std::vector<SpawnSpot> ToSpawnSpots::operator()(const SpawnZone& zone) const +{ + return zone.GetSpawnSpots(); +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/SpawnZone.h b/engine/src/Utils/Spawning/SpawnZone.h new file mode 100644 index 0000000000000000000000000000000000000000..f82ebf827b26c6992ba2e2d64a7f904f224509a9 --- /dev/null +++ b/engine/src/Utils/Spawning/SpawnZone.h @@ -0,0 +1,70 @@ +/******************************************************************************** + * Copyright (c) 2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include <MantleAPI/Traffic/i_traffic_area_service.h> +#include <MantleAPI/Traffic/i_traffic_area_stream.h> +#include <units.h> + +#include <optional> +#include <set> +#include <vector> + +#include "Common.h" +#include "Interval.h" +#include "Iterable.h" +#include "SpawnSpot.h" + +namespace OpenScenarioEngine::v1_3 +{ +/// \brief A SpawnZone is a representation of a mantle_api::ITrafficAreaStream with the assumption +/// that said stream consists of connected lanes. It reduces the stream to its length and a full +/// list of obstacles within it. The SpawnZone is used to generate SpawnSpots, which is a list of +/// empty intervals where entities can be spawned. +class SpawnZone : public Interval +{ +public: + SpawnZone(Interval, std::set<Obstacle, Less<ToMin>>); + + SpawnZone(const mantle_api::ITrafficAreaStream&); + + const Interval& GetLongestInterval() const; + Interval& GetLongestInterval(); + + std::vector<SpawnSpot> GetSpawnSpots() const; + + std::set<Obstacle, Less<ToMin>> obstacles; +}; + +struct ToSpawnZone +{ + SpawnZone operator()(const mantle_api::ITrafficAreaStream&) const; + + SpawnZone operator()(const std::shared_ptr<mantle_api::ITrafficAreaStream>&) const; +}; + +struct SpawnZones : Iterable<std::vector<SpawnZone>> +{ + SpawnZones(std::vector<SpawnZone>); + + SpawnZones(const mantle_api::TrafficArea&); +}; + +struct ToSpawnZones +{ + SpawnZones operator()(const mantle_api::TrafficArea&) const; +}; + +struct ToSpawnSpots +{ + std::vector<SpawnSpot> operator()(const SpawnZone&) const; +}; +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/ToEntity.cpp b/engine/src/Utils/Spawning/ToEntity.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6b4a63492e20302ed2ad9ebcfc06678e308ce356 --- /dev/null +++ b/engine/src/Utils/Spawning/ToEntity.cpp @@ -0,0 +1,28 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ +#include "ToEntity.h" + +namespace OpenScenarioEngine::v1_3 +{ +const mantle_api::IEntity& ToEntity::operator()(const std::unique_ptr<mantle_api::IEntity>& input) const +{ + return (*this)(*input); +} + +const mantle_api::IEntity& ToEntity::operator()(const std::shared_ptr<mantle_api::IEntity>& input) const +{ + return (*this)(*input); +} + +const mantle_api::IEntity& ToEntity::operator()(const std::weak_ptr<mantle_api::IEntity>& input) const +{ + return (*this)(input.lock()); +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/ToEntity.h b/engine/src/Utils/Spawning/ToEntity.h new file mode 100644 index 0000000000000000000000000000000000000000..e6def0ba1d2d7c5cb9916998aadd01c9cf036c15 --- /dev/null +++ b/engine/src/Utils/Spawning/ToEntity.h @@ -0,0 +1,38 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ +#include <MantleAPI/Traffic/i_entity.h> + +#include <memory> + +namespace OpenScenarioEngine::v1_3 +{ +struct ToEntity +{ + constexpr const mantle_api::IEntity& operator()(const mantle_api::IEntity&) const; + constexpr const mantle_api::IEntity& operator()(const mantle_api::IEntity*) const; + const mantle_api::IEntity& operator()(const std::unique_ptr<mantle_api::IEntity>&) const; + const mantle_api::IEntity& operator()(const std::shared_ptr<mantle_api::IEntity>&) const; + const mantle_api::IEntity& operator()(const std::weak_ptr<mantle_api::IEntity>&) const; +}; +const ToEntity to_entity; // NOLINT(readability-identifier-naming) +} // namespace OpenScenarioEngine::v1_3 + +namespace OpenScenarioEngine::v1_3 +{ +constexpr const mantle_api::IEntity& ToEntity::operator()(const mantle_api::IEntity& input) const +{ + return input; +} + +constexpr const mantle_api::IEntity& ToEntity::operator()(const mantle_api::IEntity* input) const +{ + return *input; +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Transform.h b/engine/src/Utils/Spawning/Transform.h new file mode 100644 index 0000000000000000000000000000000000000000..975b43169a52fc314c0660a6047f71e482eb51ec --- /dev/null +++ b/engine/src/Utils/Spawning/Transform.h @@ -0,0 +1,63 @@ +/******************************************************************************** + * Copyright (c) 2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include <algorithm> +#include <memory> +#include <type_traits> +#include <vector> + +namespace OpenScenarioEngine::v1_3 +{ +template <typename Invocable, typename Container> +std::vector<std::invoke_result_t<Invocable, typename std::remove_reference_t<Container>::value_type>> Transform(Container&& input, Invocable&& invocable) +{ + std::vector<std::invoke_result_t<Invocable, typename std::remove_reference_t<Container>::value_type>> result; + result.reserve(input.size()); + if constexpr (std::is_lvalue_reference_v<decltype(input)>) + { + std::transform(input.begin(), input.end(), std::back_inserter(result), std::forward<Invocable>(invocable)); + } + else + { + std::transform(std::make_move_iterator(input.begin()), std::make_move_iterator(input.end()), std::back_inserter(result), std::forward<Invocable>(invocable)); + } + return result; +} + +template <typename Invocable, typename Container> +void Transform(Container&& input, std::vector<std::invoke_result_t<Invocable, typename std::remove_reference_t<Container>::value_type>>& output, Invocable&& invocable) +{ + output.reserve(output.size() + input.size()); + if constexpr (std::is_lvalue_reference_v<decltype(input)>) + { + std::transform(input.begin(), input.end(), std::back_inserter(output), std::forward<Invocable>(invocable)); + } + else + { + std::transform(std::make_move_iterator(input.begin()), std::make_move_iterator(input.end()), std::back_inserter(output), std::forward<Invocable>(invocable)); + } +} + +template <typename Type, typename Invocable> +std::vector<std::invoke_result_t<Invocable, const Type&>> TransformSharedPointers( + const std::vector<std::shared_ptr<Type>>& items, Invocable&& invocable) +{ + std::vector<std::invoke_result_t<Invocable, const Type&>> result; + result.reserve(items.size()); + std::transform(items.begin(), + items.end(), + std::back_inserter(result), + [&](const std::shared_ptr<Type>& item) + { return invocable(*item); }); + return result; +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/VelocityRange.cpp b/engine/src/Utils/Spawning/VelocityRange.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5fc6f6855636e8235d887bb6adb6b2983f7951de --- /dev/null +++ b/engine/src/Utils/Spawning/VelocityRange.cpp @@ -0,0 +1,110 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#include "VelocityRange.h" + +#include <openScenarioLib/generated/v1_3/catalog/CatalogHelperV1_3.h> + +#include <exception> +#include <string> + +namespace OpenScenarioEngine::v1_3 +{ +VelocityRange ToSpawnVelocityRange::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IEntityObject &object) const +{ + if (object.IsSetCatalogReference()) + { + return (*this)(object.GetCatalogReference()->GetRef()); + } + if (object.IsSetVehicle()) + { + return (*this)(object.GetVehicle()); + } + if (object.IsSetPedestrian()) + { + return (*this)(object.GetPedestrian()); + } + if (object.IsSetMiscObject()) + { + return (*this)(object.GetMiscObject()); + } + throw std::runtime_error("ToSpawmVelocityRange: Invalid entity. Is neither a vehicle, pedestrian or misc. object"); +} + +VelocityRange ToSpawnVelocityRange::operator()(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ICatalogElement> &input) const +{ + auto &element{const_cast<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ICatalogElement> &>(input)}; + if (NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::IsVehicle(element)) + { + return (*this)(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsVehicle(element)); + } + if (NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::IsPedestrian(element)) + { + return (*this)(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsPedestrian(element)); + } + if (NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::IsMiscObject(element)) + { + return (*this)(NET_ASAM_OPENSCENARIO::v1_3::CatalogHelper::AsMiscObject(element)); + } + throw std::runtime_error("ToSpawnVelocityRange: Invalid catalog reference. Contains no vehicle, pedestrian or misc. object."); +} + +VelocityRange ToSpawnVelocityRange::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate &input) const +{ + return (*this)(input.GetEntitiyObject()); +} + +VelocityRange ToSpawnVelocityRange::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IVehicle &vehicle) const +{ + return (*this)(vehicle.GetProperties()); +} + +VelocityRange ToSpawnVelocityRange::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IPedestrian &pedestrian) const +{ + return (*this)(pedestrian.GetProperties()); +} + +VelocityRange ToSpawnVelocityRange::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IMiscObject &object) const +{ + return (*this)(object.GetProperties()); +} + +VelocityRange ToSpawnVelocityRange::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IProperties &properties) const +{ + const auto &entries{properties.GetProperties()}; + const auto min_match{std::find_if(entries.begin(), + entries.end(), + [](const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IProperty> &entry) + { return entry->GetName() == "min_spawn_velocity"; })}; + if (min_match == entries.end()) + { + throw std::range_error("ToSpawnVelocityRange: 'min_spawn_velocity' not found in properties"); + } + const auto max_match{std::find_if(entries.begin(), + entries.end(), + [](const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IProperty> &entry) + { return entry->GetName() == "max_spawn_velocity"; })}; + if (max_match == entries.end()) + { + throw std::range_error("ToSpawnVelocityRange: 'max_spawn_velocity' not found in properties"); + } + return {units::velocity::meters_per_second_t{std::stod((*min_match)->GetValue())}, units::velocity::meters_per_second_t{std::stod((*max_match)->GetValue())}}; +} + +units::velocity::meters_per_second_t SampleVelocity(const VelocityRange &range, StochasticsInterface &rng) +{ + return units::velocity::meters_per_second_t{rng.GetUniformDistributed(range.min.value(), range.max.value())}; +} + +std::ostream &operator<<(std::ostream &os, const VelocityRange &range) +{ + return os << '[' << range.min.value() << " - " << range.max.value() << ']'; +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/VelocityRange.h b/engine/src/Utils/Spawning/VelocityRange.h new file mode 100644 index 0000000000000000000000000000000000000000..0ac14b0bd439cfd4f3ec21454bab346fa5b8b042 --- /dev/null +++ b/engine/src/Utils/Spawning/VelocityRange.h @@ -0,0 +1,100 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ +#pragma once + +#include <Stochastics/StochasticsInterface.h> +#include <units.h> + +#include <iostream> +#include <memory> + +#include "Interval.h" +#include "openScenarioLib/generated/v1_3/api/ApiClassInterfacesV1_3.h" + +namespace OpenScenarioEngine::v1_3 +{ +struct SpawnZone; + +struct VelocityRange +{ + constexpr bool Contains(const VelocityRange &) const; + + constexpr bool Contains(units::velocity::meters_per_second_t) const; + + constexpr units::velocity::meters_per_second_t GetDifference() const; + + units::velocity::meters_per_second_t min{std::numeric_limits<double>::lowest()}; + units::velocity::meters_per_second_t max{std::numeric_limits<double>::max()}; +}; + +constexpr Interval operator*(const VelocityRange &, units::time::second_t); +constexpr Interval operator*(units::time::second_t, const VelocityRange &); + +struct ToSpawnVelocityRange +{ + template <typename Type> + VelocityRange operator()(const std::shared_ptr<Type> &) const; + + VelocityRange operator()(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ICatalogElement> &) const; + VelocityRange operator()(const NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate &) const; + VelocityRange operator()(const NET_ASAM_OPENSCENARIO::v1_3::IEntityObject &) const; + VelocityRange operator()(const NET_ASAM_OPENSCENARIO::v1_3::IVehicle &) const; + VelocityRange operator()(const NET_ASAM_OPENSCENARIO::v1_3::IPedestrian &) const; + VelocityRange operator()(const NET_ASAM_OPENSCENARIO::v1_3::IMiscObject &) const; + VelocityRange operator()(const NET_ASAM_OPENSCENARIO::v1_3::IProperties &) const; +}; + +template <typename Input> +VelocityRange GetSpawnVelocityRange(Input &&); + +units::velocity::meters_per_second_t SampleVelocity(const VelocityRange &, StochasticsInterface &); + +std::ostream &operator<<(std::ostream &, const VelocityRange &); +} // namespace OpenScenarioEngine::v1_3 + +namespace OpenScenarioEngine::v1_3 +{ +constexpr bool VelocityRange::Contains(const VelocityRange &other) const +{ + return min <= other.min && other.max <= max; +} + +constexpr bool VelocityRange::Contains(units::velocity::meters_per_second_t velocity) const +{ + return min <= velocity && velocity <= max; +} + +constexpr units::velocity::meters_per_second_t VelocityRange::GetDifference() const +{ + return max - min; +} + +constexpr Interval operator*(const VelocityRange &range, units::time::second_t time) +{ + return {range.min * time, range.max * time}; +} + +constexpr Interval operator*(units::time::second_t time, const VelocityRange &range) +{ + return range * time; +} + +template <typename Type> +VelocityRange ToSpawnVelocityRange::operator()(const std::shared_ptr<Type> &input) const +{ + return ToSpawnVelocityRange{}(*input); +} + +template <typename Input> +VelocityRange GetSpawnVelocityRange(Input &&input) +{ + return ToSpawnVelocityRange{}(std::forward<Input>(input)); +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Weight.cpp b/engine/src/Utils/Spawning/Weight.cpp new file mode 100644 index 0000000000000000000000000000000000000000..571157b6a8fbc1d04055210724cfad2988a4e50b --- /dev/null +++ b/engine/src/Utils/Spawning/Weight.cpp @@ -0,0 +1,67 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ +#include "Weight.h" + +namespace OpenScenarioEngine::v1_3 +{ +units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate& object) const +{ + return ToWeight{}(object.GetEntitiyObject()); // https://github.com/RA-Consulting-GmbH/openscenario.api.test/issues/222 +} + +units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::ITrafficDistributionEntry& entry) const +{ + return units::dimensionless::scalar_t{entry.GetWeight()}; +} + +units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IEntityObject& object) const +{ + if (object.IsSetVehicle()) + { + return ToWeight{}(object.GetVehicle()); + } + else if (object.IsSetPedestrian()) + { + return ToWeight{}(object.GetPedestrian()); + } + else if (object.IsSetMiscObject()) + { + return ToWeight{}(object.GetMiscObject()); + } + throw std::runtime_error("ToWeight: Invalid entity. Is neither a vehicle, pedestrian nor a misc. object"); +} + +units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IPedestrian& pedestrian) const +{ + return ToWeight{}(pedestrian.GetProperties()); +} + +units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IVehicle& vehicle) const +{ + return ToWeight{}(vehicle.GetProperties()); +} + +units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IMiscObject& object) const +{ + return ToWeight{}(object.GetProperties()); +} + +units::dimensionless::scalar_t ToWeight::operator()(const NET_ASAM_OPENSCENARIO::v1_3::IProperties& properties) const +{ + const auto& entries{properties.GetProperties()}; + auto match{std::find_if(entries.begin(), entries.end(), [](const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IProperty>& entry) + { return entry->GetName() == "Weight"; })}; + if (match != entries.end()) + { + return units::dimensionless::scalar_t{std::stod((*match)->GetValue())}; + } + throw std::runtime_error("ToWeight: 'Weight' not found in properties"); +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Weight.h b/engine/src/Utils/Spawning/Weight.h new file mode 100644 index 0000000000000000000000000000000000000000..f2ff9cddd543c3b3ad1e88f04112a309eff41aee --- /dev/null +++ b/engine/src/Utils/Spawning/Weight.h @@ -0,0 +1,90 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include <openScenarioLib/generated/v1_3/api/ApiClassInterfacesV1_3.h> +#include <units.h> + +#include <memory> + +#include "Weighted.h" + +namespace OpenScenarioEngine::v1_3 +{ +struct ToWeight +{ + constexpr units::dimensionless::scalar_t operator()(units::dimensionless::scalar_t) const; + + template <typename Type> + units::dimensionless::scalar_t operator()(const std::shared_ptr<Type>&) const; + + units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate&) const; + + units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::ITrafficDistributionEntry&) const; + + units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IEntityObject&) const; + + units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IPedestrian&) const; + + units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IVehicle&) const; + + units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IMiscObject&) const; + + units::dimensionless::scalar_t operator()(const NET_ASAM_OPENSCENARIO::v1_3::IProperties&) const; + + template <typename Type> + units::dimensionless::scalar_t operator()(const Weighted<Type>&) const; +}; + +template <typename Input> +units::dimensionless::scalar_t GetWeight(Input&&); + +struct ToAccumulatedWeight +{ + template <typename Entry> + units::dimensionless::scalar_t operator()(const Entry&); + + units::dimensionless::scalar_t totalWeight; +}; +} // namespace OpenScenarioEngine::v1_3 + +namespace OpenScenarioEngine::v1_3 +{ +constexpr units::dimensionless::scalar_t ToWeight::operator()(units::dimensionless::scalar_t value) const +{ + return value; +} + +template <typename Type> +units::dimensionless::scalar_t ToWeight::operator()(const std::shared_ptr<Type>& input) const +{ + return ToWeight{}(*input); +} + +template <typename Type> +units::dimensionless::scalar_t ToWeight::operator()(const Weighted<Type>& input) const +{ + return input.weight; +} + +template <typename Input> +units::dimensionless::scalar_t GetWeight(Input&& input) +{ + return ToWeight{}(std::forward<Input>(input)); +} + +template <typename Entry> +units::dimensionless::scalar_t ToAccumulatedWeight::operator()(const Entry& entry) +{ + totalWeight += GetWeight(entry); + return totalWeight; +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Weighted.cpp b/engine/src/Utils/Spawning/Weighted.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2702aecb4a936faf776ddbc01b74f089e7be002f --- /dev/null +++ b/engine/src/Utils/Spawning/Weighted.cpp @@ -0,0 +1,23 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ +#include "Weighted.h" + +namespace OpenScenarioEngine::v1_3 +{ +std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>> GetControllers(const NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate& handle) +{ + return handle.GetObjectController(); +} + +std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IEntityObject> GetEntity(const NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate& handle) +{ + return handle.GetEntitiyObject(); +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/src/Utils/Spawning/Weighted.h b/engine/src/Utils/Spawning/Weighted.h new file mode 100644 index 0000000000000000000000000000000000000000..b9288b4d755fcb185f324c94a2cec6c1d1db3631 --- /dev/null +++ b/engine/src/Utils/Spawning/Weighted.h @@ -0,0 +1,65 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ +#pragma once + +#include <openScenarioLib/generated/v1_3/api/ApiClassInterfacesV1_3.h> +#include <units.h> + +#include <memory> + +namespace OpenScenarioEngine::v1_3 +{ +//! \brief Shared point of a scenario object template, which is an entity paired with an arbitrary number of controllers +using ObjectTemplate = typename std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate>; + +template <typename Type> +struct Weighted +{ + constexpr operator const Type&() const; + constexpr operator Type&(); + + Type handle; + units::dimensionless::scalar_t weight; +}; + +template <typename Type> +std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>> GetControllers(const std::shared_ptr<Type>&); + +std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>> GetControllers(const NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate&); + +template <typename Type> +std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IEntityObject> GetEntity(const std::shared_ptr<Type>&); + +std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IEntityObject> GetEntity(const NET_ASAM_OPENSCENARIO::v1_3::IScenarioObjectTemplate&); + +template <typename Type> +constexpr Weighted<Type>::operator const Type&() const +{ + return (handle); +} + +template <typename Type> +constexpr Weighted<Type>::operator Type&() +{ + return (handle); +} + +template <typename Type> +std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IObjectController>> GetControllers(const std::shared_ptr<Type>& input) +{ + return GetControllers(*input); +} + +template <typename Type> +std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IEntityObject> GetEntity(const std::shared_ptr<Type>& input) +{ + return GetEntity(*input); +} +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/tests/Storyboard/GenericAction/ActivateControllerActionTest.cpp b/engine/tests/Storyboard/GenericAction/ActivateControllerActionTest.cpp index d8f36438c199f7772857bd0cb570b74e42b30714..389313cc3cef79444bf85f932f8205411fda2de0 100644 --- a/engine/tests/Storyboard/GenericAction/ActivateControllerActionTest.cpp +++ b/engine/tests/Storyboard/GenericAction/ActivateControllerActionTest.cpp @@ -14,7 +14,7 @@ #include <gtest/gtest.h> #include "MantleAPI/Common/i_identifiable.h" -#include "Storyboard/GenericAction/ActivateControllerAction_impl.h" +#include "Storyboard/GenericAction/ActivateControllerAction.h" #include "TestUtils/MockControllerService.h" #include "gmock/gmock.h" diff --git a/engine/tests/Storyboard/GenericAction/TrafficAreaActionTest.cpp b/engine/tests/Storyboard/GenericAction/TrafficAreaActionTest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..32713524c16e6a3d922e9e5dfa484fa2c705e2c0 --- /dev/null +++ b/engine/tests/Storyboard/GenericAction/TrafficAreaActionTest.cpp @@ -0,0 +1,135 @@ +/******************************************************************************** + * Copyright (c) 2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#include <MantleAPI/Test/test_utils.h> +#include <Storyboard/GenericAction/TrafficAreaAction.h> +#include <gmock/gmock.h> +#include <gtest/gtest.h> +#include <units.h> + +#include <memory> +#include <string> +#include <utility> +#include <vector> + +#include "OpenScenarioEngine/OpenScenarioEngine.h" +#include "TestUtils.h" +#include "TestUtils/TestLogger.h" + +using testing::_; +using testing::HasSubstr; +using testing::NiceMock; +using testing::Return; +using testing::ReturnRef; +using testing::OpenScenarioEngine::v1_3::GetScenariosPath; +using testing::OpenScenarioEngine::v1_3::OpenScenarioEngineTestBase; +using namespace units::literals; + +struct MockVehicle : mantle_api::IVehicle +{ + mantle_api::UniqueId GetUniqueId() const override { return {}; } + + void SetName(const std::string& name) override {} + const std::string& GetName() const override + { + static std::string s; + return s; + } + + void SetPosition(const mantle_api::Vec3<units::length::meter_t>& xyz) override { this->xyz = xyz; }; + mantle_api::Vec3<units::length::meter_t> GetPosition() const override { return {}; } + + void SetVelocity(const mantle_api::Vec3<units::velocity::meters_per_second_t>&) override {} + mantle_api::Vec3<units::velocity::meters_per_second_t> GetVelocity() const override { return {}; } + + void SetAcceleration(const mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>&) override {} + mantle_api::Vec3<units::acceleration::meters_per_second_squared_t> GetAcceleration() const override { return {}; } + + void SetOrientation(const mantle_api::Orientation3<units::angle::radian_t>& orientation) override { this->orientation = orientation; } + mantle_api::Orientation3<units::angle::radian_t> GetOrientation() const override { return {}; } + + void SetOrientationRate(const mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>&) override {} + mantle_api::Orientation3<units::angular_velocity::radians_per_second_t> GetOrientationRate() const override { return {}; } + + void SetOrientationAcceleration(const mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>&) override {} + mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t> GetOrientationAcceleration() const override { return {}; } + + void SetProperties(std::unique_ptr<mantle_api::EntityProperties>) override {} + mantle_api::VehicleProperties* GetProperties() const override { return {}; } + + void SetAssignedLaneIds(const std::vector<std::uint64_t>&) override {} + std::vector<std::uint64_t> GetAssignedLaneIds() const override { return {}; } + + void SetVisibility(const mantle_api::EntityVisibilityConfig& visibility) override {} + mantle_api::EntityVisibilityConfig GetVisibility() const override { return {}; } + + void SetIndicatorState(mantle_api::IndicatorState state) override {} + mantle_api::IndicatorState GetIndicatorState() const override { return {}; } + + void SetSteeringWheelAngle(units::angle::radian_t) override{}; + units::angle::radian_t GetSteeringWheelAngle() const override { return {}; } + + mantle_api::Vec3<units::length::meter_t> xyz; + mantle_api::Orientation3<units::angle::radian_t> orientation; +}; + +class TrafficAreaActionTestFixture : public OpenScenarioEngineTestBase +{ +protected: + void SetUp() override + { + auto& stream{dynamic_cast<mantle_api::MockTrafficAreaStream&>(*streams.emplace_back(std::make_shared<mantle_api::MockTrafficAreaStream>()))}; + OpenScenarioEngineTestBase::SetUp(); + ON_CALL(controller_, GetName()).WillByDefault(ReturnRef(controller_name)); + ON_CALL(controller_, GetUniqueId()).WillByDefault(Return(1234)); + LOGGER = std::make_unique<testing::OpenScenarioEngine::v1_3::TestLogger>(); + auto& mockTrafficAreaService{dynamic_cast<mantle_api::MockTrafficAreaService&>(env_->GetTrafficAreaService())}; + + ON_CALL(stream, GetLength()).WillByDefault(Return(500_m)); + ON_CALL(stream, Convert(testing::A<const mantle_api::ITrafficAreaStream::StreamPose&>())).WillByDefault([](const mantle_api::ITrafficAreaStream::StreamPose& pose) { // clang-format off + return mantle_api::Pose{mantle_api::Vec3<units::length::meter_t>{pose.s, pose.t, 0_m}, mantle_api::Orientation3<units::angle::radian_t>{}}; + }); + ON_CALL(mockTrafficAreaService, CreateTrafficArea(_)).WillByDefault([this]() -> std::vector<std::shared_ptr<mantle_api::ITrafficAreaStream>> { // + return streams; + }); // clang-format on + auto& entity_repository{static_cast<mantle_api::MockEntityRepository&>(env_->GetEntityRepository())}; // clang-format off + ON_CALL(entity_repository, Create(testing::A<const std::string&>(), testing::A<const mantle_api::VehicleProperties&>())).WillByDefault([this](const std::string&, const mantle_api::VehicleProperties&) -> NiceMock<MockVehicle>& { + return mock_vehicles.emplace_back(); + }); // clang-format on + } + + std::vector<std::shared_ptr<mantle_api::ITrafficAreaStream>> streams; + std::shared_ptr<testing::OpenScenarioEngine::v1_3::TestLogger> LOGGER; + + std::vector<NiceMock<MockVehicle>> mock_vehicles; + + std::string controller_name{"TestController"}; +}; + +TEST_F(TrafficAreaActionTestFixture, GivenScenarioWithTrafficAreaAction_WhenInitScenario_Then) +{ + std::string xosc_file_path{GetScenariosPath(test_info_->file()) + "scenario_with_traffic_area_action.xosc"}; + OpenScenarioEngine::v1_3::OpenScenarioEngine engine(xosc_file_path, env_); + engine.Init(); + engine.SetupDynamicContent(); + engine.Step({}); + const size_t offset{1}; // SetupDynamicContent spawns one entity, which we want to ignore (env_->GetEntityRepository().GetEntities().size() does not work) + // The initial area is 500m long. The vehicle is 5_m long. + // The last place its center can be placed at is 500 - 5/2 = 497.5 + EXPECT_EQ(mock_vehicles[0 + offset].xyz.x, 497.5_m); + // The prior vehicle starts at 497.5 - 5/2 = 495 + // The last place the next vehicle's center can be placed at is 495 - 5/2 = 492.5 + // If its randomly sampled velocity is greater than that of the prior vehicle, it may be placed even further back + EXPECT_LE(mock_vehicles[1 + offset].xyz.x, 492.5_m); + for (auto vehicle{std::next(mock_vehicles.begin(), offset + 1)}; vehicle < mock_vehicles.end(); ++vehicle) + { + EXPECT_LE(vehicle->xyz.x, std::prev(vehicle)->xyz.x - 5_m); + } +} diff --git a/engine/tests/Storyboard/GenericAction/data/Scenarios/RoadNetwork.xodr b/engine/tests/Storyboard/GenericAction/data/Scenarios/RoadNetwork.xodr new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/engine/tests/Storyboard/GenericAction/data/Scenarios/VehicleCatalog.xosc b/engine/tests/Storyboard/GenericAction/data/Scenarios/VehicleCatalog.xosc new file mode 100644 index 0000000000000000000000000000000000000000..bc7c5a51660afec733f5a1dd13384b70e496aeab --- /dev/null +++ b/engine/tests/Storyboard/GenericAction/data/Scenarios/VehicleCatalog.xosc @@ -0,0 +1,22 @@ +<?xml version="1.0" encoding="utf-8"?> +<OpenSCENARIO> + <FileHeader revMajor="1" revMinor="1" date="2025-02-26T00:00:00" author="BMW AG" description="Vehicle catalog" /> + <Catalog name="VehicleCatalog"> + <Vehicle name="my_car" vehicleCategory="car" model3d="medium_car"> + <BoundingBox> + <Center x="1.4" y="0.0" z="0.9" /> + <Dimensions width="2.0" length="5.0" height="1.8" /> + </BoundingBox> + <Performance maxSpeed="70" maxDeceleration="10" maxAcceleration="10" /> + <Axles> + <FrontAxle maxSteering="0.5" wheelDiameter="0.8" trackWidth="1.68" positionX="2.98" positionZ="0.4" /> + <RearAxle maxSteering="0" wheelDiameter="0.8" trackWidth="1.68" positionX="0" positionZ="0.4" /> + </Axles> + <Properties> + <Property name="custom_property" value="5" /> + <Property name="min_spawn_velocity" value="2" /> + <Property name="max_spawn_velocity" value="8" /> + </Properties> + </Vehicle> + </Catalog> +</OpenSCENARIO> diff --git a/engine/tests/Storyboard/GenericAction/data/Scenarios/scenario_with_traffic_area_action.xosc b/engine/tests/Storyboard/GenericAction/data/Scenarios/scenario_with_traffic_area_action.xosc new file mode 100644 index 0000000000000000000000000000000000000000..191987e1367d8569e6879dea97030f1ecb645b43 --- /dev/null +++ b/engine/tests/Storyboard/GenericAction/data/Scenarios/scenario_with_traffic_area_action.xosc @@ -0,0 +1,55 @@ +<?xml version='1.0' encoding='UTF-8'?> +<OpenSCENARIO> + <FileHeader revMajor="1" revMinor="3" date="2025-02-26T00:00:00" description="TrafficAreaAction Test" author="BMW AG" /> + <CatalogLocations> + <VehicleCatalog> + <Directory path="." /> + </VehicleCatalog> + </CatalogLocations> + <RoadNetwork> + <LogicFile filepath="RoadNetwork.xodr" /> + <SceneGraphFile filepath="" /> + </RoadNetwork> + <Entities> + <ScenarioObject name="Ego"> + <CatalogReference catalogName="VehicleCatalog" entryName="my_car" /> + <ObjectController> + <Controller name="Ego"> + <Properties> + <Property name="AgentProfile" value="MiddleClassCarAgent" /> + </Properties> + </Controller> + </ObjectController> + </ScenarioObject> + </Entities> + <Storyboard> + <Init> + <Actions> + <GlobalAction> + <TrafficAction> + <!-- https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/TrafficAreaAction.html --> + <TrafficAreaAction continuous="false" numberOfEntities="100"> + <TrafficDistribution> + <TrafficDistributionEntry weight="1.0"> + <EntityDistribution> + <EntityDistributionEntry weight="1.0"> + <ScenarioObjectTemplate> + <CatalogReference catalogName="VehicleCatalog" entryName="my_car" /> + </ScenarioObjectTemplate> + </EntityDistributionEntry> + </EntityDistribution> + </TrafficDistributionEntry> + </TrafficDistribution> + <TrafficArea> + <RoadRange> + <RoadCursor roadId="DOES_NOT_EXIST" s="100" /> + <RoadCursor roadId="DOES_NOT_EXIST" s="500" /> + </RoadRange> + </TrafficArea> + </TrafficAreaAction> + </TrafficAction> + </GlobalAction> + </Actions> + </Init> + </Storyboard> +</OpenSCENARIO> \ No newline at end of file diff --git a/engine/tests/Utils/ControllerCreatorTest.cpp b/engine/tests/Utils/ControllerCreatorTest.cpp index 2b5bc9ec6f0ec7580669809fff64dde05c4679d0..93f1183021f406d01f51427843209c17300e6c06 100644 --- a/engine/tests/Utils/ControllerCreatorTest.cpp +++ b/engine/tests/Utils/ControllerCreatorTest.cpp @@ -42,11 +42,11 @@ public: mockController{} { ON_CALL(mockController, GetName()).WillByDefault(ReturnRef(TEST_CONTROLLER_NAME)); - ON_CALL(mockController, GetUniqueId()).WillByDefault(testing::Invoke([] { + ON_CALL(mockController, GetUniqueId()).WillByDefault(testing::Invoke([] + { // id will be incremented at every call, starting with 1000 static mantle_api::UniqueId id{1000}; - return id++; - })); + return id++; })); } void SET_VEHICLE_MOCK_NAME_AND_ID(const std::string& name, mantle_api::UniqueId id) @@ -168,7 +168,7 @@ TEST_F(ControllerCreator, GivenScenarioObjectWithController_WhenCreateIsCalled_T controller_creator.CreateControllers(scenario_objects); } -TEST_F(ControllerCreator, GivenScenarioObjectWithController_WhenCreateIsCalled_ThenControllerServiceContainsEntitiyControllers) +TEST_F(ControllerCreator, GivenScenarioObjectWithController_WhenCreateIsCalled_ThenControllerServiceContainsEntityControllers) { constexpr mantle_api::UniqueId ENTITY_ID = 1234; SETUP_ENTITY("test_entity", ENTITY_ID, true, true); diff --git a/engine/tests/data/Scenarios/scenario_with_traffic_area_action.xosc b/engine/tests/data/Scenarios/scenario_with_traffic_area_action.xosc new file mode 100644 index 0000000000000000000000000000000000000000..f83d832af2db5954f03e61ed34facf764d9c484c --- /dev/null +++ b/engine/tests/data/Scenarios/scenario_with_traffic_area_action.xosc @@ -0,0 +1,117 @@ +<?xml version='1.0' encoding='UTF-8'?> +<OpenSCENARIO> + <FileHeader revMajor="1" revMinor="3" date="2024-12-06T10:00:00" description="TrafficAreaAction Test" author="BMW AG" /> + <CatalogLocations> + <VehicleCatalog> + <Directory path="Vehicles" /> + </VehicleCatalog> + <PedestrianCatalog> + <Directory path="Vehicles" /> + </PedestrianCatalog> + </CatalogLocations> + <RoadNetwork> + <LogicFile filepath="SceneryConfiguration.xodr" /> + <SceneGraphFile filepath="" /> + </RoadNetwork> + <Entities> + <ScenarioObject name="Ego"> + <CatalogReference catalogName="VehicleCatalog" entryName="car_mini_cooper" /> + <ObjectController name="Ego"> + <Controller name="Ego"> + <Properties> + <Property name="AgentProfile" value="MiddleClassCarAgent" /> + </Properties> + </Controller> + </ObjectController> + </ScenarioObject> + </Entities> + <Storyboard> + <Init> + <Actions> + <GlobalAction> + <TrafficAction> + <!-- + https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/TrafficAreaAction.html --> + <TrafficAreaAction continuous="false" numberOfEntities="100"> + <TrafficDistribution> + <TrafficDistributionEntry weight="1.0"> + <EntityDistribution> + <EntityDistributionEntry weight="1.0"> + <ScenarioObjectTemplate> + <CatalogReference catalogName="VehicleCatalog" entryName="car_mini_cooper" /> + <ObjectController> + <Controller name="Common"> + <Properties> + <Property name="AgentProfile" value="MiddleClassCarAgent" /> + </Properties> + </Controller> + </ObjectController> + </ScenarioObjectTemplate> + </EntityDistributionEntry> + </EntityDistribution> + </TrafficDistributionEntry> + </TrafficDistribution> + <TrafficArea> + <RoadRange> + <RoadCursor roadId="road00" s="100" /> + <RoadCursor roadId="road00" s="500" /> + </RoadRange> + </TrafficArea> + </TrafficAreaAction> + </TrafficAction> + </GlobalAction> + <Private entityRef="Ego"> + <PrivateAction> + <TeleportAction> + <Position> + <LanePosition roadId="1" laneId="-1" offset="0.0" s="0.0"> + <Orientation type="relative" /> + </LanePosition> + </Position> + </TeleportAction> + </PrivateAction> + <PrivateAction> + <LongitudinalAction> + <SpeedAction> + <SpeedActionDynamics dynamicsShape="step" value="0.0" dynamicsDimension="rate" /> + <SpeedActionTarget> + <AbsoluteTargetSpeed value="43.5" /> + </SpeedActionTarget> + </SpeedAction> + </LongitudinalAction> + </PrivateAction> + <PrivateAction> + <ControllerAction> + <ActivateControllerAction objectControllerRef="Ego" lateral="true" longitudinal="true" /> + </ControllerAction> + </PrivateAction> + </Private> + </Actions> + </Init> + <Story name=""> + <Act name=""> + <ManeuverGroup name="" maximumExecutionCount="1"> + <Actors selectTriggeringEntities="false" /> + </ManeuverGroup> + <StartTrigger> + <ConditionGroup> + <Condition name="StartTime" delay="0" conditionEdge="rising"> + <ByValueCondition> + <SimulationTimeCondition value="0.0" rule="greaterOrEqual" /> + </ByValueCondition> + </Condition> + </ConditionGroup> + </StartTrigger> + </Act> + </Story> + <StopTrigger> + <ConditionGroup> + <Condition name="EndTime" delay="0" conditionEdge="rising"> + <ByValueCondition> + <SimulationTimeCondition value="30.0" rule="greaterThan" /> + </ByValueCondition> + </Condition> + </ConditionGroup> + </StopTrigger> + </Storyboard> +</OpenSCENARIO> \ No newline at end of file diff --git a/generator/model/v1_3.json b/generator/model/v1_3.json index dc20d2c9d05f4051854dd24c8b51b8d92b6e8cd3..e74e23b4701415e0faa8731cd24fe9398f696aa6 100755 --- a/generator/model/v1_3.json +++ b/generator/model/v1_3.json @@ -10545,7 +10545,7 @@ "ScenarioObjectTemplate": { "isModelGroupChoice": false, "properties": { - "entitiyObject": { + "entityObject": { "isSimpleContentProperty": false, "isList": false, "isOptional": false, diff --git a/utils/ci/conan/conanfile.txt b/utils/ci/conan/conanfile.txt index 849dff888b3b01f202330e1ca15a313e30145083..f1302c20d1783939b52996f631c15a5f121a323f 100644 --- a/utils/ci/conan/conanfile.txt +++ b/utils/ci/conan/conanfile.txt @@ -1,9 +1,9 @@ [requires] -units/2.3.4@openscenarioengine/testing mantleapi/v11.0.0@openscenarioengine/testing -yase/d0c0e58d17358044cc9018c74308b45f6097ecfb@openscenarioengine/testing -openscenario_api/v1.4.0@openscenarioengine/testing +openscenario_api/v1.4.1@openscenarioengine/testing stochastics/0.11.0@openscenarioengine/testing +units/2.3.4@openscenarioengine/testing +yase/d0c0e58d17358044cc9018c74308b45f6097ecfb@openscenarioengine/testing [options] diff --git a/utils/ci/conan/recipe/openscenario_api/all/conandata.yml b/utils/ci/conan/recipe/openscenario_api/all/conandata.yml index bc5c5031be72c4403d16526ba3928f6a174a5a3e..7a09463a60546ec3cc72ca216c1b9ec9f7d63f6f 100644 --- a/utils/ci/conan/recipe/openscenario_api/all/conandata.yml +++ b/utils/ci/conan/recipe/openscenario_api/all/conandata.yml @@ -13,6 +13,9 @@ ################################################################################ sources: + "v1.4.1": + url: https://github.com/RA-Consulting-GmbH/openscenario.api.test.git + sha256: "a72820b00445714071d0e457afcc2112d6015c94" "v1.4.0": url: https://github.com/RA-Consulting-GmbH/openscenario.api.test.git sha256: "5980e8806216351fc2d749c4bfa8669367531688" diff --git a/utils/ci/conan/recipe/openscenario_api/all/conanfile.py b/utils/ci/conan/recipe/openscenario_api/all/conanfile.py index dbe00a8fb5e9a68476fcb66294cea2869e0c7c56..258f1a8990f21e87d582ac906818dce56140bd05 100644 --- a/utils/ci/conan/recipe/openscenario_api/all/conanfile.py +++ b/utils/ci/conan/recipe/openscenario_api/all/conanfile.py @@ -1,5 +1,5 @@ ################################################################################ -# Copyright (c) 2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) +# Copyright (c) 2023-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) # 2024 Volkswagen AG # # This program and the accompanying materials are made available under the @@ -15,7 +15,7 @@ import os from conan import ConanFile -from conan.tools.files import copy, export_conandata_patches, apply_conandata_patches +from conan.tools.files import apply_conandata_patches, copy, export_conandata_patches, replace_in_file from conan.tools.scm import Git required_conan_version = ">=1.53.0" @@ -56,7 +56,6 @@ class OpenScenarioApiConan(ConanFile): self._artifact_path = os.path.join(self._repo_source, "cpp", "buildArtifact") apply_conandata_patches(self) - def build(self): if self.settings.os == "Windows": os.chdir(os.path.join(self._repo_source, "cpp")) diff --git a/utils/ci/conan/recipe/openscenario_api/config.yml b/utils/ci/conan/recipe/openscenario_api/config.yml index 2e807a23b77bd40078c10bbcc7024e9a00821e68..1dbb91ddabd9ab982b094dd698272319af88e127 100644 --- a/utils/ci/conan/recipe/openscenario_api/config.yml +++ b/utils/ci/conan/recipe/openscenario_api/config.yml @@ -21,3 +21,6 @@ versions: "v1.4.0": folder: "all" + + "v1.4.1": + folder: "all" diff --git a/utils/ci/scripts/15_prepare_thirdParty.sh b/utils/ci/scripts/15_prepare_thirdParty.sh index 05617eb1f293a98cef16601b200e382b42d1b34a..1273e1c3e67f97599aa9a19a8433a1b094c11c9e 100755 --- a/utils/ci/scripts/15_prepare_thirdParty.sh +++ b/utils/ci/scripts/15_prepare_thirdParty.sh @@ -148,4 +148,4 @@ done # Command to install all the packages into the required folder. # --build=missing argument is necessary as at this point protobuf is checked and if not available it builds from conancenter -"$PYTHON_COMMAND" -m conans.conan install $file --build=missing --deployer=direct_deploy -of="$REPO_ROOT/../deps" -pr:a "$conanprofile" || exit 1 \ No newline at end of file +"$PYTHON_COMMAND" -m conans.conan install $file --build=missing --deployer=direct_deploy -of="$REPO_ROOT/deps" -pr:a "$conanprofile" || exit 1 \ No newline at end of file