From ad576746ed8a6cb95438adac36354ddbb4a8eadb Mon Sep 17 00:00:00 2001 From: Ali Tlisov Date: Tue, 7 May 2019 16:53:01 +0300 Subject: [PATCH 1/4] backend support for offboard/set_attitude --- backend/proto | 2 +- .../plugins/offboard/offboard_service_impl.h | 25 +++++++ backend/test/offboard_service_impl_test.cpp | 65 +++++++++++++++++-- .../include/plugins/offboard/offboard.h | 14 ++++ plugins/offboard/mocks/offboard_mock.h | 1 + plugins/offboard/offboard.cpp | 13 ++++ 6 files changed, 113 insertions(+), 7 deletions(-) diff --git a/backend/proto b/backend/proto index dccc60484a..3c5856bb14 160000 --- a/backend/proto +++ b/backend/proto @@ -1 +1 @@ -Subproject commit dccc60484a6c94ee1c11799c013c78c5da315741 +Subproject commit 3c5856bb1449285d86cf135a8387a5ff1d993cbb 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..5b59464266 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,8 @@ 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 +143,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 +159,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 +183,47 @@ 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, setAttitudeCorrectrly) +{ + 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..3bd1d2f239 100644 --- a/plugins/offboard/offboard.cpp +++ b/plugins/offboard/offboard.cpp @@ -80,6 +80,19 @@ 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 && From 7db3ebdc47196dd03c6126cff7c48ded9e462a35 Mon Sep 17 00:00:00 2001 From: Ali Tlisov Date: Fri, 10 May 2019 20:11:32 +0300 Subject: [PATCH 2/4] Update backend/test/offboard_service_impl_test.cpp typo fix Co-Authored-By: Jonas Vautherin --- backend/test/offboard_service_impl_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/backend/test/offboard_service_impl_test.cpp b/backend/test/offboard_service_impl_test.cpp index 5b59464266..1fae743115 100644 --- a/backend/test/offboard_service_impl_test.cpp +++ b/backend/test/offboard_service_impl_test.cpp @@ -208,7 +208,7 @@ OffboardServiceImplTest::createArbitraryRPCAttitudeRate() const return rpc_attitude_rate; } -TEST_F(OffboardServiceImplTest, setAttitudeCorrectrly) +TEST_F(OffboardServiceImplTest, setsAttitudeCorrectly) { MockOffboard offboard; OffboardServiceImpl offboardService(offboard); From 7190e21fffa436aced0e3f541e59222879459e4a Mon Sep 17 00:00:00 2001 From: Ali Tlisov Date: Fri, 10 May 2019 20:15:49 +0300 Subject: [PATCH 3/4] fix style --- backend/test/offboard_service_impl_test.cpp | 6 ++---- plugins/offboard/offboard.cpp | 7 +++---- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/backend/test/offboard_service_impl_test.cpp b/backend/test/offboard_service_impl_test.cpp index 1fae743115..9bc13ac038 100644 --- a/backend/test/offboard_service_impl_test.cpp +++ b/backend/test/offboard_service_impl_test.cpp @@ -40,8 +40,7 @@ class OffboardServiceImplTest : public ::testing::TestWithParam { protected: void checkReturnsCorrectIsActiveStatus(const bool expected_is_active_status); - std::unique_ptr - createArbitraryRPCAttitude() const; + std::unique_ptr createArbitraryRPCAttitude() const; std::unique_ptr createArbitraryRPCAttitudeRate() const; std::unique_ptr @@ -215,8 +214,7 @@ TEST_F(OffboardServiceImplTest, setsAttitudeCorrectly) dronecode_sdk::rpc::offboard::SetAttitudeRequest request; auto rpc_attitude = createArbitraryRPCAttitude(); - const auto expected_attitude = - OffboardServiceImpl::translateRPCAttitude(*rpc_attitude); + const auto expected_attitude = OffboardServiceImpl::translateRPCAttitude(*rpc_attitude); EXPECT_CALL(offboard, set_attitude(expected_attitude)).Times(1); request.set_allocated_attitude(rpc_attitude.release()); diff --git a/plugins/offboard/offboard.cpp b/plugins/offboard/offboard.cpp index 3bd1d2f239..6907704d2c 100644 --- a/plugins/offboard/offboard.cpp +++ b/plugins/offboard/offboard.cpp @@ -88,10 +88,9 @@ bool operator==(const Offboard::Attitude &lhs, const Offboard::Attitude &rhs) 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 << "]"; + 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) { From 1f9043e9c8ebced4c941f92ca5dec4791233fa3b Mon Sep 17 00:00:00 2001 From: Ali Tlisov Date: Fri, 10 May 2019 20:21:41 +0300 Subject: [PATCH 4/4] updated proto --- backend/proto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/backend/proto b/backend/proto index 3c5856bb14..56fb54e3fe 160000 --- a/backend/proto +++ b/backend/proto @@ -1 +1 @@ -Subproject commit 3c5856bb1449285d86cf135a8387a5ff1d993cbb +Subproject commit 56fb54e3fe8bd0c076bce72168bee05f397ecbd1