diff --git a/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp b/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp index 1df61f2097f65355f8896121f5fce97810dd6b44..f05a7937c37c69739ae22eea5993c8ad6a8db588 100644 --- a/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp +++ b/engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp @@ -10,8 +10,11 @@ #include "Storyboard/MotionControlAction/FollowTrajectoryAction_impl.h" +#include <MantleAPI/Common/clothoid_spline.h> #include <MantleAPI/Traffic/entity_helper.h> +#include <variant> + #include "Utils/EntityUtils.h" namespace OpenScenarioEngine::v1_3 @@ -39,8 +42,41 @@ mantle_api::Trajectory ConvertPolyLine( return trajectoryRef.value(); } +mantle_api::Trajectory ConvertClothoidSpline( + 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; + + auto& clothoid_spline = std::get<mantle_api::ClothoidSpline>(trajectoryRef->type); + + for (auto& segment : clothoid_spline.segments) + { + if (segment.position_start.has_value()) + { + segment.position_start.value().position = environment->GetGeometryHelper()->TranslateGlobalPositionLocally( + segment.position_start.value().position, + segment.position_start.value().orientation, + geometric_center); + } + } + + return trajectoryRef.value(); +} + } // namespace detail +template <class... Ts> +struct overloaded : Ts... +{ + using Ts::operator()...; +}; + +template <class... Ts> +overloaded(Ts...) -> overloaded<Ts...>; + void FollowTrajectoryAction::SetControlStrategy() { if (!values.trajectoryRef) @@ -57,7 +93,23 @@ void FollowTrajectoryAction::SetControlStrategy() control_strategy->movement_domain = values.timeReference ? mantle_api::MovementDomain::kBoth : mantle_api::MovementDomain::kLateral; - control_strategy->trajectory = detail::ConvertPolyLine(mantle.environment, entity, values.trajectoryRef); + + 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); + const auto entity_id = mantle.environment->GetEntityRepository().Get(entity)->get().GetUniqueId(); mantle.environment->UpdateControlStrategies(entity_id, {control_strategy}); } @@ -79,4 +131,4 @@ mantle_api::MovementDomain FollowTrajectoryAction::GetMovementDomain() const return mantle_api::MovementDomain::kBoth; } -} // namespace OpenScenarioEngine::v1_3 \ No newline at end of file +} // namespace OpenScenarioEngine::v1_3 diff --git a/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp b/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp index 87c4ce56b276513acf87e807c0d4e47e2fdc0638..9cab4e6614d775b41268a7fe30c7799fa5ec6c29 100644 --- a/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp +++ b/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp @@ -9,6 +9,8 @@ * SPDX-License-Identifier: EPL-2.0 ********************************************************************************/ +#include <MantleAPI/Common/clothoid_spline.h> +#include <MantleAPI/Common/pose.h> #include <MantleAPI/Test/test_utils.h> #include <gtest/gtest.h> @@ -434,7 +436,7 @@ TEST(FollowTrajectoryAction, GivenFollowTrajectoryAction_WhenNoTrajectoryRef_The EXPECT_THROW(followTrajectoryAction.SetControlStrategy(), std::runtime_error); } -TEST(FollowTrajectoryAction, GivenFollowTrajectoryActionAndTrajectoryRef_UpdatesControlStrategies) +TEST(FollowTrajectoryAction, GivenFollowTrajectoryActionAndTrajectoryRef_WhenPolylineShape_UpdatesControlStrategies) { Pose pose1{{1.0_m, 2.0_m, 0.0_m}, {0.1_rad, 0.0_rad, 0.0_rad}}; Pose pose2{{1.1_m, 2.1_m, 0.0_m}, {0.2_rad, 0.0_rad, 0.0_rad}}; @@ -471,6 +473,7 @@ TEST(FollowTrajectoryAction, GivenFollowTrajectoryActionAndTrajectoryRef_Updates EXPECT_THAT(control_strategies, testing::SizeIs(1)); EXPECT_EQ(control_strategies.front()->type, ControlStrategyType::kFollowTrajectory); auto& trajectory = std::dynamic_pointer_cast<FollowTrajectoryControlStrategy>(control_strategies.front())->trajectory; + ASSERT_TRUE(std::holds_alternative<PolyLine>(trajectory.type)); auto& polyline = std::get<PolyLine>(trajectory.type); EXPECT_THAT(polyline, testing::SizeIs(3)); EXPECT_EQ(polyline.at(0).pose, pose1); @@ -478,6 +481,51 @@ TEST(FollowTrajectoryAction, GivenFollowTrajectoryActionAndTrajectoryRef_Updates EXPECT_EQ(polyline.at(2).pose, pose3); } +TEST(FollowTrajectoryAction, GivenFollowTrajectoryActionAndTrajectoryRef_WhenClothoidSplineShape_UpdatesControlStrategies) +{ + mantle_api::ClothoidSpline expected_clothoid_spline; + expected_clothoid_spline.segments.push_back({1.1_curv, + 4.1_curv, + 3.7_rad, + 5.1_m, + mantle_api::Pose{{6.1_m, -1.5_m, 3.9_m}, {1.4_rad, 2.0_rad, -1.4_rad}}}); + + expected_clothoid_spline.segments.push_back({9.0_curv, + 6.8_curv, + 2.3_rad, + 1.9_m}); + + mantle_api::Trajectory expected_trajectory{}; + expected_trajectory.type = expected_clothoid_spline; + + auto mock_environment = std::make_shared<MockEnvironment>(); + auto mockGeometryHelper = static_cast<const MockGeometryHelper*>(mock_environment->GetGeometryHelper()); + + ON_CALL(*mockGeometryHelper, TranslateGlobalPositionLocally(expected_clothoid_spline.segments.front().position_start.value().position, _, _)).WillByDefault(Return(expected_clothoid_spline.segments.front().position_start.value().position)); + + std::vector<std::shared_ptr<ControlStrategy>> control_strategies{}; + EXPECT_CALL(*mock_environment, UpdateControlStrategies(_, _)).WillOnce(SaveArg<1>(&control_strategies)); + + OpenScenarioEngine::v1_3::FollowTrajectoryAction follow_trajectory_action({std::vector<std::string>{"Vehicle1"}, + 0.0, + mantle_api::Trajectory{}, + OpenScenarioEngine::v1_3::TimeReference{}, + OpenScenarioEngine::v1_3::TrajectoryFollowingMode{}, + expected_trajectory}, + {mock_environment}); + + EXPECT_NO_THROW(follow_trajectory_action.SetControlStrategy()); + + EXPECT_THAT(control_strategies, testing::SizeIs(1)); + EXPECT_EQ(control_strategies.front()->type, ControlStrategyType::kFollowTrajectory); + + auto& actual_trajectory = std::dynamic_pointer_cast<FollowTrajectoryControlStrategy>(control_strategies.front())->trajectory; + ASSERT_TRUE(std::holds_alternative<mantle_api::ClothoidSpline>(actual_trajectory.type)); + + auto& actual_clothoid_spline = std::get<mantle_api::ClothoidSpline>(actual_trajectory.type); + EXPECT_EQ(expected_clothoid_spline, actual_clothoid_spline); +} + TEST(FollowTrajectoryAction, GivenFollowTrajectoryActionWithTimeReference_WhenStartAction_ThenControlStrategyHasMovementDomainBoth) {