diff --git a/backend/proto b/backend/proto index dccc60484a..56fb54e3fe 160000 --- a/backend/proto +++ b/backend/proto @@ -1 +1 @@ -Subproject commit dccc60484a6c94ee1c11799c013c78c5da315741 +Subproject commit 56fb54e3fe8bd0c076bce72168bee05f397ecbd1 diff --git a/backend/src/plugins/offboard/offboard_service_impl.h b/backend/src/plugins/offboard/offboard_service_impl.h index 1a9401b5bb..d794146cdc 100644 --- a/backend/src/plugins/offboard/offboard_service_impl.h +++ b/backend/src/plugins/offboard/offboard_service_impl.h @@ -60,6 +60,31 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service return grpc::Status::OK; } + grpc::Status SetAttitude(grpc::ServerContext * /* context */, + const rpc::offboard::SetAttitudeRequest *request, + rpc::offboard::SetAttitudeResponse * /* response */) override + { + if (request != nullptr) { + auto requested_attitude = translateRPCAttitude(request->attitude()); + _offboard.set_attitude(requested_attitude); + } + + return grpc::Status::OK; + } + + static dronecode_sdk::Offboard::Attitude + translateRPCAttitude(const rpc::offboard::Attitude &rpc_attitude) + { + dronecode_sdk::Offboard::Attitude attitude; + + attitude.roll_deg = rpc_attitude.roll_deg(); + attitude.pitch_deg = rpc_attitude.pitch_deg(); + attitude.yaw_deg = rpc_attitude.yaw_deg(); + attitude.thrust_value = rpc_attitude.thrust_value(); + + return attitude; + } + grpc::Status SetAttitudeRate(grpc::ServerContext * /* context */, const rpc::offboard::SetAttitudeRateRequest *request, rpc::offboard::SetAttitudeRateResponse * /* response */) override diff --git a/backend/test/offboard_service_impl_test.cpp b/backend/test/offboard_service_impl_test.cpp index 9106cc3514..9bc13ac038 100644 --- a/backend/test/offboard_service_impl_test.cpp +++ b/backend/test/offboard_service_impl_test.cpp @@ -16,9 +16,12 @@ using OffboardServiceImpl = dronecode_sdk::backend::OffboardServiceImpl; -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_ROLL = 25.0f; +static constexpr float ARBITRARY_PITCH = 40.0f; +static constexpr float ARBITRARY_YAW = 37.0f; +static constexpr float ARBITRARY_ROLL_RATE = 2.5f; +static constexpr float ARBITRARY_PITCH_RATE = 4.0f; +static constexpr float ARBITRARY_YAW_RATE = 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; @@ -37,6 +40,7 @@ class OffboardServiceImplTest : public ::testing::TestWithParam { protected: void checkReturnsCorrectIsActiveStatus(const bool expected_is_active_status); + std::unique_ptr createArbitraryRPCAttitude() const; std::unique_ptr createArbitraryRPCAttitudeRate() const; std::unique_ptr @@ -138,6 +142,14 @@ TEST_F(OffboardServiceImplTest, isActiveDoesNotCrashWithNullResponse) offboardService.IsActive(nullptr, nullptr, nullptr); } +TEST_F(OffboardServiceImplTest, setAttitudeDoesNotFailWithAllNullParams) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + + offboardService.SetAttitude(nullptr, nullptr, nullptr); +} + TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithAllNullParams) { MockOffboard offboard; @@ -146,6 +158,18 @@ TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithAllNullParams) offboardService.SetAttitudeRate(nullptr, nullptr, nullptr); } +TEST_F(OffboardServiceImplTest, setAttitudeDoesNotFailWithNullResponse) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::SetAttitudeRequest request; + + auto rpc_attitude = createArbitraryRPCAttitude(); + request.set_allocated_attitude(rpc_attitude.release()); + + offboardService.SetAttitude(nullptr, &request, nullptr); +} + TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithNullResponse) { MockOffboard offboard; @@ -158,19 +182,46 @@ TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithNullResponse) offboardService.SetAttitudeRate(nullptr, &request, nullptr); } +std::unique_ptr +OffboardServiceImplTest::createArbitraryRPCAttitude() const +{ + auto rpc_attitude = std::unique_ptr( + new dronecode_sdk::rpc::offboard::Attitude()); + rpc_attitude->set_roll_deg(ARBITRARY_ROLL); + rpc_attitude->set_pitch_deg(ARBITRARY_PITCH); + rpc_attitude->set_yaw_deg(ARBITRARY_YAW); + rpc_attitude->set_thrust_value(ARBITRARY_THRUST); + + return rpc_attitude; +} 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_roll_deg_s(ARBITRARY_ROLL_RATE); + rpc_attitude_rate->set_pitch_deg_s(ARBITRARY_PITCH_RATE); + rpc_attitude_rate->set_yaw_deg_s(ARBITRARY_YAW_RATE); rpc_attitude_rate->set_thrust_value(ARBITRARY_THRUST); return rpc_attitude_rate; } +TEST_F(OffboardServiceImplTest, setsAttitudeCorrectly) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + dronecode_sdk::rpc::offboard::SetAttitudeRequest request; + + auto rpc_attitude = createArbitraryRPCAttitude(); + const auto expected_attitude = OffboardServiceImpl::translateRPCAttitude(*rpc_attitude); + EXPECT_CALL(offboard, set_attitude(expected_attitude)).Times(1); + + request.set_allocated_attitude(rpc_attitude.release()); + + offboardService.SetAttitude(nullptr, &request, nullptr); +} + TEST_F(OffboardServiceImplTest, setsAttitudeRateCorrectly) { MockOffboard offboard; diff --git a/plugins/offboard/include/plugins/offboard/offboard.h b/plugins/offboard/include/plugins/offboard/offboard.h index 574fb915c9..0f6cd63dd3 100644 --- a/plugins/offboard/include/plugins/offboard/offboard.h +++ b/plugins/offboard/include/plugins/offboard/offboard.h @@ -228,6 +228,13 @@ class Offboard : public PluginBase { std::unique_ptr _impl; }; +/** + * @brief Equal operator to compare two `Offboard::Attitude` objects. + * + * @return `true` if items are equal. + */ +bool operator==(const Offboard::Attitude &lhs, const Offboard::Attitude &rhs); + /** * @brief Equal operator to compare two `Offboard::AttitudeRate` objects. * @@ -235,6 +242,13 @@ class Offboard : public PluginBase { */ bool operator==(const Offboard::AttitudeRate &lhs, const Offboard::AttitudeRate &rhs); +/** + * @brief Stream operator to print information about a `Offboard::Attitude`. + * + * @return A reference to the stream. + */ +std::ostream &operator<<(std::ostream &str, Offboard::Attitude const &attitude); + /** * @brief Stream operator to print information about a `Offboard::AttitudeRate`. * diff --git a/plugins/offboard/mocks/offboard_mock.h b/plugins/offboard/mocks/offboard_mock.h index 7bb67e0dbe..f1c39b099f 100644 --- a/plugins/offboard/mocks/offboard_mock.h +++ b/plugins/offboard/mocks/offboard_mock.h @@ -10,6 +10,7 @@ class MockOffboard { MOCK_CONST_METHOD0(start, Offboard::Result()){}; MOCK_CONST_METHOD0(stop, Offboard::Result()){}; MOCK_CONST_METHOD0(is_active, bool()){}; + MOCK_CONST_METHOD1(set_attitude, void(Offboard::Attitude)){}; 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)){}; diff --git a/plugins/offboard/offboard.cpp b/plugins/offboard/offboard.cpp index 5a462ba91e..6907704d2c 100644 --- a/plugins/offboard/offboard.cpp +++ b/plugins/offboard/offboard.cpp @@ -80,6 +80,18 @@ const char *Offboard::result_str(Result result) } } +bool operator==(const Offboard::Attitude &lhs, const Offboard::Attitude &rhs) +{ + return lhs.roll_deg == rhs.roll_deg && lhs.pitch_deg == rhs.pitch_deg && + lhs.yaw_deg == rhs.yaw_deg && lhs.thrust_value == rhs.thrust_value; +} + +std::ostream &operator<<(std::ostream &str, Offboard::Attitude const &attitude) +{ + return str << "[roll_deg: " << attitude.roll_deg << ", pitch_deg: " << attitude.pitch_deg + << ", yaw_deg: " << attitude.yaw_deg << ", thrust_value " << attitude.thrust_value + << "]"; +} 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 &&