Skip to content
Snippets Groups Projects
Commit cc2c129d authored by Andreas Rauschert's avatar Andreas Rauschert
Browse files

Integrate MantleAPI v12

- Reimplement deleted Mocks that return other mocks on e.g.
GetEntityRepository() in test/TestUtils.h as Fake classes
- Replace mocks with fake classes in tests
- fix wrong usage of (local) LaneIds and (global) UniqueId for lanes in
LateralDistanceAction and ConvertScenarioAbsoluteTargetLaneTest
- adjust to new getter for entities (weak ptr instead of optional
reference wrapper)
- entities in the repo are now shared ptr instead of unique ptrs (for
tests)
- adjust to new route handling (removed AssignRoute(), add Create() in
route repository
and SetRoute() in entity)
- adjust to renaming of members in route definition
- adjust to redefinition of polynomial in SplineSection (struct instead
of tuple)
- fix namespace for MockStatistics
- add demo as bazel target and fix compilation
parent d430f376
No related branches found
No related tags found
1 merge request!238Integrate MantleAPI v17
Pipeline #74502 passed
Showing
with 147 additions and 120 deletions
......@@ -269,7 +269,7 @@ For more information on how to define these properties, refer to the documentati
| [CPM](https://github.com/cpm-cmake/CPM.cmake) | 03705fc | 0.36.0 | MIT License |
| [googletest](https://github.com/google/googletest) | f8d7d77 | 1.14.0 | BSD-3-Clause License |
| [JSON for Modern C++](https://github.com/nlohmann/json) | db78ac1 | 3.9.1 | MIT License |
| [MantleAPI](https://gitlab.eclipse.org/eclipse/openpass/mantle-api) | 5541bf1a | 14.0.0 | EPL 2.0 |
| [MantleAPI](https://gitlab.eclipse.org/eclipse/openpass/mantle-api) | fa2c771a | 17.0.0 | EPL 2.0 |
| [openpass/stochastics-library](https://gitlab.eclipse.org/eclipse/openpass/stochastics-library) | 6c9dde71 | 0.11.0 | EPL 2.0 |
| [OpenSCENARIO API](https://github.com/RA-Consulting-GmbH/openscenario.api.test/) | 5980e88 | 1.4.0 | Apache 2.0 |
| [Units](https://github.com/nholthaus/units) | e27eed9 | 2.3.4 | MIT License |
......
......@@ -37,6 +37,18 @@ cc_library(
],
)
cc_binary(
name = "demo",
srcs = glob([
"demo/main.cpp",
]),
deps = [
":open_scenario_engine",
":open_scenario_engine_test_utils",
"@googletest//:gtest_main",
],
)
filegroup(
name = "scenario_environment_test_data",
srcs = [
......@@ -109,6 +121,7 @@ cc_test(
tags = ["test"],
deps = [
":open_scenario_engine",
":open_scenario_engine_test_utils",
":test_utils",
"@googletest//:gtest_main",
"@mantle_api//:test_utils",
......@@ -123,6 +136,7 @@ cc_test(
tags = ["test"],
deps = [
":open_scenario_engine",
":open_scenario_engine_test_utils",
"@googletest//:gtest_main",
"@mantle_api//:test_utils",
],
......
/********************************************************************************
* 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
......@@ -9,8 +9,6 @@
********************************************************************************/
#include <MantleAPI/Common/i_logger.h>
#include <MantleAPI/Common/log_utils.h>
#include <MantleAPI/Test/test_utils.h>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
......@@ -19,6 +17,7 @@
#include <string>
#include "OpenScenarioEngine/OpenScenarioEngine.h"
#include "tests/TestUtils.h"
using namespace units::literals;
......@@ -62,42 +61,45 @@ int main(int argc, char* argv[])
return -1;
}
auto mockEnvironment = std::make_shared<mantle_api::MockEnvironment>();
auto fake_environment = std::make_shared<testing::OpenScenarioEngine::v1_3::FakeEnvironment>();
auto& mockEntityRepository = static_cast<mantle_api::MockEntityRepository&>(mockEnvironment->GetEntityRepository());
auto& mockEntityRepository = static_cast<mantle_api::MockEntityRepository&>(fake_environment->GetEntityRepository());
mantle_api::MockVehicle mockVehicle1;
ON_CALL(mockEntityRepository, Create("Vehicle1", A<const mantle_api::VehicleProperties&>())).WillByDefault(ReturnRef(mockVehicle1));
auto mock_vehicle_1 = std::make_shared<mantle_api::MockVehicle>();
ON_CALL(mockEntityRepository, Create("Vehicle1", A<const mantle_api::VehicleProperties&>())).WillByDefault(Return(mock_vehicle_1));
mantle_api::MockVehicle mockVehicle2;
ON_CALL(mockEntityRepository, Create("Vehicle2", A<const mantle_api::VehicleProperties&>())).WillByDefault(ReturnRef(mockVehicle2));
auto mock_vehicle_2 = std::make_shared<mantle_api::MockVehicle>();
ON_CALL(mockEntityRepository, Create("Vehicle2", A<const mantle_api::VehicleProperties&>())).WillByDefault(Return(mock_vehicle_2));
auto& mockControllerRepository = mockEnvironment->GetControllerRepository();
auto& mockControllerRepository = fake_environment->GetControllerRepository();
mantle_api::MockController mockController;
auto mockController = std::make_shared<mantle_api::MockController>();
auto controller_name("demo_controller"s);
ON_CALL(mockControllerRepository, Create(_)).WillByDefault(ReturnRef(mockController));
ON_CALL(mockController, GetName()).WillByDefault(ReturnRef(controller_name));
ON_CALL(mockController, GetUniqueId()).WillByDefault(Return(1234));
ON_CALL(mockControllerRepository, Create(_)).WillByDefault(Return(mockController));
ON_CALL(*mockController, GetName()).WillByDefault(ReturnRef(controller_name));
ON_CALL(*mockController, GetUniqueId()).WillByDefault(Return(mantle_api::UniqueId(1234)));
OpenScenarioEngine::v1_3::OpenScenarioEngine openScenarioEngine(scenario_file.string(), mockEnvironment, consoleLogger);
OpenScenarioEngine::v1_3::OpenScenarioEngine openScenarioEngine(scenario_file.string(), fake_environment, consoleLogger);
mantle_api::Time current_time{0_s};
ON_CALL(*mockEnvironment, GetSimulationTime())
.WillByDefault(Invoke([&current_time]() { return current_time; }));
ON_CALL(*mockEnvironment, HasControlStrategyGoalBeenReached(_, mantle_api::ControlStrategyType::kFollowVelocitySpline))
.WillByDefault(Invoke([&current_time]() -> bool { return current_time > 5.5_s; }));
ON_CALL(*fake_environment, GetSimulationTime())
.WillByDefault(Invoke([&current_time]()
{ return current_time; }));
ON_CALL(*fake_environment, HasControlStrategyGoalBeenReached(_, mantle_api::ControlStrategyType::kFollowVelocitySpline))
.WillByDefault(Invoke([&current_time]() -> bool
{ return current_time > 5.5_s; }));
try
{
openScenarioEngine.Init();
openScenarioEngine.Step(0_ms);
while (!openScenarioEngine.IsFinished())
{
openScenarioEngine.Step();
openScenarioEngine.Step(500_ms);
current_time += 500_ms;
}
}
catch(const std::runtime_error& e)
catch (const std::runtime_error& e)
{
consoleLogger->Log(mantle_api::LogLevel::kError, e.what());
}
......
/********************************************************************************
* 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
......@@ -27,7 +27,7 @@ public:
Entities entities;
double targetLaneOffset;
TransitionDynamics laneChangeActionDynamics;
std::function<mantle_api::UniqueId()> GetLaneChangeTarget;
std::function<mantle_api::LaneId()> GetLaneChangeTarget;
};
struct Interfaces
{
......@@ -48,4 +48,4 @@ protected:
Interfaces mantle;
};
} // namespace OpenScenarioEngine::v1_3
\ No newline at end of file
} // namespace OpenScenarioEngine::v1_3
......@@ -155,6 +155,9 @@ private:
/// be used instead
void ResetStochastics();
/// Reset all routes in route repository
void ResetRoutes();
/// Reset all entities and create new according to scenario description
void ResetAndCreateEntities();
......
......@@ -31,9 +31,9 @@ units::length::meter_t ConvertToAbsoluteLaneOffset(const std::shared_ptr<NET_ASA
{
const auto& reference_entity = EntityUtils::GetEntityByName(environment, relative_target_offset->GetEntityRef()->GetNameRef());
const auto& actor_entity = EntityUtils::GetEntityByName(environment, actor);
const mantle_api::Pose actor_pose{actor_entity.GetPosition(), actor_entity.GetOrientation()};
const mantle_api::Pose actor_pose{actor_entity->GetPosition(), actor_entity->GetOrientation()};
units::length::meter_t relative_target_offset_value{relative_target_offset->GetValue()};
auto absolute_target_lane_offset = environment->GetQueryService().GetLateralCenterLineOffset(actor_pose, reference_entity.GetPosition());
auto absolute_target_lane_offset = environment->GetQueryService().GetLateralCenterLineOffset(actor_pose, reference_entity->GetPosition());
if (!absolute_target_lane_offset)
{
throw std::runtime_error("ConvertScenarioLaneOffsetTarget: unable to calculate lane offset from RelativeTargetLaneOffset, please adjust the scenario.");
......
/********************************************************************************
* Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
* Copyright (c) 2022-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
......@@ -31,10 +31,10 @@ LongitudinalDistance ConvertScenarioLongitudinalDistance(const std::shared_ptr<m
if (isSetTimeGap)
{
const auto& refEntity = EntityUtils::GetEntityByName(environment, ConvertScenarioEntity(entityRef));
return longitudinalTimeGap * refEntity.GetVelocity().Length().value();
const auto refEntity = EntityUtils::GetEntityByName(environment, ConvertScenarioEntity(entityRef));
return longitudinalTimeGap * refEntity->GetVelocity().Length().value();
}
throw std::runtime_error(R"(Warning: LongitudinalDistanceAction: please set either "distance" or "timegap")");
}
} // namespace OpenScenarioEngine::v1_3
\ No newline at end of file
} // namespace OpenScenarioEngine::v1_3
......@@ -138,17 +138,18 @@ mantle_api::Pose ConvertRelativeLanePosition(
}
const auto ref_entity_name = relative_lane_position.GetEntityRef()->GetNameRef();
if (const auto& ref_entity = environment.GetEntityRepository().Get(ref_entity_name))
if (const auto ref_entity = environment.GetEntityRepository().Get(ref_entity_name))
{
const auto d_lane = relative_lane_position.GetDLane();
const auto ds_lane = relative_lane_position.GetDsLane();
const auto offset = relative_lane_position.GetOffset();
const mantle_api::Pose pose_ref{environment.GetGeometryHelper()->TranslateGlobalPositionLocally(
ref_entity.value().get().GetPosition(),
ref_entity.value().get().GetOrientation(),
-ref_entity.value().get().GetProperties()->bounding_box.geometric_center),
ref_entity.value().get().GetOrientation()};
ref_entity->GetPosition(),
ref_entity->GetOrientation(),
-ref_entity->GetProperties()->bounding_box.geometric_center),
ref_entity->GetOrientation()};
const auto calculated_pose = environment.GetQueryService().FindRelativeLanePoseAtDistanceFrom(
pose_ref, d_lane, units::length::meter_t(ds_lane), units::length::meter_t(offset));
......
/********************************************************************************
* Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
* Copyright (c) 2022-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
......@@ -23,9 +23,9 @@ mantle_api::LaneId ConvertScenarioRelativeTargetLane(
const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IRelativeTargetLane>& relativeTargetLane)
{
const auto relative_entity_name = relativeTargetLane->GetEntityRef()->GetNameRef();
const auto& relative_entity = EntityUtils::GetEntityByName(environment, relative_entity_name);
const auto relative_pose = mantle_api::Pose{relative_entity.GetPosition(),
relative_entity.GetOrientation()};
const auto relative_entity = EntityUtils::GetEntityByName(environment, relative_entity_name);
const auto relative_pose = mantle_api::Pose{relative_entity->GetPosition(),
relative_entity->GetOrientation()};
if (const auto target_lane_id =
environment->GetQueryService().GetRelativeLaneId(relative_pose, relativeTargetLane->GetValue()))
{
......@@ -36,4 +36,4 @@ mantle_api::LaneId ConvertScenarioRelativeTargetLane(
relative_entity_name + "\". Please adjust the scenario.");
}
} // namespace OpenScenarioEngine::v1_3
\ No newline at end of file
} // namespace OpenScenarioEngine::v1_3
/********************************************************************************
* 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
......@@ -15,9 +15,10 @@
#include <MantleAPI/Traffic/control_strategy.h>
#include <openScenarioLib/generated/v1_3/catalog/CatalogHelperV1_3.h>
#include "Utils/Logger.h"
#include <utility>
#include "Utils/Logger.h"
namespace OpenScenarioEngine::v1_3
{
namespace detail
......@@ -47,30 +48,30 @@ std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IRoute> ResolveChoice(const std::sh
throw std::runtime_error("ConvertScenarioRoute: No route defined or referenced. Please adjust the scenario.");
}
mantle_api::RouteStrategy ConvertRouteStrategy(
mantle_api::RoutingStrategy ConvertRouteStrategy(
const NET_ASAM_OPENSCENARIO::v1_3::RouteStrategy::RouteStrategyEnum& route_strategy_enum)
{
switch (route_strategy_enum)
{
case NET_ASAM_OPENSCENARIO::v1_3::RouteStrategy::RouteStrategyEnum::FASTEST:
return mantle_api::RouteStrategy::kFastest;
return mantle_api::RoutingStrategy::kFastest;
case NET_ASAM_OPENSCENARIO::v1_3::RouteStrategy::RouteStrategyEnum::LEAST_INTERSECTIONS:
return mantle_api::RouteStrategy::kLeastIntersections;
return mantle_api::RoutingStrategy::kLeastIntersections;
case NET_ASAM_OPENSCENARIO::v1_3::RouteStrategy::RouteStrategyEnum::SHORTEST:
return mantle_api::RouteStrategy::kShortest;
case NET_ASAM_OPENSCENARIO::v1_3::RouteStrategy::RouteStrategyEnum::RANDOM: // NOLINT(bugprone-branch-clone)
[[fallthrough]];
return mantle_api::RoutingStrategy::kShortest;
case NET_ASAM_OPENSCENARIO::v1_3::RouteStrategy::RouteStrategyEnum::RANDOM:
return mantle_api::RoutingStrategy::kRandom;
case NET_ASAM_OPENSCENARIO::v1_3::RouteStrategy::RouteStrategyEnum::UNKNOWN: // NOLINT(bugprone-branch-clone)
[[fallthrough]];
default:
return mantle_api::RouteStrategy::kUndefined;
return mantle_api::RoutingStrategy::kUnknown;
}
}
std::vector<mantle_api::RouteWaypoint> ConvertWaypoints(const std::shared_ptr<mantle_api::IEnvironment>& environment, const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IRoute>& route_object)
std::vector<mantle_api::RoutingWaypoint> ConvertWaypoints(const std::shared_ptr<mantle_api::IEnvironment>& environment, const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IRoute>& route_object)
{
const auto route_waypoints = route_object->GetWaypoints();
std::vector<mantle_api::RouteWaypoint> waypoints;
std::vector<mantle_api::RoutingWaypoint> waypoints;
for (size_t i = 0; i < route_waypoints.size(); ++i)
{
......@@ -79,14 +80,14 @@ std::vector<mantle_api::RouteWaypoint> ConvertWaypoints(const std::shared_ptr<ma
if (auto pose = ConvertScenarioPosition(environment, waypoint->GetPosition()))
{
waypoints.emplace_back(
mantle_api::RouteWaypoint{
mantle_api::RoutingWaypoint{
pose->position,
ConvertRouteStrategy(NET_ASAM_OPENSCENARIO::v1_3::RouteStrategy::GetFromLiteral(waypoint->GetRouteStrategy().GetLiteral()))});
}
else
{
Logger::Warning("ConvertScenarioRoute: Could not convert scenario position to global position for route " +
route_object->GetName() + ", waypoint index " + std::to_string(i) + ". Omitting waypoint");
route_object->GetName() + ", waypoint index " + std::to_string(i) + ". Omitting waypoint");
}
}
return waypoints;
......@@ -99,7 +100,7 @@ Route ConvertScenarioRoute(const std::shared_ptr<mantle_api::IEnvironment>& envi
const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ICatalogReference>& catalogReference)
{
route_object = detail::ResolveChoice(route_object, catalogReference);
return {route_object->GetClosed(), detail::ConvertWaypoints(environment, route_object)};
return {route_object->GetName(), route_object->GetClosed(), detail::ConvertWaypoints(environment, route_object)};
}
} // namespace OpenScenarioEngine::v1_3
\ No newline at end of file
} // namespace OpenScenarioEngine::v1_3
/********************************************************************************
* 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
......@@ -22,12 +21,13 @@ namespace OpenScenarioEngine::v1_3
{
struct Route
{
std::string name;
bool closed{false};
std::vector<mantle_api::RouteWaypoint> waypoints;
std::vector<mantle_api::RoutingWaypoint> waypoints;
};
Route ConvertScenarioRoute(const std::shared_ptr<mantle_api::IEnvironment>& environment,
std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IRoute> route,
const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ICatalogReference>& catalogReference);
} // namespace OpenScenarioEngine::v1_3
\ No newline at end of file
} // namespace OpenScenarioEngine::v1_3
/********************************************************************************
* 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
......@@ -27,9 +26,10 @@ SpeedActionTarget ConvertScenarioSpeedActionTarget(
if (relative_target_speed)
{
/// TODO: continuous need to be taken into consideration at some point as well
const auto &relative_entity_name = relative_target_speed->GetEntityRef()->GetNameRef();
const auto& relative_entity_name = relative_target_speed->GetEntityRef()->GetNameRef();
if (!environment->GetEntityRepository().Get(relative_entity_name).has_value())
auto entity = environment->GetEntityRepository().Get(relative_entity_name);
if (!entity)
{
throw std::runtime_error(
"Relative Entity with name \"" + relative_entity_name +
......@@ -37,7 +37,7 @@ SpeedActionTarget ConvertScenarioSpeedActionTarget(
"scenario.");
}
auto target_base = environment->GetEntityRepository().Get(relative_entity_name).value().get().GetVelocity();
auto target_base = entity->GetVelocity();
units::velocity::meters_per_second_t base_magnitude{target_base.Length()};
if (relative_target_speed->GetSpeedTargetValueType() ==
......@@ -55,4 +55,4 @@ SpeedActionTarget ConvertScenarioSpeedActionTarget(
throw std::runtime_error("speedActionTarget needs to have either Relative or Absolute target speed");
}
} // namespace OpenScenarioEngine::v1_3
\ No newline at end of file
} // namespace OpenScenarioEngine::v1_3
/********************************************************************************
* 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
......@@ -10,6 +9,7 @@
********************************************************************************/
#include "Conversion/OscToMantle/ConvertScenarioTrafficDefinition.h"
#include "Conversion/OscToMantle/ConvertScenarioController.h"
namespace OpenScenarioEngine::v1_3
......@@ -23,20 +23,20 @@ mantle_api::VehicleClass ConvertVehicleCategory(NET_ASAM_OPENSCENARIO::v1_3::Veh
const auto literal{vehicle_category.GetLiteral()};
const auto category{vehicle_category.GetFromLiteral(literal)};
switch(category)
switch (category)
{
case NET_ASAM_OPENSCENARIO::v1_3::VehicleCategory::VehicleCategoryEnum::UNKNOWN:
return mantle_api::VehicleClass::kInvalid;
return mantle_api::VehicleClass::kUnknown;
case NET_ASAM_OPENSCENARIO::v1_3::VehicleCategory::VehicleCategoryEnum::BICYCLE:
return mantle_api::VehicleClass::kBicycle;
case NET_ASAM_OPENSCENARIO::v1_3::VehicleCategory::VehicleCategoryEnum::BUS:
return mantle_api::VehicleClass::kBus;
case NET_ASAM_OPENSCENARIO::v1_3::VehicleCategory::VehicleCategoryEnum::CAR:
return mantle_api::VehicleClass::kMedium_car;
return mantle_api::VehicleClass::kMediumCar;
case NET_ASAM_OPENSCENARIO::v1_3::VehicleCategory::VehicleCategoryEnum::MOTORBIKE:
return mantle_api::VehicleClass::kMotorbike;
return mantle_api::VehicleClass::kMotorcycle;
case NET_ASAM_OPENSCENARIO::v1_3::VehicleCategory::VehicleCategoryEnum::SEMITRAILER:
return mantle_api::VehicleClass::kSemitrailer;
return mantle_api::VehicleClass::kSemiTrailer;
case NET_ASAM_OPENSCENARIO::v1_3::VehicleCategory::VehicleCategoryEnum::TRAILER:
return mantle_api::VehicleClass::kTrailer;
case NET_ASAM_OPENSCENARIO::v1_3::VehicleCategory::VehicleCategoryEnum::TRAIN:
......@@ -44,11 +44,11 @@ mantle_api::VehicleClass ConvertVehicleCategory(NET_ASAM_OPENSCENARIO::v1_3::Veh
case NET_ASAM_OPENSCENARIO::v1_3::VehicleCategory::VehicleCategoryEnum::TRAM:
return mantle_api::VehicleClass::kTram;
case NET_ASAM_OPENSCENARIO::v1_3::VehicleCategory::VehicleCategoryEnum::TRUCK:
return mantle_api::VehicleClass::kHeavy_truck;
return mantle_api::VehicleClass::kHeavyTruck;
case NET_ASAM_OPENSCENARIO::v1_3::VehicleCategory::VehicleCategoryEnum::VAN:
return mantle_api::VehicleClass::kDelivery_van;
return mantle_api::VehicleClass::kDeliveryVan;
default:
return mantle_api::VehicleClass::kInvalid;
return mantle_api::VehicleClass::kOther;
}
}
......@@ -56,12 +56,12 @@ VehicleCategoryDistribution ConvertVehicleCategoryDistributionEntry(std::shared_
{
VehicleCategoryDistribution vehicle_category_distribution{};
if(entry->IsSetCategory())
if (entry->IsSetCategory())
{
vehicle_category_distribution.category = ConvertVehicleCategory(entry->GetCategory());
}
if(entry->IsSetWeight())
if (entry->IsSetWeight())
{
vehicle_category_distribution.weight = entry->GetWeight();
}
......@@ -73,33 +73,33 @@ ControllerDistribution ConvertControllerDistributionEntry(std::shared_ptr<NET_AS
{
ControllerDistribution controller_distribution{};
if(entry->IsSetWeight())
if (entry->IsSetWeight())
{
controller_distribution.weight = entry->GetWeight();
}
controller_distribution.controller = ConvertScenarioController(entry->GetController(), entry->GetCatalogReference());
return controller_distribution;
}
} // namespace detail
} // namespace detail
TrafficDefinition ConvertScenarioTrafficDefinition(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrafficDefinition>& osc_traffic_definition)
{
if(osc_traffic_definition->GetVehicleCategoryDistribution()->GetVehicleCategoryDistributionEntries().empty())
if (osc_traffic_definition->GetVehicleCategoryDistribution()->GetVehicleCategoryDistributionEntries().empty())
{
throw std::runtime_error("ConvertScenarioTrafficDefinition : the traffic definition has no vehicle distribution");
}
if(osc_traffic_definition->GetControllerDistribution()->GetControllerDistributionEntries().empty())
if (osc_traffic_definition->GetControllerDistribution()->GetControllerDistributionEntries().empty())
{
throw std::runtime_error("ConvertScenarioTrafficDefinition : the traffic definition has no controller distribution");
}
std::vector<VehicleCategoryDistribution> vehicle_category_distribution_entries;
for(const auto& vehicle_category_distribution_entry : osc_traffic_definition->GetVehicleCategoryDistribution()->GetVehicleCategoryDistributionEntries())
for (const auto& vehicle_category_distribution_entry : osc_traffic_definition->GetVehicleCategoryDistribution()->GetVehicleCategoryDistributionEntries())
{
auto converted{detail::ConvertVehicleCategoryDistributionEntry(vehicle_category_distribution_entry)};
vehicle_category_distribution_entries.push_back(std::move(converted));
......@@ -107,7 +107,7 @@ TrafficDefinition ConvertScenarioTrafficDefinition(const std::shared_ptr<NET_ASA
std::vector<ControllerDistribution> controller_distribution_entries;
for(const auto& controller_distribution_entry : osc_traffic_definition->GetControllerDistribution()->GetControllerDistributionEntries())
for (const auto& controller_distribution_entry : osc_traffic_definition->GetControllerDistribution()->GetControllerDistributionEntries())
{
auto converted{detail::ConvertControllerDistributionEntry(controller_distribution_entry)};
controller_distribution_entries.push_back(std::move(converted));
......@@ -116,4 +116,4 @@ TrafficDefinition ConvertScenarioTrafficDefinition(const std::shared_ptr<NET_ASA
return {osc_traffic_definition->GetName(), vehicle_category_distribution_entries, controller_distribution_entries};
}
} // namespace OpenScenarioEngine::v1_3
\ No newline at end of file
} // namespace OpenScenarioEngine::v1_3
......@@ -154,6 +154,7 @@ void OpenScenarioEngine::Init()
void OpenScenarioEngine::SetupDynamicContent()
{
ResetStochastics();
ResetRoutes();
ResetAndCreateEntities();
ResetAndCreateControllers();
ResetAndCreateStoryboard();
......@@ -319,6 +320,11 @@ std::unique_ptr<mantle_api::MapDetails> OpenScenarioEngine::GetMapDetails() cons
return map_details;
}
void OpenScenarioEngine::ResetRoutes()
{
environment_->GetRouteRepository().Reset();
}
void OpenScenarioEngine::ResetAndCreateEntities()
{
if (!scenario_definition_ptr_->GetEntities())
......
/********************************************************************************
* 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
......@@ -62,9 +62,9 @@ bool DistanceCondition::IsSatisfied() const
if (values.relativeDistanceType == RelativeDistanceType::kLongitudinal &&
values.coordinateSystem == CoordinateSystem::kEntity)
{
const auto& triggeringEntity = EntityUtils::GetEntityByName(mantle.environment, values.triggeringEntity);
const auto triggeringEntity = EntityUtils::GetEntityByName(mantle.environment, values.triggeringEntity);
const auto distance = EntityUtils::CalculateRelativeLongitudinalDistance(
mantle.environment, triggeringEntity, pose.value().position);
mantle.environment, *triggeringEntity, pose.value().position);
return values.rule.IsSatisfied(distance.value());
}
......
/********************************************************************************
* 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
......@@ -25,15 +25,15 @@ bool ReachPositionCondition::IsSatisfied() const
return false;
}
const auto& entity = mantle.environment->GetEntityRepository().Get(values.triggeringEntity);
auto entity = mantle.environment->GetEntityRepository().Get(values.triggeringEntity);
if (!entity)
{
Logger::Warning("ReachPositionCondition: ReachPositionCondition cannot be satisfied (entity undefined).");
return false;
}
auto current_position = entity->get().GetPosition();
auto current_position = entity->GetPosition();
return (position->position - current_position).Length().value() <= values.tolerance;
}
} // namespace OpenScenarioEngine::v1_3
\ No newline at end of file
} // namespace OpenScenarioEngine::v1_3
/********************************************************************************
* Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
* Copyright (c) 2021-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
* Copyright (c) 2023 Ansys, Inc.
*
* This program and the accompanying materials are made available under the
......@@ -20,23 +20,23 @@ bool RelativeDistanceCondition::IsSatisfied() const
{
if (values.coordinateSystem == CoordinateSystem::kEntity)
{
const auto& triggeringEntity = EntityUtils::GetEntityByName(mantle.environment, values.triggeringEntity);
const auto& referencedEntity = EntityUtils::GetEntityByName(mantle.environment, values.entityRef);
const auto triggeringEntity = EntityUtils::GetEntityByName(mantle.environment, values.triggeringEntity);
const auto referencedEntity = EntityUtils::GetEntityByName(mantle.environment, values.entityRef);
if (values.relativeDistanceType == RelativeDistanceType::kLongitudinal)
{
const auto distance = values.freespace
? EntityUtils::CalculateLongitudinalFreeSpaceDistance(
mantle.environment, triggeringEntity, referencedEntity)
mantle.environment, *triggeringEntity, *referencedEntity)
: EntityUtils::CalculateRelativeLongitudinalDistance(
mantle.environment, triggeringEntity, referencedEntity);
mantle.environment, *triggeringEntity, *referencedEntity);
return values.rule.IsSatisfied(distance.value());
}
if (values.relativeDistanceType == RelativeDistanceType::kLateral && values.freespace)
{
const auto distance = EntityUtils::CalculateLateralFreeSpaceDistance(
mantle.environment, triggeringEntity, referencedEntity);
mantle.environment, *triggeringEntity, *referencedEntity);
return values.rule.IsSatisfied(distance.value());
}
}
......
/********************************************************************************
* 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
......@@ -16,14 +16,14 @@ namespace OpenScenarioEngine::v1_3
{
bool RelativeSpeedCondition::IsSatisfied() const
{
const auto& triggeringEntity = EntityUtils::GetEntityByName(mantle.environment, values.triggeringEntity);
const auto triggeringVelocity = triggeringEntity.GetVelocity().Length();
const auto triggeringEntity = EntityUtils::GetEntityByName(mantle.environment, values.triggeringEntity);
const auto triggeringVelocity = triggeringEntity->GetVelocity().Length();
const auto& referencedEntity = EntityUtils::GetEntityByName(mantle.environment, values.entityRef);
const auto referencedVelocity = referencedEntity.GetVelocity().Length();
const auto referencedEntity = EntityUtils::GetEntityByName(mantle.environment, values.entityRef);
const auto referencedVelocity = referencedEntity->GetVelocity().Length();
const auto relativeSpeed = triggeringVelocity - referencedVelocity;
return values.rule.IsSatisfied(relativeSpeed.value());
}
} // namespace OpenScenarioEngine::v1_3
\ No newline at end of file
} // namespace OpenScenarioEngine::v1_3
/********************************************************************************
* 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
......@@ -16,8 +16,8 @@ namespace OpenScenarioEngine::v1_3
{
bool SpeedCondition::IsSatisfied() const
{
const auto& triggeringEntity = EntityUtils::GetEntityByName(mantle.environment, values.triggeringEntity);
const auto speed = triggeringEntity.GetVelocity().Length();
const auto triggeringEntity = EntityUtils::GetEntityByName(mantle.environment, values.triggeringEntity);
const auto speed = triggeringEntity->GetVelocity().Length();
return values.rule.IsSatisfied(speed.value());
}
......
/********************************************************************************
* Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
* Copyright (c) 2021-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
* Copyright (c) 2023 Ansys, Inc.
*
* This program and the accompanying materials are made available under the
......@@ -31,21 +31,21 @@ bool TimeHeadwayCondition::IsSatisfied() const
return false;
}
const auto& ref_entity = EntityUtils::GetEntityByName(mantle.environment, values.entityRef);
const auto& trigger_entity = EntityUtils::GetEntityByName(mantle.environment, values.triggeringEntity);
const auto ref_entity = EntityUtils::GetEntityByName(mantle.environment, values.entityRef);
const auto trigger_entity = EntityUtils::GetEntityByName(mantle.environment, values.triggeringEntity);
units::length::meter_t distance{};
if (values.coordinateSystem == CoordinateSystem::kEntity)
{
distance = values.freespace
? EntityUtils::CalculateLongitudinalFreeSpaceDistance(mantle.environment, trigger_entity, ref_entity)
: EntityUtils::CalculateRelativeLongitudinalDistance(mantle.environment, trigger_entity, ref_entity);
? EntityUtils::CalculateLongitudinalFreeSpaceDistance(mantle.environment, *trigger_entity, *ref_entity)
: EntityUtils::CalculateRelativeLongitudinalDistance(mantle.environment, *trigger_entity, *ref_entity);
}
else if (values.coordinateSystem == CoordinateSystem::kLane)
{
const auto longitudinal_lane_distance =
mantle.environment->GetQueryService().GetLongitudinalLaneDistanceBetweenPositions(trigger_entity.GetPosition(),
ref_entity.GetPosition());
mantle.environment->GetQueryService().GetLongitudinalLaneDistanceBetweenPositions(trigger_entity->GetPosition(),
ref_entity->GetPosition());
if (!longitudinal_lane_distance.has_value())
{
throw std::runtime_error(
......@@ -57,18 +57,18 @@ bool TimeHeadwayCondition::IsSatisfied() const
if (values.freespace)
{
const auto trigger_entity_lane_orientation =
mantle.environment->GetQueryService().GetLaneOrientation(trigger_entity.GetPosition());
mantle.environment->GetQueryService().GetLaneOrientation(trigger_entity->GetPosition());
const auto trigger_entity_corners_along_lane =
EntityUtils::GetCornerPositionsInLocalSortedByLongitudinalDistance(
mantle.environment, trigger_entity, trigger_entity.GetPosition(), trigger_entity_lane_orientation);
mantle.environment, *trigger_entity, trigger_entity->GetPosition(), trigger_entity_lane_orientation);
const auto ref_entity_lane_pose = mantle.environment->GetQueryService().FindLanePoseAtDistanceFrom(
{trigger_entity.GetPosition(), trigger_entity.GetOrientation()},
{trigger_entity->GetPosition(), trigger_entity->GetOrientation()},
distance,
mantle_api::Direction::kForward);
const auto ref_entity_corners_along_lane =
EntityUtils::GetCornerPositionsInLocalSortedByLongitudinalDistance(
mantle.environment, ref_entity, ref_entity.GetPosition(), ref_entity_lane_pose.value().orientation);
mantle.environment, *ref_entity, ref_entity->GetPosition(), ref_entity_lane_pose.value().orientation);
distance = distance - (units::math::abs(trigger_entity_corners_along_lane.front().x) +
units::math::abs(ref_entity_corners_along_lane.back().x));
......@@ -80,7 +80,7 @@ bool TimeHeadwayCondition::IsSatisfied() const
throw std::runtime_error("TimeHeadwayCondition: The given CoordinateSystem is not supported. Only \"entity\" and \"lane\" coordinate systems are supported for now.");
}
const auto trigger_entity_velocity = trigger_entity.GetVelocity().Length();
const auto trigger_entity_velocity = trigger_entity->GetVelocity().Length();
const auto time_headway = trigger_entity_velocity > limits::kAcceptableVelocityMin
? distance / trigger_entity_velocity
: limits::kTimeHeadwayMax;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment