Port MotionControlActionTest

engine/tests/Storyboard/MotionControlAction/MotionControlActionTest.cpp

+/*******************************************************************************
+ * Copyright (c) 2023, 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 https://www.eclipse.org/legal/epl-2.0/
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *******************************************************************************/
+
+#include "Storyboard/MotionControlAction/MotionControlAction.h"
+#include "Storyboard/MotionControlAction/LongitudinalDistanceAction.h"
+#include "Storyboard/MotionControlAction/LateralDistanceAction.h"
+#include "TestUtils.h"
+
+#include <gmock/gmock.h>
+
+namespace OPENSCENARIO
+{
+
+using OpenScenarioEngine::v1_1::MotionControlAction;
+using OpenScenarioEngine::v1_1::LongitudinalDistanceAction;
+using OpenScenarioEngine::v1_1::LateralDistanceAction;
+
+class MockLongitudinalDistanceAction : public LongitudinalDistanceAction
+{
+  public:
+    MockLongitudinalDistanceAction(LongitudinalDistanceAction::Values values, LongitudinalDistanceAction::Interfaces interfaces)
+        : LongitudinalDistanceAction(values, interfaces) {}
+    MOCK_METHOD(bool, HasControlStrategyGoalBeenReached, (const std::string&), (override));
+};
+
+class MockLateralDistanceAction : public LateralDistanceAction
+{
+  public:
+    MockLateralDistanceAction(LateralDistanceAction::Values values, LateralDistanceAction::Interfaces interfaces)
+        : LateralDistanceAction(values, interfaces) {}
+    MOCK_METHOD(bool, HasControlStrategyGoalBeenReached, (const std::string&), (override));
+};
+
+class MotionControlActionTestFixture : public OpenScenarioEngineLibraryTestBase
+{
+  protected:
+    void SetUp() override { OpenScenarioEngineLibraryTestBase::SetUp(); }
+};
+
+TEST_F(MotionControlActionTestFixture, GivenLongitudinalAction_WhenControlStrategyGoalReached_ThenResetLongitudinal)
+{
+    MotionControlAction<MockLongitudinalDistanceAction> mock_motion_control_action(
+        LongitudinalDistanceAction::Values{
+            {"Ego"},
+            false,
+            false,
+            [=]()
+            {
+              return OPENSCENARIO::ConvertScenarioLongitudinalDistance(
+                  env_,
+                  true,
+                  10.0,
+                  false,
+                  0.0,
+                  {nullptr});
+            },
+            OPENSCENARIO::LongitudinalDisplacement{},
+            OPENSCENARIO::DynamicConstraints{},
+            "Ego",
+            OPENSCENARIO::CoordinateSystem{}},
+        LongitudinalDistanceAction::Interfaces{env_});
+
+    auto expected_control_strategy = mantle_api::KeepVelocityControlStrategy();
+
+    EXPECT_CALL(*env_, UpdateControlStrategies(0, testing::_))
+        .Times(1)
+        .WillRepeatedly([expected_control_strategy](
+                            std::uint64_t controller_id,
+                            std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies) {
+            std::ignore = controller_id;
+            EXPECT_EQ(1, control_strategies.size());
+            auto control_strategy = dynamic_cast<mantle_api::KeepVelocityControlStrategy*>(control_strategies[0].get());
+            EXPECT_NE(nullptr, control_strategy);
+            EXPECT_EQ(*control_strategy, expected_control_strategy);
+        });
+
+    mock_motion_control_action.Step();
+}
+
+TEST_F(MotionControlActionTestFixture, GivenLateralAction_WhenControlStrategyGoalReached_ThenResetLateral)
+{
+    MotionControlAction<MockLateralDistanceAction> mock_motion_control_action(
+        LateralDistanceAction::Values{
+            {"Ego"},
+            false,
+            10.0,
+            false,
+            OPENSCENARIO::DynamicConstraints{},
+            "Ego",
+            OPENSCENARIO::CoordinateSystem{}},
+        LateralDistanceAction::Interfaces{env_});
+    EXPECT_CALL(mock_motion_control_action, HasControlStrategyGoalBeenReached("Ego")).WillOnce(::testing::Return(true));
+
+    auto expected_control_strategy = mantle_api::KeepLaneOffsetControlStrategy();
+
+    EXPECT_CALL(*env_, UpdateControlStrategies(0, testing::_))
+        .Times(1)
+        .WillRepeatedly(
+            [expected_control_strategy](std::uint64_t controller_id,
+                                        std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies) {
+                std::ignore = controller_id;
+                EXPECT_EQ(1, control_strategies.size());
+                auto control_strategy =
+                    dynamic_cast<mantle_api::KeepLaneOffsetControlStrategy*>(control_strategies[0].get());
+                EXPECT_NE(nullptr, control_strategy);
+                EXPECT_EQ(*control_strategy, expected_control_strategy);
+            });
+
+    mock_motion_control_action.Step();
+}
+
+}  // namespace OPENSCENARIO