diff --git a/README.md b/README.md
index 1100ed55091995a8268e6e77084244a0a3f26107..e65e90420077b951ae850763c92f5ef2b0af1ea1 100644
--- a/README.md
+++ b/README.md
@@ -133,7 +133,7 @@ The following Actions and Conditions of [ASAM OpenSCENARIO XML](https://publicat
| _none_ | [TrafficSignalStateAction](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/TrafficSignalStateAction.html) | âœ”ï¸ Complete |
| _none_ | [TrafficSinkAction](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/TrafficSinkAction.html) | âœ”ï¸ Does not interpret `rate` and `TrafficDefinition`, and restricted by conversion: see [Conversions::Position](#position) below |
| _none_ | [VisibilityAction](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/VisibilityAction.html) | âœ”ï¸ Complete |
-| MotionControlAction | [FollowTrajectoryAction](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/FollowTrajectoryAction.html) | âœ”ï¸ Complete (Possibility for optimization in the base class) |
+| MotionControlAction | [FollowTrajectoryAction](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/FollowTrajectoryAction.html) | 🚧 Supports trajectory shapes polyline and clothoid spline + (Possibility for optimization in the base class) |
| MotionControlAction | [LaneChangeAction](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/LaneChangeAction.html) | âœ”ï¸ Complete |
| MotionControlAction | [LaneOffsetAction](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/LaneOffsetAction.html) | 🚧 Supports only AbsoluteTargetLaneOffset. In addition, LaneOffsetActionDynamics and continuous are ignored at the moment |
| MotionControlAction | [LateralDistanceAction](https://www.asam.net/static_downloads/ASAM_OpenSCENARIO_V1.1.1_Model_Documentation/modelDocumentation/content/LateralDistanceAction.html) | âœ”ï¸ Does not interpret `DynamicConstraints` and restricted by conversion: see [Conversions::Coordinate System](#coordinate-system) and [Conversions::Continuous](#continuous) |
diff --git a/engine/src/Conversion/OscToMantle/ConvertScenarioTrajectoryRef.cpp b/engine/src/Conversion/OscToMantle/ConvertScenarioTrajectoryRef.cpp
index 25ec2da833e8a7d309146902391bd27d3653ea44..c0b3de455ef7915f9265b4528c2d2b9b37bfb71e 100644
--- a/engine/src/Conversion/OscToMantle/ConvertScenarioTrajectoryRef.cpp
+++ b/engine/src/Conversion/OscToMantle/ConvertScenarioTrajectoryRef.cpp
@@ -19,6 +19,7 @@
#include "Conversion/OscToMantle/ConvertScenarioClothoidSpline.h"
#include "Conversion/OscToMantle/ConvertScenarioPosition.h"
+#include "Utils/Logger.h"
namespace OpenScenarioEngine::v1_3
{
@@ -46,7 +47,7 @@ std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrajectory> OpenScenarioToMantle(c
{
return ConvertCatalogReferenceToTrajectory(catalogReference);
}
- throw std::runtime_error("convertScenarioRoute: Either trajectorReference or catalogReference must be set");
+ throw std::runtime_error("ConvertScenarioTrajectoryRef: Either trajectorReference or catalogReference must be set. Note: Deprecated element Trajectory is not supported.");
}
mantle_api::TrajectoryReferencePoint ParseTrajectoryReferencePoint(const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrajectory>& follow_trajectory_action_trajectory)
@@ -100,31 +101,9 @@ mantle_api::Trajectory ConvertPolyline(const std::shared_ptr<NET_ASAM_OPENSCENAR
{
mantle_api::PolyLinePoint poly_line_point;
- // TODO CK Separate method to parse position, which can be used anywhere in the openScenarioEngine
auto poly_line_vertex_position = poly_line_vertex->GetPosition();
auto pose = ConvertScenarioPosition(environment, poly_line_vertex_position);
poly_line_point.pose = pose.value();
- // if (poly_line_vertex_position->GetWorldPosition())
- // {
- // auto world_position = poly_line_vertex_position->GetWorldPosition();
-
- // mantle_api::Vec3<units::length::meter_t> position;
- // position.x = units::length::meter_t(world_position->GetX());
- // position.y = units::length::meter_t(world_position->GetY());
- // position.z = units::length::meter_t(world_position->GetZ());
-
- // mantle_api::Orientation3<units::angle::radian_t> orientation;
- // orientation.yaw = units::angle::radian_t(world_position->GetH());
- // orientation.pitch = units::angle::radian_t(world_position->GetP());
- // orientation.roll = units::angle::radian_t(world_position->GetR());
-
- // poly_line_point.pose = {position, orientation};
- // }
- // else
- // {
- // Logger::Warning("FollowTrajectoryAction currently does not support other Position types than "
- // "WorldPosition in Polyline shape");
- // }
poly_line_point.time = poly_line_vertex->IsSetTime() ? std::make_optional<units::time::second_t>(poly_line_vertex->GetTime()) : std::nullopt;
@@ -141,18 +120,20 @@ TrajectoryRef ConvertScenarioTrajectoryRef(const std::shared_ptr<mantle_api::IEn
const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ITrajectoryRef>& trajectoryRef,
const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ICatalogReference>& catalogReference)
{
- auto follow_trajectory_action_trajectory = detail::OpenScenarioToMantle(trajectoryRef, catalogReference);
+ const auto trajectory = detail::OpenScenarioToMantle(trajectoryRef, catalogReference);
+ const auto shape = trajectory->GetShape();
- auto follow_trajectory_action_trajectory_shape = follow_trajectory_action_trajectory->GetShape();
-
- if (follow_trajectory_action_trajectory_shape->GetPolyline())
+ if (shape->IsSetPolyline())
{
- return detail::ConvertPolyline(follow_trajectory_action_trajectory, environment);
+ return detail::ConvertPolyline(trajectory, environment);
}
- else if (follow_trajectory_action_trajectory_shape->GetClothoidSpline())
+
+ if (shape->IsSetClothoidSpline())
{
- return detail::ConvertClothoidShape(follow_trajectory_action_trajectory);
+ return detail::ConvertClothoidShape(trajectory);
}
+
+ Logger::Error("ConvertScenarioTrajectoryRef: Unsupported trajectory shape for trajectory '" + trajectory->GetName() + "'. Only Polyline and ClothoidSpline are supported!");
return std::nullopt;
}
diff --git a/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp b/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp
index f05a7937c37c69739ae22eea5993c8ad6a8db588..97f3bb412fe6bb1a47e48a2761783fe79843b03a 100644
--- a/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp
+++ b/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.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 which is available at
@@ -11,11 +11,14 @@
#include "Storyboard/MotionControlAction/FollowTrajectoryAction_impl.h"
#include <MantleAPI/Common/clothoid_spline.h>
+#include <MantleAPI/Common/trajectory.h>
#include <MantleAPI/Traffic/entity_helper.h>
+#include <optional>
#include <variant>
#include "Utils/EntityUtils.h"
+#include "Utils/Logger.h"
namespace OpenScenarioEngine::v1_3
{
@@ -81,9 +84,9 @@ void FollowTrajectoryAction::SetControlStrategy()
{
if (!values.trajectoryRef)
{
- throw std::runtime_error(
- "FollowTrajectoryAction does not contain TrajectoryRef. "
- "Note: Deprecated element Trajectory is not supported.");
+ Logger::Error(
+ "FollowTrajectoryAction does not contain TrajectoryRef.");
+ return;
}
for (const auto& entity : values.entities)
@@ -94,22 +97,29 @@ void FollowTrajectoryAction::SetControlStrategy()
? mantle_api::MovementDomain::kBoth
: mantle_api::MovementDomain::kLateral;
- std::visit(
- overloaded{
- [&](const mantle_api::PolyLine&)
- {
- control_strategy->trajectory = detail::ConvertPolyLine(mantle.environment, entity, values.trajectoryRef);
- },
- [&](const mantle_api::ClothoidSpline&)
- {
- control_strategy->trajectory = detail::ConvertClothoidSpline(mantle.environment, entity, values.trajectoryRef);
- },
- [](auto)
- {
- throw std::runtime_error("Unsupported type of Trajectory shape");
- }},
- values.trajectoryRef->type);
-
+ std::optional<mantle_api::Trajectory> trajectory =
+ std::visit(
+ overloaded{
+ [&](const mantle_api::PolyLine&)
+ {
+ return detail::ConvertPolyLine(mantle.environment, entity, values.trajectoryRef);
+ },
+ [&](const mantle_api::ClothoidSpline&)
+ {
+ return detail::ConvertClothoidSpline(mantle.environment, entity, values.trajectoryRef);
+ },
+ [](auto)
+ {
+ Logger::Error("Unsupported type of Trajectory shape");
+ return std::nullopt;
+ }},
+ values.trajectoryRef->type);
+
+ if (!trajectory)
+ {
+ return;
+ }
+ control_strategy->trajectory = *trajectory;
const auto entity_id = mantle.environment->GetEntityRepository().Get(entity)->get().GetUniqueId();
mantle.environment->UpdateControlStrategies(entity_id, {control_strategy});
}
diff --git a/engine/tests/Conversion/OscToMantle/ConvertScenarioTrajectoryRefTest.cpp b/engine/tests/Conversion/OscToMantle/ConvertScenarioTrajectoryRefTest.cpp
index 7dbb6fe6884b5f61967bfdadd1aae138a6986f57..6a223eef6016c734b1041fd1731c9a6bed591af2 100644
--- a/engine/tests/Conversion/OscToMantle/ConvertScenarioTrajectoryRefTest.cpp
+++ b/engine/tests/Conversion/OscToMantle/ConvertScenarioTrajectoryRefTest.cpp
@@ -13,6 +13,7 @@
#include <openScenarioLib/generated/v1_3/api/ApiClassInterfacesV1_3.h>
#include <openScenarioLib/generated/v1_3/impl/ApiClassImplV1_3.h>
+#include <functional>
#include <optional>
#include <stdexcept>
#include <tuple>
@@ -20,6 +21,7 @@
#include "Conversion/OscToMantle/ConvertScenarioTrajectoryRef.h"
#include "TestUtils/ClothoidSplineUtils.h"
+#include "TestUtils/TestLogger.h"
namespace
{
@@ -29,6 +31,13 @@ using namespace units::literals;
class ConvertScenarioTrajectoryRefTest : public ::testing::Test
{
protected:
+ void SetTrajectoryShapeAndName()
+ {
+ trajectory->SetShape(trajectory_shape);
+ trajectory->SetName("Trajectory1");
+ trajectory_ref->SetTrajectory(trajectory);
+ }
+
void SetTrajectoryWithClothoidSpline()
{
expected_clothoid_spline.segments.push_back({
@@ -54,10 +63,7 @@ protected:
clothoid_spline->SetSegments(clothoid_spline_segments);
trajectory_shape->SetClothoidSpline(clothoid_spline);
- trajectory->SetShape(trajectory_shape);
- trajectory->SetName("Trajectory1");
-
- trajectory_ref->SetTrajectory(trajectory);
+ SetTrajectoryShapeAndName();
}
std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::PositionImpl> SetWorldPosition(double x, double y, double z, double h, double p, double r)
@@ -107,6 +113,8 @@ protected:
std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IParameterDeclarationWriter>> parameter_declarations;
mantle_api::ClothoidSpline expected_clothoid_spline;
+
+ testing::OpenScenarioEngine::v1_3::TestLogger logger;
};
TEST_F(ConvertScenarioTrajectoryRefTest, GivenPolyLine_ReturnMantleAPITrajectoryType)
@@ -119,10 +127,7 @@ TEST_F(ConvertScenarioTrajectoryRefTest, GivenPolyLine_ReturnMantleAPITrajectory
polyline->SetVertices(vertices);
trajectory_shape->SetPolyline(polyline);
- trajectory->SetShape(trajectory_shape);
- trajectory->SetName("Trajectory1");
-
- trajectory_ref->SetTrajectory(trajectory);
+ SetTrajectoryShapeAndName();
auto converted_trajectory_ref = OpenScenarioEngine::v1_3::ConvertScenarioTrajectoryRef(mockEnvironment, trajectory_ref, nullptr);
@@ -193,14 +198,24 @@ TEST_F(ConvertScenarioTrajectoryRefTest, GivenTrajectoryWithClothoidSpline_WhenI
EXPECT_THROW(OpenScenarioEngine::v1_3::ConvertScenarioTrajectoryRef(mockEnvironment, trajectory_ref, nullptr), std::runtime_error);
}
-TEST_F(ConvertScenarioTrajectoryRefTest, GivenNoPolyLineAndNoClothoidSpline_ReturnNullPtr)
+using SetShapeFunction = std::function<void(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ShapeImpl>)>;
+
+class ConvertScenarioTrajectoryRefTestShapeParam : public ConvertScenarioTrajectoryRefTest, public ::testing::WithParamInterface<SetShapeFunction>
{
- trajectory->SetShape(trajectory_shape);
- trajectory->SetName("Trajectory1");
- trajectory_ref->SetTrajectory(trajectory);
+};
- auto converted_trajectory_ref = OpenScenarioEngine::v1_3::ConvertScenarioTrajectoryRef(mockEnvironment, trajectory_ref, nullptr);
+INSTANTIATE_TEST_CASE_P(
+ ConvertScenarioTrajectoryRefTestShapeInstParam,
+ ConvertScenarioTrajectoryRefTestShapeParam,
+ ::testing::Values([](std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ShapeImpl> shape){shape->SetClothoid(std::make_shared<NET_ASAM_OPENSCENARIO::v1_3::ClothoidImpl>());},
+ [](std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::ShapeImpl> shape){shape->SetNurbs(std::make_shared<NET_ASAM_OPENSCENARIO::v1_3::NurbsImpl>());}));
- EXPECT_EQ(std::nullopt, converted_trajectory_ref);
+TEST_P(ConvertScenarioTrajectoryRefTestShapeParam, GivenUnsupportedShape_ThenException)
+{
+ GetParam()(trajectory_shape);
+ SetTrajectoryShapeAndName();
+ EXPECT_EQ(std::nullopt, OpenScenarioEngine::v1_3::ConvertScenarioTrajectoryRef(mockEnvironment, trajectory_ref, nullptr));
+ EXPECT_THAT(logger.LastLogLevel(), mantle_api::LogLevel::kError);
+ EXPECT_THAT(logger.LastLogMessage(), testing::HasSubstr("Only Polyline and ClothoidSpline are supported!"));
}
} // namespace
diff --git a/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp b/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp
index 9cab4e6614d775b41268a7fe30c7799fa5ec6c29..8536a2f735faaadf9841c7126489ccf513d62e07 100644
--- a/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp
+++ b/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp
@@ -20,6 +20,7 @@
#include "Storyboard/MotionControlAction/FollowTrajectoryAction.h"
#include "Storyboard/MotionControlAction/FollowTrajectoryAction_impl.h"
#include "TestUtils.h"
+#include "TestUtils/TestLogger.h"
#include "builders/ActionBuilder.h"
#include "builders/PositionBuilder.h"
#include "gmock/gmock.h"
@@ -421,8 +422,9 @@ TEST(FollowTrajectoryAction, GivenUnmatchingControlStrategyGoalIsReached_Returns
ASSERT_FALSE(followTrajectoryAction.HasControlStrategyGoalBeenReached("Vehicle1"));
}
-TEST(FollowTrajectoryAction, GivenFollowTrajectoryAction_WhenNoTrajectoryRef_ThenThrowError)
+TEST(FollowTrajectoryAction, GivenFollowTrajectoryAction_WhenNoTrajectoryRef_ThenNoUpdateControlStrategyAndLogError)
{
+ testing::OpenScenarioEngine::v1_3::TestLogger logger;
auto mockEnvironment = std::make_shared<MockEnvironment>();
OpenScenarioEngine::v1_3::FollowTrajectoryAction followTrajectoryAction({std::vector<std::string>{"Vehicle1"},
@@ -433,7 +435,10 @@ TEST(FollowTrajectoryAction, GivenFollowTrajectoryAction_WhenNoTrajectoryRef_The
OpenScenarioEngine::v1_3::TrajectoryRef{}},
{mockEnvironment});
- EXPECT_THROW(followTrajectoryAction.SetControlStrategy(), std::runtime_error);
+ EXPECT_CALL(*mockEnvironment, UpdateControlStrategies(_, _)).Times(::testing::AtMost(0));
+ EXPECT_NO_THROW(followTrajectoryAction.SetControlStrategy());
+ EXPECT_THAT(logger.LastLogLevel(), mantle_api::LogLevel::kError);
+ EXPECT_THAT(logger.LastLogMessage(), testing::HasSubstr("FollowTrajectoryAction does not contain TrajectoryRef."));
}
TEST(FollowTrajectoryAction, GivenFollowTrajectoryActionAndTrajectoryRef_WhenPolylineShape_UpdatesControlStrategies)