diff --git a/backend/src/plugins/action/action_service_impl.h b/backend/src/plugins/action/action_service_impl.h index e226db0094..e074ae821c 100644 --- a/backend/src/plugins/action/action_service_impl.h +++ b/backend/src/plugins/action/action_service_impl.h @@ -5,61 +5,178 @@ namespace dronecore { namespace backend { template -class ActionServiceImpl final : public dronecore::rpc::action::ActionService::Service +class ActionServiceImpl final : public rpc::action::ActionService::Service { public: - ActionServiceImpl(const Action &action) + ActionServiceImpl(Action &action) : _action(action) {} grpc::Status Arm(grpc::ServerContext * /* context */, - const dronecore::rpc::action::ArmRequest * /* request */, - dronecore::rpc::action::ArmResponse *response) + const rpc::action::ArmRequest * /* request */, + rpc::action::ArmResponse *response) override { auto action_result = _action.arm(); - auto rpc_result = static_cast(action_result); - auto *rpc_action_result = new dronecore::rpc::action::ActionResult(); + if (response != nullptr) { + fillResponseWithResult(response, action_result); + } + + return grpc::Status::OK; + } + + template + void fillResponseWithResult(ResponseType *response, ActionResult &action_result) const + { + auto rpc_result = static_cast(action_result); + + auto *rpc_action_result = new rpc::action::ActionResult(); rpc_action_result->set_result(rpc_result); - rpc_action_result->set_result_str(dronecore::action_result_str(action_result)); + rpc_action_result->set_result_str(action_result_str(action_result)); response->set_allocated_action_result(rpc_action_result); + } + + grpc::Status Disarm(grpc::ServerContext * /* context */, + const rpc::action::DisarmRequest * /* request */, + rpc::action::DisarmResponse *response) override + { + auto action_result = _action.disarm(); + + if (response != nullptr) { + fillResponseWithResult(response, action_result); + } return grpc::Status::OK; } grpc::Status Takeoff(grpc::ServerContext * /* context */, - const dronecore::rpc::action::TakeoffRequest * /* request */, - dronecore::rpc::action::TakeoffResponse *response) + const rpc::action::TakeoffRequest * /* request */, + rpc::action::TakeoffResponse *response) override { auto action_result = _action.takeoff(); - auto rpc_result = static_cast(action_result); - auto *rpc_action_result = new dronecore::rpc::action::ActionResult(); - rpc_action_result->set_result(rpc_result); - rpc_action_result->set_result_str(dronecore::action_result_str(action_result)); - - response->set_allocated_action_result(rpc_action_result); + if (response != nullptr) { + fillResponseWithResult(response, action_result); + } return grpc::Status::OK; } grpc::Status Land(grpc::ServerContext * /* context */, - const dronecore::rpc::action::LandRequest * /* request */, - dronecore::rpc::action::LandResponse *response) + const rpc::action::LandRequest * /* request */, + rpc::action::LandResponse *response) override { auto action_result = _action.land(); - auto rpc_result = static_cast(action_result); - auto *rpc_action_result = new dronecore::rpc::action::ActionResult(); - rpc_action_result->set_result(rpc_result); - rpc_action_result->set_result_str(dronecore::action_result_str(action_result)); + if (response != nullptr) { + fillResponseWithResult(response, action_result); + } - response->set_allocated_action_result(rpc_action_result); + return grpc::Status::OK; + } + + grpc::Status Kill(grpc::ServerContext * /* context */, + const rpc::action::KillRequest * /* request */, + rpc::action::KillResponse *response) override + { + auto action_result = _action.kill(); + + if (response != nullptr) { + fillResponseWithResult(response, action_result); + } return grpc::Status::OK; } + + grpc::Status ReturnToLaunch(grpc::ServerContext * /* context */, + const rpc::action::ReturnToLaunchRequest * /* request */, + rpc::action::ReturnToLaunchResponse *response) override + { + auto action_result = _action.return_to_launch(); + + if (response != nullptr) { + fillResponseWithResult(response, action_result); + } + + return grpc::Status::OK; + } + + grpc::Status TransitionToFixedWings(grpc::ServerContext * /* context */, + const rpc::action::TransitionToFixedWingsRequest * /* request */, + rpc::action::TransitionToFixedWingsResponse *response) override + { + auto action_result = _action.transition_to_fixedwing(); + + if (response != nullptr) { + fillResponseWithResult(response, action_result); + } + + return grpc::Status::OK; + } + + grpc::Status TransitionToMulticopter(grpc::ServerContext * /* context */, + const rpc::action::TransitionToMulticopterRequest * /* request */, + rpc::action::TransitionToMulticopterResponse *response) override + { + auto action_result = _action.transition_to_multicopter(); + + if (response != nullptr) { + fillResponseWithResult(response, action_result); + } + + return grpc::Status::OK; + } + + grpc::Status GetTakeoffAltitude(grpc::ServerContext * /* context */, + const rpc::action::GetTakeoffAltitudeRequest * /* request */, + rpc::action::GetTakeoffAltitudeResponse *response) override + { + if (response != nullptr) { + auto takeoff_altitude = _action.get_takeoff_altitude_m(); + response->set_altitude_m(takeoff_altitude); + } + + return grpc::Status::OK; + } + + grpc::Status SetTakeoffAltitude(grpc::ServerContext * /* context */, + const rpc::action::SetTakeoffAltitudeRequest *request, + rpc::action::SetTakeoffAltitudeResponse * /* response */) override + { + if (request != nullptr) { + const auto requested_altitude = request->altitude_m(); + _action.set_takeoff_altitude(requested_altitude); + } + + return grpc::Status::OK; + } + + grpc::Status GetMaximumSpeed(grpc::ServerContext * /* context */, + const rpc::action::GetMaximumSpeedRequest * /* request */, + rpc::action::GetMaximumSpeedResponse *response) override + { + if (response != nullptr) { + auto max_speed = _action.get_max_speed_m_s(); + response->set_speed_m_s(max_speed); + } + + return grpc::Status::OK; + } + + grpc::Status SetMaximumSpeed(grpc::ServerContext * /* context */, + const rpc::action::SetMaximumSpeedRequest *request, + rpc::action::SetMaximumSpeedResponse * /* response */) override + { + if (request != nullptr) { + const auto requested_speed = request->speed_m_s(); + _action.set_max_speed(requested_speed); + } + + return grpc::Status::OK; + } + private: - const Action &_action; + Action &_action; }; } // namespace backend diff --git a/backend/test/action_service_impl_test.cpp b/backend/test/action_service_impl_test.cpp index 1bb0045f65..21cb4fe9d4 100644 --- a/backend/test/action_service_impl_test.cpp +++ b/backend/test/action_service_impl_test.cpp @@ -8,6 +8,7 @@ namespace { +using testing::_; using testing::NiceMock; using testing::Return; @@ -17,10 +18,20 @@ using ActionServiceImpl = dronecore::backend::ActionServiceImpl; using ActionResult = dronecore::rpc::action::ActionResult; using InputPair = std::pair; +static constexpr auto ARBITRARY_ALTITUDE = 42.42f; +static constexpr auto ARBITRARY_SPEED = 8.24f; + std::vector generateInputPairs(); std::string armAndGetTranslatedResult(dronecore::ActionResult arm_result); +std::string disarmAndGetTranslatedResult(dronecore::ActionResult disarm_result); std::string takeoffAndGetTranslatedResult(dronecore::ActionResult takeoff_result); std::string landAndGetTranslatedResult(dronecore::ActionResult land_result); +std::string killAndGetTranslatedResult(dronecore::ActionResult kill_result); +std::string returnToLaunchAndGetTranslatedResult(dronecore::ActionResult rtl_result); +std::string transitionToFWAndGetTranslatedResult(const dronecore::ActionResult + transition_to_fw_result); +std::string transitionToMCAndGetTranslatedResult(const dronecore::ActionResult + transition_to_fw_result); class ActionServiceImplTest : public ::testing::TestWithParam {}; @@ -43,6 +54,45 @@ std::string armAndGetTranslatedResult(const dronecore::ActionResult arm_result) return ActionResult::Result_Name(response.action_result().result()); } +TEST_F(ActionServiceImplTest, armsEvenWhenArgsAreNull) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, arm()) + .Times(1); + + actionService.Arm(nullptr, nullptr, nullptr); +} + +TEST_P(ActionServiceImplTest, disarmResultIsTranslatedCorrectly) +{ + const auto rpc_result = disarmAndGetTranslatedResult(GetParam().second); + EXPECT_EQ(rpc_result, GetParam().first); +} + +std::string disarmAndGetTranslatedResult(dronecore::ActionResult disarm_result) +{ + MockAction action; + ON_CALL(action, disarm()) + .WillByDefault(Return(disarm_result)); + ActionServiceImpl actionService(action); + dronecore::rpc::action::DisarmResponse response; + + actionService.Disarm(nullptr, nullptr, &response); + + return ActionResult::Result_Name(response.action_result().result()); +} + +TEST_F(ActionServiceImplTest, disarmsEvenWhenArgsAreNull) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, disarm()) + .Times(1); + + actionService.Disarm(nullptr, nullptr, nullptr); +} + TEST_P(ActionServiceImplTest, takeoffResultIsTranslatedCorrectly) { const auto rpc_result = takeoffAndGetTranslatedResult(GetParam().second); @@ -62,6 +112,16 @@ std::string takeoffAndGetTranslatedResult(const dronecore::ActionResult takeoff_ return ActionResult::Result_Name(response.action_result().result()); } +TEST_F(ActionServiceImplTest, takeoffEvenWhenArgsAreNull) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, takeoff()) + .Times(1); + + actionService.Takeoff(nullptr, nullptr, nullptr); +} + TEST_P(ActionServiceImplTest, landResultIsTranslatedCorrectly) { const auto rpc_result = landAndGetTranslatedResult(GetParam().second); @@ -81,6 +141,263 @@ std::string landAndGetTranslatedResult(const dronecore::ActionResult land_result return ActionResult::Result_Name(response.action_result().result()); } +TEST_F(ActionServiceImplTest, landsEvenWhenArgsAreNull) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, land()) + .Times(1); + + actionService.Land(nullptr, nullptr, nullptr); +} + +TEST_P(ActionServiceImplTest, killResultIsTranslatedCorrectly) +{ + const auto rpc_result = killAndGetTranslatedResult(GetParam().second); + EXPECT_EQ(rpc_result, GetParam().first); +} + +std::string killAndGetTranslatedResult(const dronecore::ActionResult kill_result) +{ + MockAction action; + ON_CALL(action, kill()) + .WillByDefault(Return(kill_result)); + ActionServiceImpl actionService(action); + dronecore::rpc::action::KillResponse response; + + actionService.Kill(nullptr, nullptr, &response); + + return ActionResult::Result_Name(response.action_result().result()); +} + +TEST_F(ActionServiceImplTest, killsEvenWhenArgsAreNull) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, kill()) + .Times(1); + + actionService.Kill(nullptr, nullptr, nullptr); +} + +TEST_P(ActionServiceImplTest, rtlResultIsTranslatedCorrectly) +{ + const auto rpc_result = returnToLaunchAndGetTranslatedResult(GetParam().second); + EXPECT_EQ(rpc_result, GetParam().first); +} + +std::string returnToLaunchAndGetTranslatedResult(const dronecore::ActionResult rtl_result) +{ + MockAction action; + ON_CALL(action, return_to_launch()) + .WillByDefault(Return(rtl_result)); + ActionServiceImpl actionService(action); + dronecore::rpc::action::ReturnToLaunchResponse response; + + actionService.ReturnToLaunch(nullptr, nullptr, &response); + + return ActionResult::Result_Name(response.action_result().result()); +} + +TEST_F(ActionServiceImplTest, rtlsEvenWhenArgsAreNull) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, return_to_launch()) + .Times(1); + + actionService.ReturnToLaunch(nullptr, nullptr, nullptr); +} + +TEST_P(ActionServiceImplTest, transition2fwResultIsTranslatedCorrectly) +{ + const auto rpc_result = transitionToFWAndGetTranslatedResult(GetParam().second); + EXPECT_EQ(rpc_result, GetParam().first); +} + +std::string transitionToFWAndGetTranslatedResult(const dronecore::ActionResult + transition_to_fw_result) +{ + MockAction action; + ON_CALL(action, transition_to_fixedwing()) + .WillByDefault(Return(transition_to_fw_result)); + ActionServiceImpl actionService(action); + dronecore::rpc::action::TransitionToFixedWingsResponse response; + + actionService.TransitionToFixedWings(nullptr, nullptr, &response); + + return ActionResult::Result_Name(response.action_result().result()); +} + +TEST_F(ActionServiceImplTest, transitions2fwEvenWhenArgsAreNull) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, transition_to_fixedwing()) + .Times(1); + + actionService.TransitionToFixedWings(nullptr, nullptr, nullptr); +} + +TEST_P(ActionServiceImplTest, transition2mcResultIsTranslatedCorrectly) +{ + const auto rpc_result = transitionToMCAndGetTranslatedResult(GetParam().second); + EXPECT_EQ(rpc_result, GetParam().first); +} + +std::string transitionToMCAndGetTranslatedResult(const dronecore::ActionResult + transition_to_mc_result) +{ + MockAction action; + ON_CALL(action, transition_to_multicopter()) + .WillByDefault(Return(transition_to_mc_result)); + ActionServiceImpl actionService(action); + dronecore::rpc::action::TransitionToMulticopterResponse response; + + actionService.TransitionToMulticopter(nullptr, nullptr, &response); + + return ActionResult::Result_Name(response.action_result().result()); +} + +TEST_F(ActionServiceImplTest, transitions2mcEvenWhenArgsAreNull) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, transition_to_multicopter()) + .Times(1); + + actionService.TransitionToMulticopter(nullptr, nullptr, nullptr); +} + +TEST_F(ActionServiceImplTest, getTakeoffAltitudeCallsGetter) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, get_takeoff_altitude_m()) + .Times(1); + dronecore::rpc::action::GetTakeoffAltitudeResponse response; + + actionService.GetTakeoffAltitude(nullptr, nullptr, &response); +} + +TEST_F(ActionServiceImplTest, getsCorrectTakeoffAltitude) +{ + MockAction action; + ActionServiceImpl actionService(action); + const float expected_altitude = ARBITRARY_ALTITUDE; + ON_CALL(action, get_takeoff_altitude_m()) + .WillByDefault(Return(expected_altitude)); + dronecore::rpc::action::GetTakeoffAltitudeResponse response; + + actionService.GetTakeoffAltitude(nullptr, nullptr, &response); + + EXPECT_EQ(expected_altitude, response.altitude_m()); +} + +TEST_F(ActionServiceImplTest, getTakeoffAltitudeDoesNotCrashWithNullResponse) +{ + MockAction action; + ActionServiceImpl actionService(action); + + actionService.GetTakeoffAltitude(nullptr, nullptr, nullptr); +} + +TEST_F(ActionServiceImplTest, setTakeoffAltitudeDoesNotCrashWithNullRequest) +{ + MockAction action; + ActionServiceImpl actionService(action); + + actionService.SetTakeoffAltitude(nullptr, nullptr, nullptr); +} + +TEST_F(ActionServiceImplTest, setTakeoffAltitudeCallsSetter) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, set_takeoff_altitude(_)) + .Times(1); + dronecore::rpc::action::SetTakeoffAltitudeRequest request; + + actionService.SetTakeoffAltitude(nullptr, &request, nullptr); +} + +TEST_F(ActionServiceImplTest, setTakeoffAltitudeSetsRightValue) +{ + MockAction action; + ActionServiceImpl actionService(action); + float expected_altitude = ARBITRARY_ALTITUDE; + EXPECT_CALL(action, set_takeoff_altitude(expected_altitude)) + .Times(1); + dronecore::rpc::action::SetTakeoffAltitudeRequest request; + request.set_altitude_m(expected_altitude); + + actionService.SetTakeoffAltitude(nullptr, &request, nullptr); +} + +TEST_F(ActionServiceImplTest, getMaxSpeedDoesNotCrashWithNullResponse) +{ + MockAction action; + ActionServiceImpl actionService(action); + + actionService.GetMaximumSpeed(nullptr, nullptr, nullptr); +} + +TEST_F(ActionServiceImplTest, getMaxSpeedCallsGetter) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, get_max_speed_m_s()) + .Times(1); + dronecore::rpc::action::GetMaximumSpeedResponse response; + + actionService.GetMaximumSpeed(nullptr, nullptr, &response); +} + +TEST_F(ActionServiceImplTest, getMaxSpeedGetsRightValue) +{ + MockAction action; + ActionServiceImpl actionService(action); + const auto expected_max_speed = ARBITRARY_SPEED; + ON_CALL(action, get_max_speed_m_s()) + .WillByDefault(Return(expected_max_speed)); + dronecore::rpc::action::GetMaximumSpeedResponse response; + + actionService.GetMaximumSpeed(nullptr, nullptr, &response); + + EXPECT_EQ(expected_max_speed, response.speed_m_s()); +} + +TEST_F(ActionServiceImplTest, setMaxSpeedDoesNotCrashWithNullRequest) +{ + MockAction action; + ActionServiceImpl actionService(action); + + actionService.SetMaximumSpeed(nullptr, nullptr, nullptr); +} + +TEST_F(ActionServiceImplTest, setMaxSpeedCallsSetter) +{ + MockAction action; + ActionServiceImpl actionService(action); + EXPECT_CALL(action, set_max_speed(_)) + .Times(1); + dronecore::rpc::action::SetMaximumSpeedRequest request; + + actionService.SetMaximumSpeed(nullptr, &request, nullptr); +} + +TEST_F(ActionServiceImplTest, setMaxSpeedSetsRightValue) +{ + MockAction action; + ActionServiceImpl actionService(action); + const auto expected_speed = ARBITRARY_SPEED; + EXPECT_CALL(action, set_max_speed(expected_speed)) + .Times(1); + dronecore::rpc::action::SetMaximumSpeedRequest request; + request.set_speed_m_s(expected_speed); + + actionService.SetMaximumSpeed(nullptr, &request, nullptr); +} INSTANTIATE_TEST_CASE_P(ActionResultCorrespondences, ActionServiceImplTest, diff --git a/plugins/action/mocks/action_mock.h b/plugins/action/mocks/action_mock.h index b19e264c9d..9214845ee1 100644 --- a/plugins/action/mocks/action_mock.h +++ b/plugins/action/mocks/action_mock.h @@ -9,8 +9,17 @@ class MockAction { public: MOCK_CONST_METHOD0(arm, ActionResult()); + MOCK_CONST_METHOD0(disarm, ActionResult()); MOCK_CONST_METHOD0(takeoff, ActionResult()); MOCK_CONST_METHOD0(land, ActionResult()); + MOCK_CONST_METHOD0(kill, ActionResult()); + MOCK_CONST_METHOD0(return_to_launch, ActionResult()); + MOCK_CONST_METHOD0(transition_to_fixedwing, ActionResult()); + MOCK_CONST_METHOD0(transition_to_multicopter, ActionResult()); + MOCK_CONST_METHOD0(get_takeoff_altitude_m, float()); + MOCK_CONST_METHOD1(set_takeoff_altitude, void(float)); + MOCK_CONST_METHOD0(get_max_speed_m_s, float()); + MOCK_CONST_METHOD1(set_max_speed, void(float)); }; } // namespace testing