From db2923a37f241a6e86042293c6bf50f7c9aab491 Mon Sep 17 00:00:00 2001 From: Christopher Kim Date: Mon, 29 Apr 2019 15:55:56 -0400 Subject: [PATCH 01/12] Add gRPC backend support for Offboard plugin. Also add backend test and plugin mock. --- backend/src/CMakeLists.txt | 3 +- backend/src/core/core_service_impl.h | 1 + backend/src/grpc_server.cpp | 1 + backend/src/grpc_server.h | 6 + .../plugins/offboard/offboard_service_impl.h | 103 ++++++++++++++++++ plugins/offboard/mocks/offboard_mock.h | 20 ++++ 6 files changed, 133 insertions(+), 1 deletion(-) create mode 100644 backend/src/plugins/offboard/offboard_service_impl.h create mode 100644 plugins/offboard/mocks/offboard_mock.h 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..00c30fb318 100644 --- a/backend/src/core/core_service_impl.h +++ b/backend/src/core/core_service_impl.h @@ -50,6 +50,7 @@ class CoreServiceImpl final : public dronecode_sdk::rpc::core::CoreService::Serv "gimbal", "info", "mission", + "offboard", "param", "telemetry"}; 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..8392001510 --- /dev/null +++ b/backend/src/plugins/offboard/offboard_service_impl.h @@ -0,0 +1,103 @@ +#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 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 = request->attitude_rate(); + _offboard.set_attitude_rate(requested_attitude_rate) + } + + return grpc::Status::OK; + } + + grpc::Status SetPositionNed(grpc::ServerContext * /* context */, + const rpc::Offboard::SetPositionNedRequest * /* request */, + rpc::Offboard::SetPositionNedResponse *response) override + { + if (request != nullptr) { + auto requested_position_ned_yaw = request->position_ned_yaw(); + _offboard.set_position_ned(requested_position_ned_yaw) + } + + return grpc::Status::OK; + } + + grpc::Status SetVelocityBody(grpc::ServerContext * /* context */, + const rpc::Offboard::SetVelocityBodyRequest * /* request */, + rpc::Offboard::SetVelocityBodyResponse *response) override + { + if (request != nullptr) { + auto requested_velocity_body_yawspeed = request->velocity_body_yawspeed(); + _offboard.set_velocity_body(requested_velocity_body_yawspeed) + } + + return grpc::Status::OK; + } + + grpc::Status SetVelocityNed(grpc::ServerContext * /* context */, + const rpc::Offboard::SetVelocityNedRequest * /* request */, + rpc::Offboard::SetVelocityNedResponse *response) override + { + if (request != nullptr) { + auto requested_velocity_ned_yaw = request->velocity_ned_yaw(); + _offboard.set_velocity_ned(requested_velocity_ned_yaw) + } + + return grpc::Status::OK; + } + +private: + Offboard &_offboard; +}; + +} // namespace backend +} // 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 From 623e4fe369b6f1f2f7bf8f9c0fdf9abb6bb90701 Mon Sep 17 00:00:00 2001 From: Christopher Kim Date: Tue, 30 Apr 2019 15:49:02 -0400 Subject: [PATCH 02/12] Add missing Stop method. --- .../src/plugins/offboard/offboard_service_impl.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/backend/src/plugins/offboard/offboard_service_impl.h b/backend/src/plugins/offboard/offboard_service_impl.h index 8392001510..1f44d1ee76 100644 --- a/backend/src/plugins/offboard/offboard_service_impl.h +++ b/backend/src/plugins/offboard/offboard_service_impl.h @@ -35,6 +35,19 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service 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 From 755935acbf4498666d39684f9335449e6670e574 Mon Sep 17 00:00:00 2001 From: Christopher Kim Date: Tue, 30 Apr 2019 17:58:29 -0400 Subject: [PATCH 03/12] Add backend unit test for offboard plugin (WIP). --- backend/test/CMakeLists.txt | 1 + backend/test/offboard_service_impl_test.cpp | 194 ++++++++++++++++++++ 2 files changed, 195 insertions(+) create mode 100644 backend/test/offboard_service_impl_test.cpp 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..f83901c1ea --- /dev/null +++ b/backend/test/offboard_service_impl_test.cpp @@ -0,0 +1,194 @@ +#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; + +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; +}; + +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; + + offboard.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); +} + +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 From 01c63078b0c754254b76a6f5460c152c77a89c34 Mon Sep 17 00:00:00 2001 From: Christopher Kim Date: Thu, 2 May 2019 12:16:45 -0400 Subject: [PATCH 04/12] Update protos submodule to include recent offboard proto. --- backend/proto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From d253346099e20303ec3d2ef89ffcd6d4dd49ea33 Mon Sep 17 00:00:00 2001 From: Christopher Kim Date: Fri, 3 May 2019 14:09:12 -0400 Subject: [PATCH 05/12] Update the plugin_names array size to account for offboard. --- backend/src/core/core_service_impl.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/backend/src/core/core_service_impl.h b/backend/src/core/core_service_impl.h index 00c30fb318..ee12e6e844 100644 --- a/backend/src/core/core_service_impl.h +++ b/backend/src/core/core_service_impl.h @@ -43,16 +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", - "offboard", - "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(); From 109e4b05cdb4d75be7985a1b450eadc472dd4898 Mon Sep 17 00:00:00 2001 From: Christopher Kim Date: Fri, 3 May 2019 14:11:28 -0400 Subject: [PATCH 06/12] Fix function signatures in terms of request and response. Add translate rpc functions for attitude rate and position ned yaw. --- .../plugins/offboard/offboard_service_impl.h | 62 +++++++++++++------ 1 file changed, 44 insertions(+), 18 deletions(-) diff --git a/backend/src/plugins/offboard/offboard_service_impl.h b/backend/src/plugins/offboard/offboard_service_impl.h index 1f44d1ee76..4d1f43f805 100644 --- a/backend/src/plugins/offboard/offboard_service_impl.h +++ b/backend/src/plugins/offboard/offboard_service_impl.h @@ -23,8 +23,8 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service } grpc::Status Start(grpc::ServerContext * /* context */, - const rpc::Offboard::StartRequest * /* request */, - rpc::Offboard::StartResponse *response) override + const rpc::offboard::StartRequest * /* request */, + rpc::offboard::StartResponse *response) override { auto offboard_result = _offboard.start(); @@ -36,8 +36,8 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service } grpc::Status Stop(grpc::ServerContext * /* context */, - const rpc::Offboard::StopRequest * /* request */, - rpc::Offboard::StopResponse *response) override + const rpc::offboard::StopRequest * /* request */, + rpc::offboard::StopResponse *response) override { auto offboard_result = _offboard.stop(); @@ -49,8 +49,8 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service } grpc::Status IsActive(grpc::ServerContext * /* context */, - const rpc::Offboard::IsActiveRequest * /* request */, - rpc::Offboard::IsActiveResponse *response) override + const rpc::offboard::IsActiveRequest * /* request */, + rpc::offboard::IsActiveResponse *response) override { if (response != nullptr) { auto is_active = _offboard.is_active(); @@ -61,48 +61,74 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service } grpc::Status SetAttitudeRate(grpc::ServerContext * /* context */, - const rpc::Offboard::SetAttitudeRateRequest * /* request */, - rpc::Offboard::SetAttitudeRateResponse *response) override + const rpc::offboard::SetAttitudeRateRequest *request, + rpc::offboard::SetAttitudeRateResponse * /* response */) override { if (request != nullptr) { auto requested_attitude_rate = request->attitude_rate(); - _offboard.set_attitude_rate(requested_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 + const rpc::offboard::SetPositionNedRequest *request, + rpc::offboard::SetPositionNedResponse * /* response */) override { if (request != nullptr) { auto requested_position_ned_yaw = request->position_ned_yaw(); - _offboard.set_position_ned(requested_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 + const rpc::offboard::SetVelocityBodyRequest *request, + rpc::offboard::SetVelocityBodyResponse * /* response */) override { if (request != nullptr) { auto requested_velocity_body_yawspeed = request->velocity_body_yawspeed(); - _offboard.set_velocity_body(requested_velocity_body_yawspeed) + _offboard.set_velocity_body(requested_velocity_body_yawspeed); } return grpc::Status::OK; } grpc::Status SetVelocityNed(grpc::ServerContext * /* context */, - const rpc::Offboard::SetVelocityNedRequest * /* request */, - rpc::Offboard::SetVelocityNedResponse *response) override + const rpc::offboard::SetVelocityNedRequest *request, + rpc::offboard::SetVelocityNedResponse * /* response */) override { if (request != nullptr) { auto requested_velocity_ned_yaw = request->velocity_ned_yaw(); - _offboard.set_velocity_ned(requested_velocity_ned_yaw) + _offboard.set_velocity_ned(requested_velocity_ned_yaw); } return grpc::Status::OK; From eaf98b9bd282f31c91c4e497f99abaa8e0be5ca1 Mon Sep 17 00:00:00 2001 From: Christopher Kim Date: Fri, 3 May 2019 14:14:17 -0400 Subject: [PATCH 07/12] Add tests for offboard position ned yaw. Remove unique_ptr approach for creating arbitrary structs for testing. --- backend/test/offboard_service_impl_test.cpp | 68 ++++++++++++++++++--- 1 file changed, 60 insertions(+), 8 deletions(-) diff --git a/backend/test/offboard_service_impl_test.cpp b/backend/test/offboard_service_impl_test.cpp index f83901c1ea..4f59e0679e 100644 --- a/backend/test/offboard_service_impl_test.cpp +++ b/backend/test/offboard_service_impl_test.cpp @@ -20,6 +20,9 @@ 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; std::vector generateInputPairs(); std::string startAndGetTranslatedResult(dronecode_sdk::Offboard::Result start_result); @@ -29,8 +32,8 @@ class OffboardServiceImplTest : public ::testing::TestWithParam { protected: void checkReturnsCorrectIsActiveStatus(const bool expected_is_active_status); - std::unique_ptr - createArbitraryRPCAttitudeRate() const; + dronecode_sdk::rpc::offboard::AttitudeRate createArbitraryRPCAttitudeRate() const; + dronecode_sdk::rpc::offboard::PositionNEDYaw createArbitraryRPCPositionNEDYaw() const; }; TEST_P(OffboardServiceImplTest, startResultIsTranslatedCorrectly) @@ -94,7 +97,7 @@ TEST_F(OffboardServiceImplTest, isActiveCallsGetter) EXPECT_CALL(offboard, is_active()).Times(1); dronecode_sdk::rpc::offboard::IsActiveResponse response; - offboard.IsActive(nullptr, nullptr, &response); + offboardService.IsActive(nullptr, nullptr, &response); } TEST_F(OffboardServiceImplTest, isActiveGetsCorrectValue) @@ -139,15 +142,15 @@ TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithNullResponse) dronecode_sdk::rpc::offboard::SetAttitudeRateRequest request; auto rpc_attitude_rate = createArbitraryRPCAttitudeRate(); - request.set_allocated_attitude_rate(rpc_attitude_rate.release()); + request.set_attitude_rate(rpc_attitude_rate); offboardService.SetAttitudeRate(nullptr, &request, nullptr); } -std::unique_ptr +dronecode_sdk::rpc::offboard::AttitudeRate OffboardServiceImplTest::createArbitraryRPCAttitudeRate() const { - auto rpc_attitude_rate = std::unique_ptr( + auto rpc_attitude_rate = dronecode_sdk::rpc::offboard::AttitudeRate( new dronecode_sdk::rpc::offboard::AttitudeRate()); rpc_attitude_rate->set_roll_deg_s(ARBITRARY_ROLL); rpc_attitude_rate->set_pitch_deg_s(ARBITRARY_PITCH); @@ -165,14 +168,63 @@ TEST_F(OffboardServiceImplTest, setsAttitudeRateCorrectly) auto rpc_attitude_rate = createArbitraryRPCAttitudeRate(); const auto expected_attitude_rate = - OffboardServiceImpl::translateRPCAttitudeRate(*rpc_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()); + request.set_attitude_rate(rpc_attitude_rate); 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_position_ned(rpc_position_ned_yaw); + + offboardService.SetPositionNed(nullptr, &request, nullptr); +} + +dronecode_sdk::rpc::offboard::PositionNEDYaw +OffboardServiceImplTest::createArbitraryRPCPositionNEDYaw() const +{ + auto rpc_position_ned_yaw = dronecode_sdk::rpc::offboard::PositionNEDYaw( + 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_position_ned(rpc_position_ned_yaw); + + offboardService.SetPositionNed(nullptr, &request, nullptr); +} + std::vector generateInputPairs() { std::vector input_pairs; From bb78a9ad380e1744f113e14c456857d14296fdd2 Mon Sep 17 00:00:00 2001 From: Christopher Kim Date: Fri, 3 May 2019 16:56:40 -0400 Subject: [PATCH 08/12] Add operator overloads for Offboard structs. --- .../include/plugins/offboard/offboard.h | 58 ++++++++++++++++ plugins/offboard/offboard.cpp | 66 +++++++++++++++++++ 2 files changed, 124 insertions(+) diff --git a/plugins/offboard/include/plugins/offboard/offboard.h b/plugins/offboard/include/plugins/offboard/offboard.h index 6bd2ccec19..70e88e9b47 100644 --- a/plugins/offboard/include/plugins/offboard/offboard.h +++ b/plugins/offboard/include/plugins/offboard/offboard.h @@ -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/offboard.cpp b/plugins/offboard/offboard.cpp index d4c72f63d0..42fede2295 100644 --- a/plugins/offboard/offboard.cpp +++ b/plugins/offboard/offboard.cpp @@ -80,4 +80,70 @@ 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 From 3dd6e151d7a789ed2b6b2777a8664e8abdf11464 Mon Sep 17 00:00:00 2001 From: Christopher Kim Date: Fri, 3 May 2019 17:31:26 -0400 Subject: [PATCH 09/12] Update results enum order so that UNKNOWN is first, matching the proto definition. --- plugins/offboard/include/plugins/offboard/offboard.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/plugins/offboard/include/plugins/offboard/offboard.h b/plugins/offboard/include/plugins/offboard/offboard.h index 70e88e9b47..0c5a0695f5 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. */ }; /** From 76d5059104865bd044a9bcfdc5eb1cbf8683afba Mon Sep 17 00:00:00 2001 From: Christopher Kim Date: Fri, 3 May 2019 18:08:55 -0400 Subject: [PATCH 10/12] Add tests for remaining Offboard structs. --- .../plugins/offboard/offboard_service_impl.h | 37 ++++- backend/test/offboard_service_impl_test.cpp | 137 ++++++++++++++++-- 2 files changed, 158 insertions(+), 16 deletions(-) diff --git a/backend/src/plugins/offboard/offboard_service_impl.h b/backend/src/plugins/offboard/offboard_service_impl.h index 4d1f43f805..463cda822b 100644 --- a/backend/src/plugins/offboard/offboard_service_impl.h +++ b/backend/src/plugins/offboard/offboard_service_impl.h @@ -65,7 +65,7 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service rpc::offboard::SetAttitudeRateResponse * /* response */) override { if (request != nullptr) { - auto requested_attitude_rate = request->attitude_rate(); + auto requested_attitude_rate = translateRPCAttitudeRate(request->attitude_rate()); _offboard.set_attitude_rate(requested_attitude_rate); } @@ -90,7 +90,8 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service rpc::offboard::SetPositionNedResponse * /* response */) override { if (request != nullptr) { - auto requested_position_ned_yaw = request->position_ned_yaw(); + auto requested_position_ned_yaw = translateRPCPositionNEDYaw( + request->position_ned_yaw()); _offboard.set_position_ned(requested_position_ned_yaw); } @@ -115,25 +116,53 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service rpc::offboard::SetVelocityBodyResponse * /* response */) override { if (request != nullptr) { - auto requested_velocity_body_yawspeed = request->velocity_body_yawspeed(); + 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 = request->velocity_ned_yaw(); + 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; }; diff --git a/backend/test/offboard_service_impl_test.cpp b/backend/test/offboard_service_impl_test.cpp index 4f59e0679e..9106cc3514 100644 --- a/backend/test/offboard_service_impl_test.cpp +++ b/backend/test/offboard_service_impl_test.cpp @@ -23,6 +23,11 @@ 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); @@ -32,8 +37,14 @@ class OffboardServiceImplTest : public ::testing::TestWithParam { protected: void checkReturnsCorrectIsActiveStatus(const bool expected_is_active_status); - dronecode_sdk::rpc::offboard::AttitudeRate createArbitraryRPCAttitudeRate() const; - dronecode_sdk::rpc::offboard::PositionNEDYaw createArbitraryRPCPositionNEDYaw() const; + std::unique_ptr + createArbitraryRPCAttitudeRate() const; + std::unique_ptr + createArbitraryRPCPositionNEDYaw() const; + std::unique_ptr + createArbitraryRPCVelocityBodyYawspeed() const; + std::unique_ptr + createArbitraryRPCVelocityNedYaw() const; }; TEST_P(OffboardServiceImplTest, startResultIsTranslatedCorrectly) @@ -142,15 +153,15 @@ TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithNullResponse) dronecode_sdk::rpc::offboard::SetAttitudeRateRequest request; auto rpc_attitude_rate = createArbitraryRPCAttitudeRate(); - request.set_attitude_rate(rpc_attitude_rate); + request.set_allocated_attitude_rate(rpc_attitude_rate.release()); offboardService.SetAttitudeRate(nullptr, &request, nullptr); } -dronecode_sdk::rpc::offboard::AttitudeRate +std::unique_ptr OffboardServiceImplTest::createArbitraryRPCAttitudeRate() const { - auto rpc_attitude_rate = dronecode_sdk::rpc::offboard::AttitudeRate( + 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); @@ -168,10 +179,10 @@ TEST_F(OffboardServiceImplTest, setsAttitudeRateCorrectly) auto rpc_attitude_rate = createArbitraryRPCAttitudeRate(); const auto expected_attitude_rate = - OffboardServiceImpl::translateRPCAttitudeRate(rpc_attitude_rate); + OffboardServiceImpl::translateRPCAttitudeRate(*rpc_attitude_rate); EXPECT_CALL(offboard, set_attitude_rate(expected_attitude_rate)).Times(1); - request.set_attitude_rate(rpc_attitude_rate); + request.set_allocated_attitude_rate(rpc_attitude_rate.release()); offboardService.SetAttitudeRate(nullptr, &request, nullptr); } @@ -191,15 +202,15 @@ TEST_F(OffboardServiceImplTest, setPositionNEDYawDoesNotFailWithNullResponse) dronecode_sdk::rpc::offboard::SetPositionNedRequest request; auto rpc_position_ned_yaw = createArbitraryRPCPositionNEDYaw(); - request.set_position_ned(rpc_position_ned_yaw); + request.set_allocated_position_ned_yaw(rpc_position_ned_yaw.release()); offboardService.SetPositionNed(nullptr, &request, nullptr); } -dronecode_sdk::rpc::offboard::PositionNEDYaw +std::unique_ptr OffboardServiceImplTest::createArbitraryRPCPositionNEDYaw() const { - auto rpc_position_ned_yaw = dronecode_sdk::rpc::offboard::PositionNEDYaw( + 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); @@ -217,14 +228,116 @@ TEST_F(OffboardServiceImplTest, setsPositionNEDYawCorrectly) auto rpc_position_ned_yaw = createArbitraryRPCPositionNEDYaw(); const auto expected_position_ned_yaw = - OffboardServiceImpl::translateRPCPositionNEDYaw(rpc_position_ned_yaw); + OffboardServiceImpl::translateRPCPositionNEDYaw(*rpc_position_ned_yaw); EXPECT_CALL(offboard, set_position_ned(expected_position_ned_yaw)).Times(1); - request.set_position_ned(rpc_position_ned_yaw); + 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; From 06969a8e3ecd5bba4295485dba72c4951a8375ac Mon Sep 17 00:00:00 2001 From: Christopher Kim Date: Fri, 3 May 2019 19:21:48 -0400 Subject: [PATCH 11/12] Fix style errors. --- .../plugins/offboard/offboard_service_impl.h | 24 +++--- core/mavlink_parameters.cpp | 15 ++-- plugins/camera/camera_impl.cpp | 80 ++++++++++--------- plugins/follow_me/follow_me_impl.cpp | 61 +++++++------- .../include/plugins/offboard/offboard.h | 16 ++-- plugins/offboard/offboard.cpp | 25 ++---- 6 files changed, 109 insertions(+), 112 deletions(-) diff --git a/backend/src/plugins/offboard/offboard_service_impl.h b/backend/src/plugins/offboard/offboard_service_impl.h index 463cda822b..1a9401b5bb 100644 --- a/backend/src/plugins/offboard/offboard_service_impl.h +++ b/backend/src/plugins/offboard/offboard_service_impl.h @@ -72,8 +72,8 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service return grpc::Status::OK; } - static dronecode_sdk::Offboard::AttitudeRate translateRPCAttitudeRate( - const rpc::offboard::AttitudeRate &rpc_attitude_rate) + static dronecode_sdk::Offboard::AttitudeRate + translateRPCAttitudeRate(const rpc::offboard::AttitudeRate &rpc_attitude_rate) { dronecode_sdk::Offboard::AttitudeRate attitude_rate; @@ -90,16 +90,16 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service rpc::offboard::SetPositionNedResponse * /* response */) override { if (request != nullptr) { - auto requested_position_ned_yaw = translateRPCPositionNEDYaw( - request->position_ned_yaw()); + 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) + static dronecode_sdk::Offboard::PositionNEDYaw + translateRPCPositionNEDYaw(const rpc::offboard::PositionNEDYaw &rpc_position_ned_yaw) { dronecode_sdk::Offboard::PositionNEDYaw position_ned_yaw; @@ -116,8 +116,8 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service rpc::offboard::SetVelocityBodyResponse * /* response */) override { if (request != nullptr) { - auto requested_velocity_body_yawspeed = translateRPCVelocityBodyYawspeed( - request->velocity_body_yawspeed()); + auto requested_velocity_body_yawspeed = + translateRPCVelocityBodyYawspeed(request->velocity_body_yawspeed()); _offboard.set_velocity_body(requested_velocity_body_yawspeed); } @@ -142,16 +142,16 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service rpc::offboard::SetVelocityNedResponse * /* response */) override { if (request != nullptr) { - auto requested_velocity_ned_yaw = translateRPCVelocityNEDYaw( - request->velocity_ned_yaw()); + 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) + static dronecode_sdk::Offboard::VelocityNEDYaw + translateRPCVelocityNEDYaw(const rpc::offboard::VelocityNEDYaw &rpc_velocity_ned_yaw) { dronecode_sdk::Offboard::VelocityNEDYaw velocity_ned_yaw; diff --git a/core/mavlink_parameters.cpp b/core/mavlink_parameters.cpp index 8ff1bb783f..eceed5e4f4 100644 --- a/core/mavlink_parameters.cpp +++ b/core/mavlink_parameters.cpp @@ -114,13 +114,14 @@ MAVLinkParameters::get_param(const std::string &name, ParamValue value_type, boo auto prom = std::promise>(); auto res = prom.get_future(); - get_param_async(name, - value_type, - [&prom](Result result, ParamValue value) { - prom.set_value(std::make_pair<>(result, value)); - }, - this, - extended); + get_param_async( + name, + value_type, + [&prom](Result result, ParamValue value) { + prom.set_value(std::make_pair<>(result, value)); + }, + this, + extended); return res.get(); } diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index f36eabe4e5..ab971a8b54 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -1259,25 +1259,26 @@ void CameraImpl::set_option_async(const std::string &setting_id, return; } - _parent->set_param_async(setting_id, - value, - [this, callback, setting_id, value](MAVLinkParameters::Result result) { - if (result == MAVLinkParameters::Result::SUCCESS) { - if (this->_camera_definition) { - _camera_definition->set_setting(setting_id, value); - refresh_params(); - } - if (callback) { - callback(Camera::Result::SUCCESS); - } - } else { - if (callback) { - callback(Camera::Result::ERROR); - } - } - }, - this, - true); + _parent->set_param_async( + setting_id, + value, + [this, callback, setting_id, value](MAVLinkParameters::Result result) { + if (result == MAVLinkParameters::Result::SUCCESS) { + if (this->_camera_definition) { + _camera_definition->set_setting(setting_id, value); + refresh_params(); + } + if (callback) { + callback(Camera::Result::SUCCESS); + } + } else { + if (callback) { + callback(Camera::Result::ERROR); + } + } + }, + this, + true); } Camera::Result CameraImpl::get_option(const std::string &setting_id, Camera::Option &option) @@ -1457,26 +1458,27 @@ void CameraImpl::refresh_params() const std::string ¶m_name = param.first; const MAVLinkParameters::ParamValue ¶m_value_type = param.second; const bool is_last = (count + 1 == params.size()); - _parent->get_param_async(param_name, - param_value_type, - [param_name, is_last, this](MAVLinkParameters::Result result, - MAVLinkParameters::ParamValue value) { - if (result != MAVLinkParameters::Result::SUCCESS) { - return; - } - // We need to check again by the time this callback runs - if (!this->_camera_definition) { - return; - } - this->_camera_definition->set_setting(param_name, value); - - if (is_last) { - notify_current_settings(); - notify_possible_setting_options(); - } - }, - this, - true); + _parent->get_param_async( + param_name, + param_value_type, + [param_name, is_last, this](MAVLinkParameters::Result result, + MAVLinkParameters::ParamValue value) { + if (result != MAVLinkParameters::Result::SUCCESS) { + return; + } + // We need to check again by the time this callback runs + if (!this->_camera_definition) { + return; + } + this->_camera_definition->set_setting(param_name, value); + + if (is_last) { + notify_current_settings(); + notify_possible_setting_options(); + } + }, + this, + true); ++count; } } diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp index 39418f40e7..7c1eccae6e 100644 --- a/plugins/follow_me/follow_me_impl.cpp +++ b/plugins/follow_me/follow_me_impl.cpp @@ -34,35 +34,38 @@ void FollowMeImpl::deinit() void FollowMeImpl::enable() { - _parent->get_param_float_async("NAV_MIN_FT_HT", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.min_height_m = value; - } - }, - this); - _parent->get_param_float_async("NAV_FT_DST", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.follow_distance_m = value; - } - }, - this); - _parent->get_param_int_async("NAV_FT_FS", - [this](MAVLinkParameters::Result result, int32_t value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.follow_direction = - static_cast(value); - } - }, - this); - _parent->get_param_float_async("NAV_FT_RS", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.responsiveness = value; - } - }, - this); + _parent->get_param_float_async( + "NAV_MIN_FT_HT", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.min_height_m = value; + } + }, + this); + _parent->get_param_float_async( + "NAV_FT_DST", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.follow_distance_m = value; + } + }, + this); + _parent->get_param_int_async( + "NAV_FT_FS", + [this](MAVLinkParameters::Result result, int32_t value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.follow_direction = static_cast(value); + } + }, + this); + _parent->get_param_float_async( + "NAV_FT_RS", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.responsiveness = value; + } + }, + this); } void FollowMeImpl::disable() diff --git a/plugins/offboard/include/plugins/offboard/offboard.h b/plugins/offboard/include/plugins/offboard/offboard.h index 0c5a0695f5..574fb915c9 100644 --- a/plugins/offboard/include/plugins/offboard/offboard.h +++ b/plugins/offboard/include/plugins/offboard/offboard.h @@ -273,17 +273,17 @@ 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. -*/ + * @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. -*/ + * @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/offboard.cpp b/plugins/offboard/offboard.cpp index 42fede2295..5a462ba91e 100644 --- a/plugins/offboard/offboard.cpp +++ b/plugins/offboard/offboard.cpp @@ -82,10 +82,8 @@ 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; + 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) @@ -98,27 +96,22 @@ std::ostream &operator<<(std::ostream &str, Offboard::AttitudeRate const &attitu 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 && + 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 + << ", 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; + 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, @@ -132,10 +125,8 @@ std::ostream &operator<<(std::ostream &str, 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; + 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) From 93bd11ea28d6ce648cbb0578b120d1696a8a3950 Mon Sep 17 00:00:00 2001 From: Jonas Vautherin Date: Sat, 4 May 2019 01:55:07 +0200 Subject: [PATCH 12/12] fix style with clang-format 6 (clang-format 8 having a bug) --- core/mavlink_parameters.cpp | 15 +++--- plugins/camera/camera_impl.cpp | 80 ++++++++++++++-------------- plugins/follow_me/follow_me_impl.cpp | 61 ++++++++++----------- 3 files changed, 75 insertions(+), 81 deletions(-) diff --git a/core/mavlink_parameters.cpp b/core/mavlink_parameters.cpp index eceed5e4f4..8ff1bb783f 100644 --- a/core/mavlink_parameters.cpp +++ b/core/mavlink_parameters.cpp @@ -114,14 +114,13 @@ MAVLinkParameters::get_param(const std::string &name, ParamValue value_type, boo auto prom = std::promise>(); auto res = prom.get_future(); - get_param_async( - name, - value_type, - [&prom](Result result, ParamValue value) { - prom.set_value(std::make_pair<>(result, value)); - }, - this, - extended); + get_param_async(name, + value_type, + [&prom](Result result, ParamValue value) { + prom.set_value(std::make_pair<>(result, value)); + }, + this, + extended); return res.get(); } diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index ab971a8b54..f36eabe4e5 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -1259,26 +1259,25 @@ void CameraImpl::set_option_async(const std::string &setting_id, return; } - _parent->set_param_async( - setting_id, - value, - [this, callback, setting_id, value](MAVLinkParameters::Result result) { - if (result == MAVLinkParameters::Result::SUCCESS) { - if (this->_camera_definition) { - _camera_definition->set_setting(setting_id, value); - refresh_params(); - } - if (callback) { - callback(Camera::Result::SUCCESS); - } - } else { - if (callback) { - callback(Camera::Result::ERROR); - } - } - }, - this, - true); + _parent->set_param_async(setting_id, + value, + [this, callback, setting_id, value](MAVLinkParameters::Result result) { + if (result == MAVLinkParameters::Result::SUCCESS) { + if (this->_camera_definition) { + _camera_definition->set_setting(setting_id, value); + refresh_params(); + } + if (callback) { + callback(Camera::Result::SUCCESS); + } + } else { + if (callback) { + callback(Camera::Result::ERROR); + } + } + }, + this, + true); } Camera::Result CameraImpl::get_option(const std::string &setting_id, Camera::Option &option) @@ -1458,27 +1457,26 @@ void CameraImpl::refresh_params() const std::string ¶m_name = param.first; const MAVLinkParameters::ParamValue ¶m_value_type = param.second; const bool is_last = (count + 1 == params.size()); - _parent->get_param_async( - param_name, - param_value_type, - [param_name, is_last, this](MAVLinkParameters::Result result, - MAVLinkParameters::ParamValue value) { - if (result != MAVLinkParameters::Result::SUCCESS) { - return; - } - // We need to check again by the time this callback runs - if (!this->_camera_definition) { - return; - } - this->_camera_definition->set_setting(param_name, value); - - if (is_last) { - notify_current_settings(); - notify_possible_setting_options(); - } - }, - this, - true); + _parent->get_param_async(param_name, + param_value_type, + [param_name, is_last, this](MAVLinkParameters::Result result, + MAVLinkParameters::ParamValue value) { + if (result != MAVLinkParameters::Result::SUCCESS) { + return; + } + // We need to check again by the time this callback runs + if (!this->_camera_definition) { + return; + } + this->_camera_definition->set_setting(param_name, value); + + if (is_last) { + notify_current_settings(); + notify_possible_setting_options(); + } + }, + this, + true); ++count; } } diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp index 7c1eccae6e..39418f40e7 100644 --- a/plugins/follow_me/follow_me_impl.cpp +++ b/plugins/follow_me/follow_me_impl.cpp @@ -34,38 +34,35 @@ void FollowMeImpl::deinit() void FollowMeImpl::enable() { - _parent->get_param_float_async( - "NAV_MIN_FT_HT", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.min_height_m = value; - } - }, - this); - _parent->get_param_float_async( - "NAV_FT_DST", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.follow_distance_m = value; - } - }, - this); - _parent->get_param_int_async( - "NAV_FT_FS", - [this](MAVLinkParameters::Result result, int32_t value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.follow_direction = static_cast(value); - } - }, - this); - _parent->get_param_float_async( - "NAV_FT_RS", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.responsiveness = value; - } - }, - this); + _parent->get_param_float_async("NAV_MIN_FT_HT", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.min_height_m = value; + } + }, + this); + _parent->get_param_float_async("NAV_FT_DST", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.follow_distance_m = value; + } + }, + this); + _parent->get_param_int_async("NAV_FT_FS", + [this](MAVLinkParameters::Result result, int32_t value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.follow_direction = + static_cast(value); + } + }, + this); + _parent->get_param_float_async("NAV_FT_RS", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.responsiveness = value; + } + }, + this); } void FollowMeImpl::disable()