add waypoints to test

test/AssignRouteActionTest.cpp

@@ -13,9 +13,9 @@
 #include "builders/ActionBuilder.h"
 #include "builders/PositionBuilder.h"
 
-#include <openScenarioLib/generated/v1_1/impl/OpenScenarioWriterFactoryImplV1_1.h>
-
+#include <MantleAPI/Common/vector.h>
 #include <gtest/gtest.h>
+#include <openScenarioLib/generated/v1_1/impl/OpenScenarioWriterFactoryImplV1_1.h>
 
 namespace OPENSCENARIO
 {
@@ -27,23 +27,89 @@ class AssignRouteActionTestFixture : public OpenScenarioEngineLibraryTestBase
     void SetUp() override { OpenScenarioEngineLibraryTestBase::SetUp(); }
 };
 
+MATCHER_P(IsControlStrategySame, control_strategies, "Waypoints do not match")
+{
+    bool are_waypoints_equal = false;
+    for (size_t i = 0; i < control_strategies.size(); ++i)
+    {
+        auto follow_route_control_strategy =
+            std::dynamic_pointer_cast<mantle_api::FollowRouteControlStrategy>(control_strategies.at(i));
+        auto arg_ptr = std::dynamic_pointer_cast<mantle_api::FollowRouteControlStrategy>(arg.at(i));
+
+        if (follow_route_control_strategy != nullptr && arg_ptr != nullptr)
+        {
+            for (size_t j = 0; j < follow_route_control_strategy->waypoints.size(); ++j)
+            {
+                are_waypoints_equal = (arg_ptr->waypoints.at(j).x == follow_route_control_strategy->waypoints.at(j).x &&
+                                       arg_ptr->waypoints.at(j).y == follow_route_control_strategy->waypoints.at(j).y &&
+                                       arg_ptr->waypoints.at(j).z == follow_route_control_strategy->waypoints.at(j).z);
+                if (are_waypoints_equal == true)
+                {
+                    continue;
+                }
+                else
+                {
+                    return false;
+                }
+            }
+        }
+        else
+        {
+            return false;
+        }
+    }
+    return true;
+}
+
 TEST_F(AssignRouteActionTestFixture, GivenAssignRouteAction_WhenStartAction_ThenControlStrategySetAndNoExceptionInStep)
 {
     using namespace OPENSCENARIO::TESTING;
 
     NET_ASAM_OPENSCENARIO::v1_1::OpenScenarioWriterFactoryImpl open_scenario_writer_factory_impl;
 
-    auto fake_assign_route_action =
-        FakeAssignRouteActionBuilder{}
-            .WithRoute(open_scenario_writer_factory_impl.CreateRouteWriter())
-            .Build();
+    using namespace OPENSCENARIO::TESTING;
+    auto fake_assign_route_action = std::make_shared<NET_ASAM_OPENSCENARIO::v1_1::AssignRouteActionImpl>();
+
+    auto route = std::make_shared<NET_ASAM_OPENSCENARIO::v1_1::RouteImpl>();
+
+    auto way_point_one = std::make_shared<NET_ASAM_OPENSCENARIO::v1_1::WaypointImpl>();
+    auto way_point_two = std::make_shared<NET_ASAM_OPENSCENARIO::v1_1::WaypointImpl>();
+
+    auto position_one = std::make_shared<NET_ASAM_OPENSCENARIO::v1_1::PositionImpl>();
+    auto position_two = std::make_shared<NET_ASAM_OPENSCENARIO::v1_1::PositionImpl>();
+
+    auto world_position_one = std::make_shared<NET_ASAM_OPENSCENARIO::v1_1::WorldPositionImpl>();
+    auto world_position_two = std::make_shared<NET_ASAM_OPENSCENARIO::v1_1::WorldPositionImpl>();
+
+    world_position_one->SetX(1.0);
+    world_position_one->SetY(0.0);
+    world_position_one->SetZ(0.0);
+
+    world_position_two->SetX(42.0);
+    world_position_two->SetY(0.0);
+    world_position_two->SetZ(10.0);
+
+    position_one->SetWorldPosition(world_position_one);
+    position_two->SetWorldPosition(world_position_two);
+
+    way_point_one->SetPosition(position_one);
+    way_point_two->SetPosition(position_two);
+
+    std::vector<std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_1::IWaypointWriter>> route_way_points = {way_point_one,
+                                                                                                   way_point_two};
+    route->SetWaypoints(route_way_points);
+    fake_assign_route_action->SetRoute(route);
 
-    OPENSCENARIO::AssignRouteAction assign_route_action{fake_assign_route_action, env_, std::vector<std::string>{"Ego"}};
+    OPENSCENARIO::AssignRouteAction assign_route_action{
+        fake_assign_route_action, env_, std::vector<std::string>{"Ego"}};
 
     std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
+    auto route_control_strategy = std::make_unique<mantle_api::FollowRouteControlStrategy>();
+    route_control_strategy->waypoints.push_back(mantle_api::Vec3<units::length::meter_t>{1_m, 0_m, 0_m});
+    route_control_strategy->waypoints.push_back(mantle_api::Vec3<units::length::meter_t>{42_m, 0_m, 10_m});
+    control_strategies.push_back(std::move(route_control_strategy));
 
-    EXPECT_CALL(*env_,
-                UpdateControlStrategies(testing::_,testing::_))
+    EXPECT_CALL(*env_, UpdateControlStrategies(testing::_, IsControlStrategySame(control_strategies)))
         .WillOnce(testing::SaveArg<1>(&control_strategies));
 
     assign_route_action.Start();