diff --git a/test/AssignRouteActionTest.cpp b/test/AssignRouteActionTest.cpp index 34e499370c70c7241da5bd3aac256056199d5b32..f1280c0c32d3ef2ed634a1598fc2a8586525d7a2 100644 --- a/test/AssignRouteActionTest.cpp +++ b/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();