diff --git a/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp b/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp index a4c9a876b236123503d3ea3834c1e458dbb15648..020308ea1111c45e37d2b551719b599b8a62f265 100644 --- a/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp +++ b/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp @@ -1,5 +1,5 @@ /******************************************************************************** - * Copyright (c) 2021-2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * 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 @@ -16,6 +16,31 @@ namespace OpenScenarioEngine::v1_2 { + +namespace detail +{ + +mantle_api::Trajectory ConvertPolyLine( + const std::shared_ptr<mantle_api::IEnvironment>& environment, + const std::string& entity, + TrajectoryRef trajectoryRef) +{ + const auto& refEntity = EntityUtils::GetEntityByName(environment, entity); + const auto geometric_center = refEntity.GetProperties()->bounding_box.geometric_center; + + // modifies the copy of the trajectory ref! + for (auto& polyLinePoint : std::get<mantle_api::PolyLine>(trajectoryRef->type)) + { + polyLinePoint.pose.position = environment->GetGeometryHelper()->TranslateGlobalPositionLocally( + polyLinePoint.pose.position, + polyLinePoint.pose.orientation, + geometric_center); + } + return trajectoryRef.value(); +} + +} // namespace detail + void FollowTrajectoryAction::SetControlStrategy() { if (!values.trajectoryRef) @@ -27,23 +52,12 @@ void FollowTrajectoryAction::SetControlStrategy() for (const auto& entity : values.entities) { - const auto& refEntity = EntityUtils::GetEntityByName(mantle.environment, entity); - mantle_api::MovementDomain movementDomain{mantle_api::MovementDomain::kLateral}; - for (auto& polyLinePoint : std::get<mantle_api::PolyLine>(values.trajectoryRef->type)) - { - if (polyLinePoint.time.has_value()) - { - movementDomain = mantle_api::MovementDomain::kBoth; - } - polyLinePoint.pose.position = mantle.environment->GetGeometryHelper()->TranslateGlobalPositionLocally(polyLinePoint.pose.position, - polyLinePoint.pose.orientation, - refEntity.GetProperties()->bounding_box.geometric_center); - } auto control_strategy = std::make_shared<mantle_api::FollowTrajectoryControlStrategy>(); - control_strategy->movement_domain = movementDomain; - control_strategy->trajectory = values.trajectoryRef.value(); control_strategy->timeReference = values.timeReference; - + control_strategy->movement_domain = values.timeReference + ? mantle_api::MovementDomain::kBoth + : mantle_api::MovementDomain::kLateral; + control_strategy->trajectory = detail::ConvertPolyLine(mantle.environment, entity, values.trajectoryRef); const auto entity_id = mantle.environment->GetEntityRepository().Get(entity)->get().GetUniqueId(); mantle.environment->UpdateControlStrategies(entity_id, {control_strategy}); } diff --git a/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp b/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp index 96f713cca0b7fa52a91d0d57879c5c2f0b906738..146766dcfe2c40ed0ec9a586814b95d962fdcb1d 100644 --- a/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp +++ b/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp @@ -1,5 +1,5 @@ /******************************************************************************** - * Copyright (c) 2021-2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * Copyright (c) 2021-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) * Copyright (c) 2023 Ansys, Inc. * * This program and the accompanying materials are made available under the @@ -483,3 +483,49 @@ TEST(FollowTrajectoryAction, GivenFollowTrajectoryActionAndTrajectoryRef_Updates EXPECT_EQ(polyline.at(1).pose, pose2); EXPECT_EQ(polyline.at(2).pose, pose3); } + +TEST(FollowTrajectoryAction, + GivenFollowTrajectoryActionWithTimeReference_WhenStartAction_ThenControlStrategyHasMovementDomainBoth) +{ + auto mockEnvironment = std::make_shared<MockEnvironment>(); + std::vector<std::shared_ptr<ControlStrategy>> control_strategies{}; + EXPECT_CALL(*mockEnvironment, UpdateControlStrategies(_, _)).WillOnce(SaveArg<1>(&control_strategies)); + + OpenScenarioEngine::v1_2::FollowTrajectoryAction followTrajectoryAction({std::vector<std::string>{"Vehicle1"}, + 0.0, + mantle_api::Trajectory{}, + {mantle_api::FollowTrajectoryControlStrategy::TrajectoryTimeReference{}}, + OpenScenarioEngine::v1_2::TrajectoryFollowingMode{}, + mantle_api::Trajectory{}}, + {mockEnvironment}); + + followTrajectoryAction.SetControlStrategy(); + ASSERT_THAT(control_strategies, testing::SizeIs(1)); + auto control_strategy = std::dynamic_pointer_cast<mantle_api::FollowTrajectoryControlStrategy>(control_strategies.front()); + + EXPECT_TRUE(control_strategy->timeReference.has_value()); + EXPECT_THAT(control_strategy->movement_domain, mantle_api::MovementDomain::kBoth); +} + +TEST(FollowTrajectoryAction, + GivenFollowTrajectoryActionWithoutTimeReference_WhenStartAction_ThenControlStrategyHasMovementDomainLateral) +{ + auto mockEnvironment = std::make_shared<MockEnvironment>(); + std::vector<std::shared_ptr<ControlStrategy>> control_strategies{}; + EXPECT_CALL(*mockEnvironment, UpdateControlStrategies(_, _)).WillOnce(SaveArg<1>(&control_strategies)); + + OpenScenarioEngine::v1_2::FollowTrajectoryAction followTrajectoryAction({std::vector<std::string>{"Vehicle1"}, + 0.0, + mantle_api::Trajectory{}, + std::nullopt, + OpenScenarioEngine::v1_2::TrajectoryFollowingMode{}, + mantle_api::Trajectory{}}, + {mockEnvironment}); + + followTrajectoryAction.SetControlStrategy(); + ASSERT_THAT(control_strategies, testing::SizeIs(1)); + auto control_strategy = std::dynamic_pointer_cast<mantle_api::FollowTrajectoryControlStrategy>(control_strategies.front()); + + EXPECT_FALSE(control_strategy->timeReference.has_value()); + EXPECT_THAT(control_strategy->movement_domain, mantle_api::MovementDomain::kLateral); +}