diff --git a/backend/proto b/backend/proto index d65288df81..71c2e73500 160000 --- a/backend/proto +++ b/backend/proto @@ -1 +1 @@ -Subproject commit d65288df81b68f270223db14d977ee813b317843 +Subproject commit 71c2e7350015f358f972d9b32a5b24cc500eba74 diff --git a/backend/src/CMakeLists.txt b/backend/src/CMakeLists.txt index fbbfc562f6..d8b6f05f02 100644 --- a/backend/src/CMakeLists.txt +++ b/backend/src/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.1) -set(COMPONENTS_LIST core action calibration gimbal camera mission telemetry info param) +set(COMPONENTS_LIST core action calibration gimbal camera mission offboard telemetry info param) include(cmake/compile_proto.cmake) @@ -41,6 +41,7 @@ target_link_libraries(backend dronecode_sdk_gimbal dronecode_sdk_camera dronecode_sdk_mission + dronecode_sdk_offboard dronecode_sdk_telemetry dronecode_sdk_info dronecode_sdk_param diff --git a/backend/src/core/core_service_impl.h b/backend/src/core/core_service_impl.h index 4e19e3b34d..ee12e6e844 100644 --- a/backend/src/core/core_service_impl.h +++ b/backend/src/core/core_service_impl.h @@ -43,15 +43,16 @@ class CoreServiceImpl final : public dronecode_sdk::rpc::core::CoreService::Serv const rpc::core::ListRunningPluginsRequest * /* request */, dronecode_sdk::rpc::core::ListRunningPluginsResponse *response) override { - std::string plugin_names[9] = {"action", - "calibration", - "camera", - "core", - "gimbal", - "info", - "mission", - "param", - "telemetry"}; + std::string plugin_names[10] = {"action", + "calibration", + "camera", + "core", + "gimbal", + "info", + "mission", + "offboard", + "param", + "telemetry"}; for (const auto plugin_name : plugin_names) { auto plugin_info = response->add_plugin_info(); diff --git a/backend/src/grpc_server.cpp b/backend/src/grpc_server.cpp index 6ee12cc062..a5b7241471 100644 --- a/backend/src/grpc_server.cpp +++ b/backend/src/grpc_server.cpp @@ -19,6 +19,7 @@ void GRPCServer::run() builder.RegisterService(&_gimbal_service); builder.RegisterService(&_camera_service); builder.RegisterService(&_mission_service); + builder.RegisterService(&_offboard_service); builder.RegisterService(&_telemetry_service); builder.RegisterService(&_info_service); builder.RegisterService(&_param_service); diff --git a/backend/src/grpc_server.h b/backend/src/grpc_server.h index 72101ad936..41e170fe0e 100644 --- a/backend/src/grpc_server.h +++ b/backend/src/grpc_server.h @@ -19,6 +19,8 @@ #include "gimbal/gimbal_service_impl.h" #include "plugins/param/param.h" #include "param/param_service_impl.h" +#include "plugins/offboard/offboard.h" +#include "offboard/offboard_service_impl.h" namespace dronecode_sdk { namespace backend { @@ -38,6 +40,8 @@ class GRPCServer { _camera_service(_camera), _mission(_dc.system()), _mission_service(_mission), + _offboard(_dc.system()), + _offboard_service(_offboard), _telemetry(_dc.system()), _telemetry_service(_telemetry), _info(_dc.system()), @@ -65,6 +69,8 @@ class GRPCServer { CameraServiceImpl<> _camera_service; Mission _mission; MissionServiceImpl<> _mission_service; + Offboard _offboard; + OffboardServiceImpl<> _offboard_service; Telemetry _telemetry; TelemetryServiceImpl<> _telemetry_service; Info _info; diff --git a/backend/src/plugins/offboard/offboard_service_impl.h b/backend/src/plugins/offboard/offboard_service_impl.h new file mode 100644 index 0000000000..1a9401b5bb --- /dev/null +++ b/backend/src/plugins/offboard/offboard_service_impl.h @@ -0,0 +1,171 @@ +#include "plugins/offboard/offboard.h" +#include "offboard/offboard.grpc.pb.h" + +namespace dronecode_sdk { +namespace backend { + +template +class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service { +public: + OffboardServiceImpl(Offboard &offboard) : _offboard(offboard) {} + + template + void fillResponseWithResult(ResponseType *response, + dronecode_sdk::Offboard::Result &offboard_result) const + { + auto rpc_result = static_cast(offboard_result); + + auto *rpc_offboard_result = new rpc::offboard::OffboardResult(); + rpc_offboard_result->set_result(rpc_result); + rpc_offboard_result->set_result_str(dronecode_sdk::Offboard::result_str(offboard_result)); + + response->set_allocated_offboard_result(rpc_offboard_result); + } + + grpc::Status Start(grpc::ServerContext * /* context */, + const rpc::offboard::StartRequest * /* request */, + rpc::offboard::StartResponse *response) override + { + auto offboard_result = _offboard.start(); + + if (response != nullptr) { + fillResponseWithResult(response, offboard_result); + } + + return grpc::Status::OK; + } + + grpc::Status Stop(grpc::ServerContext * /* context */, + const rpc::offboard::StopRequest * /* request */, + rpc::offboard::StopResponse *response) override + { + auto offboard_result = _offboard.stop(); + + if (response != nullptr) { + fillResponseWithResult(response, offboard_result); + } + + return grpc::Status::OK; + } + + grpc::Status IsActive(grpc::ServerContext * /* context */, + const rpc::offboard::IsActiveRequest * /* request */, + rpc::offboard::IsActiveResponse *response) override + { + if (response != nullptr) { + auto is_active = _offboard.is_active(); + response->set_is_active(is_active); + } + + return grpc::Status::OK; + } + + grpc::Status SetAttitudeRate(grpc::ServerContext * /* context */, + const rpc::offboard::SetAttitudeRateRequest *request, + rpc::offboard::SetAttitudeRateResponse * /* response */) override + { + if (request != nullptr) { + auto requested_attitude_rate = translateRPCAttitudeRate(request->attitude_rate()); + _offboard.set_attitude_rate(requested_attitude_rate); + } + + return grpc::Status::OK; + } + + static dronecode_sdk::Offboard::AttitudeRate + translateRPCAttitudeRate(const rpc::offboard::AttitudeRate &rpc_attitude_rate) + { + dronecode_sdk::Offboard::AttitudeRate attitude_rate; + + attitude_rate.roll_deg_s = rpc_attitude_rate.roll_deg_s(); + attitude_rate.pitch_deg_s = rpc_attitude_rate.pitch_deg_s(); + attitude_rate.yaw_deg_s = rpc_attitude_rate.yaw_deg_s(); + attitude_rate.thrust_value = rpc_attitude_rate.thrust_value(); + + return attitude_rate; + } + + grpc::Status SetPositionNed(grpc::ServerContext * /* context */, + const rpc::offboard::SetPositionNedRequest *request, + rpc::offboard::SetPositionNedResponse * /* response */) override + { + if (request != nullptr) { + auto requested_position_ned_yaw = + translateRPCPositionNEDYaw(request->position_ned_yaw()); + _offboard.set_position_ned(requested_position_ned_yaw); + } + + return grpc::Status::OK; + } + + static dronecode_sdk::Offboard::PositionNEDYaw + translateRPCPositionNEDYaw(const rpc::offboard::PositionNEDYaw &rpc_position_ned_yaw) + { + dronecode_sdk::Offboard::PositionNEDYaw position_ned_yaw; + + position_ned_yaw.north_m = rpc_position_ned_yaw.north_m(); + position_ned_yaw.east_m = rpc_position_ned_yaw.east_m(); + position_ned_yaw.down_m = rpc_position_ned_yaw.down_m(); + position_ned_yaw.yaw_deg = rpc_position_ned_yaw.yaw_deg(); + + return position_ned_yaw; + } + + grpc::Status SetVelocityBody(grpc::ServerContext * /* context */, + const rpc::offboard::SetVelocityBodyRequest *request, + rpc::offboard::SetVelocityBodyResponse * /* response */) override + { + if (request != nullptr) { + auto requested_velocity_body_yawspeed = + translateRPCVelocityBodyYawspeed(request->velocity_body_yawspeed()); + _offboard.set_velocity_body(requested_velocity_body_yawspeed); + } + + return grpc::Status::OK; + } + + static dronecode_sdk::Offboard::VelocityBodyYawspeed translateRPCVelocityBodyYawspeed( + const rpc::offboard::VelocityBodyYawspeed &rpc_velocity_body_yawspeed) + { + dronecode_sdk::Offboard::VelocityBodyYawspeed velocity_body_yawspeed; + + velocity_body_yawspeed.forward_m_s = rpc_velocity_body_yawspeed.forward_m_s(); + velocity_body_yawspeed.right_m_s = rpc_velocity_body_yawspeed.right_m_s(); + velocity_body_yawspeed.down_m_s = rpc_velocity_body_yawspeed.down_m_s(); + velocity_body_yawspeed.yawspeed_deg_s = rpc_velocity_body_yawspeed.yawspeed_deg_s(); + + return velocity_body_yawspeed; + } + + grpc::Status SetVelocityNed(grpc::ServerContext * /* context */, + const rpc::offboard::SetVelocityNedRequest *request, + rpc::offboard::SetVelocityNedResponse * /* response */) override + { + if (request != nullptr) { + auto requested_velocity_ned_yaw = + translateRPCVelocityNEDYaw(request->velocity_ned_yaw()); + _offboard.set_velocity_ned(requested_velocity_ned_yaw); + } + + return grpc::Status::OK; + } + + static dronecode_sdk::Offboard::VelocityNEDYaw + translateRPCVelocityNEDYaw(const rpc::offboard::VelocityNEDYaw &rpc_velocity_ned_yaw) + { + dronecode_sdk::Offboard::VelocityNEDYaw velocity_ned_yaw; + + velocity_ned_yaw.north_m_s = rpc_velocity_ned_yaw.north_m_s(); + velocity_ned_yaw.east_m_s = rpc_velocity_ned_yaw.east_m_s(); + velocity_ned_yaw.down_m_s = rpc_velocity_ned_yaw.down_m_s(); + velocity_ned_yaw.yaw_deg = rpc_velocity_ned_yaw.yaw_deg(); + + return velocity_ned_yaw; + } + +private: + Offboard &_offboard; +}; + +} // namespace backend +} // namespace dronecode_sdk diff --git a/backend/test/CMakeLists.txt b/backend/test/CMakeLists.txt index 3318c40dff..80c6f1fd6c 100644 --- a/backend/test/CMakeLists.txt +++ b/backend/test/CMakeLists.txt @@ -7,6 +7,7 @@ add_executable(unit_tests_backend connection_initiator_test.cpp core_service_impl_test.cpp mission_service_impl_test.cpp + offboard_service_impl_test.cpp telemetry_service_impl_test.cpp info_service_impl_test.cpp ) diff --git a/backend/test/offboard_service_impl_test.cpp b/backend/test/offboard_service_impl_test.cpp new file mode 100644 index 0000000000..9106cc3514 --- /dev/null +++ b/backend/test/offboard_service_impl_test.cpp @@ -0,0 +1,359 @@ +#include +#include +#include + +#include "offboard/offboard_service_impl.h" +#include "offboard/mocks/offboard_mock.h" + +namespace { + +using testing::_; +using testing::NiceMock; +using testing::Return; + +using MockOffboard = NiceMock; +using OffboardServiceImpl = dronecode_sdk::backend::OffboardServiceImpl; +using OffboardResult = dronecode_sdk::rpc::offboard::OffboardResult; +using InputPair = std::pair; + +static constexpr float ARBITRARY_ROLL = 2.5f; +static constexpr float ARBITRARY_PITCH = 4.0f; +static constexpr float ARBITRARY_YAW = 3.7f; +static constexpr float ARBITRARY_THRUST = 0.5f; +static constexpr float ARBITRARY_NORTH_M = 10.54f; +static constexpr float ARBITRARY_EAST_M = 5.62f; +static constexpr float ARBITRARY_DOWN_M = 1.44f; +static constexpr float ARBITRARY_VELOCITY_LOW = 1.7f; +static constexpr float ARBITRARY_VELOCITY_MID = 7.3f; +static constexpr float ARBITRARY_VELOCITY_HIGH = 14.6f; +static constexpr float ARBITRARY_VELOCITY_NEG = -0.5f; +static constexpr float ARBITRARY_YAWSPEED = 3.1f; + +std::vector generateInputPairs(); +std::string startAndGetTranslatedResult(dronecode_sdk::Offboard::Result start_result); +std::string stopAndGetTranslatedResult(dronecode_sdk::Offboard::Result stop_result); + +class OffboardServiceImplTest : public ::testing::TestWithParam { +protected: + void checkReturnsCorrectIsActiveStatus(const bool expected_is_active_status); + + std::unique_ptr + createArbitraryRPCAttitudeRate() const; + std::unique_ptr + createArbitraryRPCPositionNEDYaw() const; + std::unique_ptr + createArbitraryRPCVelocityBodyYawspeed() const; + std::unique_ptr + createArbitraryRPCVelocityNedYaw() const; +}; + +TEST_P(OffboardServiceImplTest, startResultIsTranslatedCorrectly) +{ + const auto rpc_result = startAndGetTranslatedResult(GetParam().second); + EXPECT_EQ(rpc_result, GetParam().first); +} + +std::string startAndGetTranslatedResult(const dronecode_sdk::Offboard::Result start_result) +{ + MockOffboard offboard; + ON_CALL(offboard, start()).WillByDefault(Return(start_result)); + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::StartResponse response; + + offboardService.Start(nullptr, nullptr, &response); + + return OffboardResult::Result_Name(response.offboard_result().result()); +} + +TEST_F(OffboardServiceImplTest, startsEvenWhenArgsAreNull) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + EXPECT_CALL(offboard, start()).Times(1); + + offboardService.Start(nullptr, nullptr, nullptr); +} + +TEST_P(OffboardServiceImplTest, stopResultIsTranslatedCorrectly) +{ + const auto rpc_result = stopAndGetTranslatedResult(GetParam().second); + EXPECT_EQ(rpc_result, GetParam().first); +} + +std::string stopAndGetTranslatedResult(const dronecode_sdk::Offboard::Result stop_result) +{ + MockOffboard offboard; + ON_CALL(offboard, stop()).WillByDefault(Return(stop_result)); + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::StopResponse response; + + offboardService.Stop(nullptr, nullptr, &response); + + return OffboardResult::Result_Name(response.offboard_result().result()); +} + +TEST_F(OffboardServiceImplTest, stopsEvenWhenArgsAreNull) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + EXPECT_CALL(offboard, stop()).Times(1); + + offboardService.Stop(nullptr, nullptr, nullptr); +} + +TEST_F(OffboardServiceImplTest, isActiveCallsGetter) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + EXPECT_CALL(offboard, is_active()).Times(1); + dronecode_sdk::rpc::offboard::IsActiveResponse response; + + offboardService.IsActive(nullptr, nullptr, &response); +} + +TEST_F(OffboardServiceImplTest, isActiveGetsCorrectValue) +{ + checkReturnsCorrectIsActiveStatus(false); + checkReturnsCorrectIsActiveStatus(true); +} + +void OffboardServiceImplTest::checkReturnsCorrectIsActiveStatus( + const bool expected_is_active_status) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + ON_CALL(offboard, is_active()).WillByDefault(Return(expected_is_active_status)); + dronecode_sdk::rpc::offboard::IsActiveResponse response; + + offboardService.IsActive(nullptr, nullptr, &response); + + EXPECT_EQ(expected_is_active_status, response.is_active()); +} + +TEST_F(OffboardServiceImplTest, isActiveDoesNotCrashWithNullResponse) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + + offboardService.IsActive(nullptr, nullptr, nullptr); +} + +TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithAllNullParams) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + + offboardService.SetAttitudeRate(nullptr, nullptr, nullptr); +} + +TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithNullResponse) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::SetAttitudeRateRequest request; + + auto rpc_attitude_rate = createArbitraryRPCAttitudeRate(); + request.set_allocated_attitude_rate(rpc_attitude_rate.release()); + + offboardService.SetAttitudeRate(nullptr, &request, nullptr); +} + +std::unique_ptr +OffboardServiceImplTest::createArbitraryRPCAttitudeRate() const +{ + auto rpc_attitude_rate = std::unique_ptr( + new dronecode_sdk::rpc::offboard::AttitudeRate()); + rpc_attitude_rate->set_roll_deg_s(ARBITRARY_ROLL); + rpc_attitude_rate->set_pitch_deg_s(ARBITRARY_PITCH); + rpc_attitude_rate->set_yaw_deg_s(ARBITRARY_YAW); + rpc_attitude_rate->set_thrust_value(ARBITRARY_THRUST); + + return rpc_attitude_rate; +} + +TEST_F(OffboardServiceImplTest, setsAttitudeRateCorrectly) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::SetAttitudeRateRequest request; + + auto rpc_attitude_rate = createArbitraryRPCAttitudeRate(); + const auto expected_attitude_rate = + OffboardServiceImpl::translateRPCAttitudeRate(*rpc_attitude_rate); + EXPECT_CALL(offboard, set_attitude_rate(expected_attitude_rate)).Times(1); + + request.set_allocated_attitude_rate(rpc_attitude_rate.release()); + + offboardService.SetAttitudeRate(nullptr, &request, nullptr); +} + +TEST_F(OffboardServiceImplTest, setPositionNEDYawDoesNotFailWithAllNullParams) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + + offboardService.SetPositionNed(nullptr, nullptr, nullptr); +} + +TEST_F(OffboardServiceImplTest, setPositionNEDYawDoesNotFailWithNullResponse) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::SetPositionNedRequest request; + + auto rpc_position_ned_yaw = createArbitraryRPCPositionNEDYaw(); + request.set_allocated_position_ned_yaw(rpc_position_ned_yaw.release()); + + offboardService.SetPositionNed(nullptr, &request, nullptr); +} + +std::unique_ptr +OffboardServiceImplTest::createArbitraryRPCPositionNEDYaw() const +{ + auto rpc_position_ned_yaw = std::unique_ptr( + new dronecode_sdk::rpc::offboard::PositionNEDYaw()); + rpc_position_ned_yaw->set_north_m(ARBITRARY_NORTH_M); + rpc_position_ned_yaw->set_east_m(ARBITRARY_EAST_M); + rpc_position_ned_yaw->set_down_m(ARBITRARY_DOWN_M); + rpc_position_ned_yaw->set_yaw_deg(ARBITRARY_YAW); + + return rpc_position_ned_yaw; +} + +TEST_F(OffboardServiceImplTest, setsPositionNEDYawCorrectly) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::SetPositionNedRequest request; + + auto rpc_position_ned_yaw = createArbitraryRPCPositionNEDYaw(); + const auto expected_position_ned_yaw = + OffboardServiceImpl::translateRPCPositionNEDYaw(*rpc_position_ned_yaw); + EXPECT_CALL(offboard, set_position_ned(expected_position_ned_yaw)).Times(1); + + request.set_allocated_position_ned_yaw(rpc_position_ned_yaw.release()); + + offboardService.SetPositionNed(nullptr, &request, nullptr); +} + +TEST_F(OffboardServiceImplTest, setVelocityBodyDoesNotFailWithAllNullParams) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + + offboardService.SetVelocityBody(nullptr, nullptr, nullptr); +} + +TEST_F(OffboardServiceImplTest, setVelocityBodyDoesNotFailWithNullResponse) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::SetVelocityBodyRequest request; + + auto rpc_velocity_body = createArbitraryRPCVelocityBodyYawspeed(); + request.set_allocated_velocity_body_yawspeed(rpc_velocity_body.release()); + + offboardService.SetVelocityBody(nullptr, &request, nullptr); +} + +std::unique_ptr +OffboardServiceImplTest::createArbitraryRPCVelocityBodyYawspeed() const +{ + auto rpc_velocity_body = std::unique_ptr( + new dronecode_sdk::rpc::offboard::VelocityBodyYawspeed()); + rpc_velocity_body->set_forward_m_s(ARBITRARY_VELOCITY_HIGH); + rpc_velocity_body->set_right_m_s(ARBITRARY_VELOCITY_LOW); + rpc_velocity_body->set_down_m_s(ARBITRARY_VELOCITY_NEG); + rpc_velocity_body->set_yawspeed_deg_s(ARBITRARY_YAWSPEED); + + return rpc_velocity_body; +} + +TEST_F(OffboardServiceImplTest, setsVelocityBodyCorrectly) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::SetVelocityBodyRequest request; + + auto rpc_velocity_body = createArbitraryRPCVelocityBodyYawspeed(); + const auto expected_velocity_body = + OffboardServiceImpl::translateRPCVelocityBodyYawspeed(*rpc_velocity_body); + EXPECT_CALL(offboard, set_velocity_body(expected_velocity_body)).Times(1); + + request.set_allocated_velocity_body_yawspeed(rpc_velocity_body.release()); + + offboardService.SetVelocityBody(nullptr, &request, nullptr); +} + +TEST_F(OffboardServiceImplTest, setVelocityNedDoesNotFailWithAllNullParams) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + + offboardService.SetVelocityNed(nullptr, nullptr, nullptr); +} + +TEST_F(OffboardServiceImplTest, setVelocityNedDoesNotFailWithNullResponse) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::SetVelocityNedRequest request; + + auto rpc_velocity_ned = createArbitraryRPCVelocityNedYaw(); + request.set_allocated_velocity_ned_yaw(rpc_velocity_ned.release()); + + offboardService.SetVelocityNed(nullptr, &request, nullptr); +} + +std::unique_ptr +OffboardServiceImplTest::createArbitraryRPCVelocityNedYaw() const +{ + auto rpc_velocity_ned = std::unique_ptr( + new dronecode_sdk::rpc::offboard::VelocityNEDYaw()); + rpc_velocity_ned->set_north_m_s(ARBITRARY_VELOCITY_MID); + rpc_velocity_ned->set_east_m_s(ARBITRARY_VELOCITY_LOW); + rpc_velocity_ned->set_down_m_s(ARBITRARY_VELOCITY_NEG); + rpc_velocity_ned->set_yaw_deg(ARBITRARY_YAW); + + return rpc_velocity_ned; +} + +TEST_F(OffboardServiceImplTest, setsVelocityNedCorrectly) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::SetVelocityNedRequest request; + + auto rpc_velocity_ned = createArbitraryRPCVelocityNedYaw(); + const auto expected_velocity_ned = + OffboardServiceImpl::translateRPCVelocityNEDYaw(*rpc_velocity_ned); + EXPECT_CALL(offboard, set_velocity_ned(expected_velocity_ned)).Times(1); + + request.set_allocated_velocity_ned_yaw(rpc_velocity_ned.release()); + + offboardService.SetVelocityNed(nullptr, &request, nullptr); +} + +INSTANTIATE_TEST_CASE_P(OffboardResultCorrespondences, + OffboardServiceImplTest, + ::testing::ValuesIn(generateInputPairs())); + +std::vector generateInputPairs() +{ + std::vector input_pairs; + input_pairs.push_back(std::make_pair("SUCCESS", dronecode_sdk::Offboard::Result::SUCCESS)); + input_pairs.push_back(std::make_pair("NO_SYSTEM", dronecode_sdk::Offboard::Result::NO_SYSTEM)); + input_pairs.push_back( + std::make_pair("CONNECTION_ERROR", dronecode_sdk::Offboard::Result::CONNECTION_ERROR)); + input_pairs.push_back(std::make_pair("BUSY", dronecode_sdk::Offboard::Result::BUSY)); + input_pairs.push_back( + std::make_pair("COMMAND_DENIED", dronecode_sdk::Offboard::Result::COMMAND_DENIED)); + input_pairs.push_back(std::make_pair("TIMEOUT", dronecode_sdk::Offboard::Result::TIMEOUT)); + input_pairs.push_back( + std::make_pair("NO_SETPOINT_SET", dronecode_sdk::Offboard::Result::NO_SETPOINT_SET)); + input_pairs.push_back(std::make_pair("UNKNOWN", dronecode_sdk::Offboard::Result::UNKNOWN)); + + return input_pairs; +} + +} // namespace diff --git a/plugins/offboard/include/plugins/offboard/offboard.h b/plugins/offboard/include/plugins/offboard/offboard.h index 6bd2ccec19..574fb915c9 100644 --- a/plugins/offboard/include/plugins/offboard/offboard.h +++ b/plugins/offboard/include/plugins/offboard/offboard.h @@ -48,14 +48,14 @@ class Offboard : public PluginBase { * @brief Results for offboard requests. */ enum class Result { - SUCCESS = 0, /**< @brief %Request succeeded. */ + UNKNOWN, /**< @brief Unknown error. */ + SUCCESS, /**< @brief %Request succeeded. */ NO_SYSTEM, /**< @brief No system connected. */ CONNECTION_ERROR, /**< @brief %Connection error. */ BUSY, /**< @brief Vehicle busy. */ COMMAND_DENIED, /**< @brief Command denied. */ TIMEOUT, /**< @brief %Request timeout. */ - NO_SETPOINT_SET, /**< Can't start without setpoint set. */ - UNKNOWN /**< @brief Unknown error. */ + NO_SETPOINT_SET /**< Can't start without setpoint set. */ }; /** @@ -228,4 +228,62 @@ class Offboard : public PluginBase { std::unique_ptr _impl; }; +/** + * @brief Equal operator to compare two `Offboard::AttitudeRate` objects. + * + * @return `true` if items are equal. + */ +bool operator==(const Offboard::AttitudeRate &lhs, const Offboard::AttitudeRate &rhs); + +/** + * @brief Stream operator to print information about a `Offboard::AttitudeRate`. + * + * @return A reference to the stream. + */ +std::ostream &operator<<(std::ostream &str, Offboard::AttitudeRate const &attitude_rate); + +/** + * @brief Equal operator to compare two `Offboard::PositionNEDYaw` objects. + * + * @return `true` if items are equal. + */ +bool operator==(const Offboard::PositionNEDYaw &lhs, const Offboard::PositionNEDYaw &rhs); + +/** + * @brief Stream operator to print information about a `Offboard::PositionNEDYaw`. + * + * @return A reference to the stream. + */ +std::ostream &operator<<(std::ostream &str, Offboard::PositionNEDYaw const &position_ned_yaw); + +/** + * @brief Equal operator to compare two `Offboard::VelocityBodyYawspeed` objects. + * + * @return `true` if items are equal. + */ +bool operator==(const Offboard::VelocityBodyYawspeed &lhs, + const Offboard::VelocityBodyYawspeed &rhs); + +/** + * @brief Stream operator to print information about a `Offboard::VelocityBodyYawspeed`. + * + * @return A reference to the stream. + */ +std::ostream &operator<<(std::ostream &str, + Offboard::VelocityBodyYawspeed const &velocity_body_yawspeed); + +/** + * @brief Equal operator to compare two `Offboard::VelocityNEDYaw` objects. + * + * @return `true` if items are equal. + */ +bool operator==(const Offboard::VelocityNEDYaw &lhs, const Offboard::VelocityNEDYaw &rhs); + +/** + * @brief Stream operator to print information about a `Offboard::VelocityNEDYaw`. + * + * @return A reference to the stream. + */ +std::ostream &operator<<(std::ostream &str, Offboard::VelocityNEDYaw const &velocity_ned_yaw); + } // namespace dronecode_sdk diff --git a/plugins/offboard/mocks/offboard_mock.h b/plugins/offboard/mocks/offboard_mock.h new file mode 100644 index 0000000000..7bb67e0dbe --- /dev/null +++ b/plugins/offboard/mocks/offboard_mock.h @@ -0,0 +1,20 @@ +#include + +#include "plugins/offboard/offboard.h" + +namespace dronecode_sdk { +namespace testing { + +class MockOffboard { +public: + MOCK_CONST_METHOD0(start, Offboard::Result()){}; + MOCK_CONST_METHOD0(stop, Offboard::Result()){}; + MOCK_CONST_METHOD0(is_active, bool()){}; + MOCK_CONST_METHOD1(set_attitude_rate, void(Offboard::AttitudeRate)){}; + MOCK_CONST_METHOD1(set_position_ned, void(Offboard::PositionNEDYaw)){}; + MOCK_CONST_METHOD1(set_velocity_body, void(Offboard::VelocityBodyYawspeed)){}; + MOCK_CONST_METHOD1(set_velocity_ned, void(Offboard::VelocityNEDYaw)){}; +}; + +} // namespace testing +} // namespace dronecode_sdk diff --git a/plugins/offboard/offboard.cpp b/plugins/offboard/offboard.cpp index d4c72f63d0..5a462ba91e 100644 --- a/plugins/offboard/offboard.cpp +++ b/plugins/offboard/offboard.cpp @@ -80,4 +80,61 @@ const char *Offboard::result_str(Result result) } } +bool operator==(const Offboard::AttitudeRate &lhs, const Offboard::AttitudeRate &rhs) +{ + return lhs.roll_deg_s == rhs.roll_deg_s && lhs.pitch_deg_s == rhs.pitch_deg_s && + lhs.yaw_deg_s == rhs.yaw_deg_s && lhs.thrust_value == rhs.thrust_value; +} + +std::ostream &operator<<(std::ostream &str, Offboard::AttitudeRate const &attitude_rate) +{ + return str << "[roll_deg_s: " << attitude_rate.roll_deg_s + << ", pitch_deg_s: " << attitude_rate.pitch_deg_s + << ", yaw_deg_s: " << attitude_rate.yaw_deg_s + << ", thrust_value: " << attitude_rate.thrust_value << "]"; +} + +bool operator==(const Offboard::PositionNEDYaw &lhs, const Offboard::PositionNEDYaw &rhs) +{ + return lhs.north_m == rhs.north_m && lhs.east_m == rhs.east_m && lhs.down_m == rhs.down_m && + lhs.yaw_deg == rhs.yaw_deg; +} + +std::ostream &operator<<(std::ostream &str, Offboard::PositionNEDYaw const &position_ned_yaw) +{ + return str << "[north_m: " << position_ned_yaw.north_m + << ", east_m: " << position_ned_yaw.east_m << ", down_m: " << position_ned_yaw.down_m + << ", yaw_deg: " << position_ned_yaw.yaw_deg << "]"; +} + +bool operator==(const Offboard::VelocityBodyYawspeed &lhs, + const Offboard::VelocityBodyYawspeed &rhs) +{ + return lhs.forward_m_s == rhs.forward_m_s && lhs.right_m_s == rhs.right_m_s && + lhs.down_m_s == rhs.down_m_s && lhs.yawspeed_deg_s == rhs.yawspeed_deg_s; +} + +std::ostream &operator<<(std::ostream &str, + Offboard::VelocityBodyYawspeed const &velocity_body_yawspeed) +{ + return str << "[forward_m_s: " << velocity_body_yawspeed.forward_m_s + << ", right_m_s: " << velocity_body_yawspeed.right_m_s + << ", down_m_s: " << velocity_body_yawspeed.down_m_s + << ", yawspeed_deg_s: " << velocity_body_yawspeed.yawspeed_deg_s << "]"; +} + +bool operator==(const Offboard::VelocityNEDYaw &lhs, const Offboard::VelocityNEDYaw &rhs) +{ + return lhs.north_m_s == rhs.north_m_s && lhs.east_m_s == rhs.east_m_s && + lhs.down_m_s == rhs.down_m_s && lhs.yaw_deg == rhs.yaw_deg; +} + +std::ostream &operator<<(std::ostream &str, Offboard::VelocityNEDYaw const &velocity_ned_yaw) +{ + return str << "[north_m_s: " << velocity_ned_yaw.north_m_s + << ", east_m_s: " << velocity_ned_yaw.east_m_s + << ", down_m_s: " << velocity_ned_yaw.down_m_s + << ", yaw_deg: " << velocity_ned_yaw.yaw_deg << "]"; +} + } // namespace dronecode_sdk