Skip to content

Commit

Permalink
backend: add connection_initiator and corresponding tests
Browse files Browse the repository at this point in the history
  • Loading branch information
JonasVautherin committed Mar 1, 2018
1 parent 614a0c6 commit 2ac2e5c
Show file tree
Hide file tree
Showing 9 changed files with 239 additions and 82 deletions.
23 changes: 15 additions & 8 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,17 @@ list(APPEND CMAKE_FIND_ROOT_PATH ${CMAKE_BINARY_DIR}/third_party)

project(dronecore)

option(BUILD_TESTS "Build tests" ON)

include(cmake/compiler_flags.cmake)
include(cmake/zlib.cmake)
include(cmake/curl.cmake)

if(BUILD_TESTS AND (IOS OR ANDROID))
message(STATUS "Building for iOS or Android: forcing BUILD_TESTS to FALSE...")
set(BUILD_TESTS OFF)
endif()

if(ANDROID)
set(lib_path "lib/android/${ANDROID_ABI}")
elseif(IOS)
Expand All @@ -31,14 +38,7 @@ if (DEFINED EXTERNAL_DIR AND NOT EXTERNAL_DIR STREQUAL "")
include_directories(${EXTERNAL_DIR})
endif()

if (CMAKE_BUILD_DRONECORESERVER)
message(STATUS "Building dronecore server")
add_subdirectory(grpc/server)
else()
message(STATUS "BUILD_DRONECORESERVER not set: not building dronecore server")
endif()

if(NOT IOS AND NOT ANDROID)
if(BUILD_TESTS)
enable_testing()
add_subdirectory(${CMAKE_SOURCE_DIR}/third_party/gtest EXCLUDE_FROM_ALL)

Expand All @@ -52,6 +52,13 @@ if(NOT IOS AND NOT ANDROID)
include(cmake/unit_tests.cmake)
endif()

if (CMAKE_BUILD_DRONECORESERVER)
message(STATUS "Building dronecore server")
add_subdirectory(grpc/server)
else()
message(STATUS "BUILD_DRONECORESERVER not set: not building dronecore server")
endif()

if (DROP_DEBUG EQUAL 1)
add_definitions(-DDROP_DEBUG=${DROP_DEBUG})

Expand Down
4 changes: 1 addition & 3 deletions cmake/unit_tests.cmake
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,4 @@ target_link_libraries(unit_tests_runner
gmock
)

add_test(unit_tests
unit_tests_runner
)
add_test(unit_tests unit_tests_runner)
19 changes: 19 additions & 0 deletions core/mocks/dronecore_mock.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
#include <gmock/gmock.h>

#include "connection_result.h"

namespace dronecore {
namespace testing {

typedef std::function<void(uint64_t uuid)> event_callback_t;

class MockDroneCore
{
public:
MOCK_CONST_METHOD1(add_udp_connection, ConnectionResult(int local_port_number));
MOCK_CONST_METHOD1(register_on_discover, void(event_callback_t));
MOCK_CONST_METHOD1(register_on_timeout, void(event_callback_t));
};

} // testing
} // dronecore
4 changes: 4 additions & 0 deletions grpc/server/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -9,3 +9,7 @@ include(GNUInstallDirs)
include(cmake/helpers/build_external.cmake)

add_subdirectory(src)

if(BUILD_TESTS)
add_subdirectory(test)
endif()
70 changes: 8 additions & 62 deletions grpc/server/src/backend.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,88 +17,34 @@ namespace backend {

bool DroneCoreBackend::run(const int mavlink_listen_port)
{
if (!connect_to_vehicle(mavlink_listen_port)) {
return false;
}
_connection_initiator.start(_dc, 14540);
_connection_initiator.wait();

grpc::ServerBuilder builder;
setup_port(builder);

CoreServiceImpl core(dc);
CoreServiceImpl core(_dc);
builder.RegisterService(&core);

Action action(&dc.device());
Action action(&_dc.device());
ActionServiceImpl actionService(action);
builder.RegisterService(&actionService);

Mission mission(&dc.device());
Mission mission(&_dc.device());
MissionServiceImpl missionService(mission);
builder.RegisterService(&missionService);

Telemetry telemetry(&dc.device());
Telemetry telemetry(&_dc.device());
TelemetryServiceImpl telemetryService(telemetry);
builder.RegisterService(&telemetryService);

server = builder.BuildAndStart();
_server = builder.BuildAndStart();
LogInfo() << "Server started";
server->Wait();
_server->Wait();

return true;
}

bool DroneCoreBackend::connect_to_vehicle(const int port)
{
if (!add_udp_connection(port)) {
return false;
}

log_uuid_on_timeout();
wait_for_discovery();

return true;
}

bool DroneCoreBackend::add_udp_connection(const int port)
{
dronecore::ConnectionResult connection_result = dc.add_udp_connection(port);

if (connection_result != ConnectionResult::SUCCESS) {
LogErr() << "Connection failed: " << connection_result_str(connection_result);
return false;
}

return true;
}

void DroneCoreBackend::log_uuid_on_timeout()
{
dc.register_on_timeout([](uint64_t uuid) {
LogInfo() << "Device timed out [UUID: " << uuid << "]";
});
}

void DroneCoreBackend::wait_for_discovery()
{
LogInfo() << "Waiting to discover device...";
auto discoveryFuture = wrapped_register_on_discover();
discoveryFuture.wait();
LogInfo() << "Device discovered [UUID: " << discoveryFuture.get() << "]";
}

std::future<uint64_t> DroneCoreBackend::wrapped_register_on_discover()
{
auto promise = std::make_shared<std::promise<uint64_t>>();
auto future = promise->get_future();

dc.register_on_discover([this, promise](uint64_t uuid) {
std::call_once(discovery_flag, [promise, uuid]() {
promise->set_value(uuid);
});
});

return future;
}

void DroneCoreBackend::setup_port(grpc::ServerBuilder &builder)
{
std::string server_address("0.0.0.0:50051");
Expand Down
14 changes: 5 additions & 9 deletions grpc/server/src/backend.h
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
#include <future>
#include <grpc++/server.h>
#include <memory>
#include <mutex>

#include "connection_initiator.h"
#include "dronecore.h"

namespace dronecore {
Expand All @@ -16,17 +17,12 @@ class DroneCoreBackend
bool run(const int mavlink_listen_port = 14540);

private:
bool connect_to_vehicle(int port);
bool add_udp_connection(int port);
void log_uuid_on_timeout();
void wait_for_discovery();
std::future<uint64_t> wrapped_register_on_discover();

bool run_server();
void setup_port(grpc::ServerBuilder &builder);

dronecore::DroneCore dc;
std::unique_ptr<grpc::Server> server;
dronecore::DroneCore _dc;
dronecore::backend::ConnectionInitiator<dronecore::DroneCore> _connection_initiator;
std::unique_ptr<grpc::Server> _server;
};

} // namespace backend
Expand Down
82 changes: 82 additions & 0 deletions grpc/server/src/connection_initiator.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,82 @@
#include <future>

#include "connection_result.h"
#include "log.h"

namespace dronecore {
namespace backend {

template <typename DroneCore>
class ConnectionInitiator
{
public:
ConnectionInitiator() {}
~ConnectionInitiator() {}

bool start(DroneCore &dc, const int port)
{
init_mutex();
init_timeout_logging(dc);

_discovery_future = wrapped_register_on_discover(dc);

if (!add_udp_connection(dc, port)) {
return false;
}

return true;
}

void wait()
{
_discovery_future.wait();
}

private:
void init_mutex()
{
_discovery_promise = std::make_shared<std::promise<uint64_t>>();
}

void init_timeout_logging(DroneCore &dc) const
{
dc.register_on_timeout([](uint64_t uuid) {
LogInfo() << "Device timed out [UUID: " << uuid << "]";
});
}

bool add_udp_connection(DroneCore &dc, const int port)
{
dronecore::ConnectionResult connection_result = dc.add_udp_connection(port);

if (connection_result != ConnectionResult::SUCCESS) {
LogErr() << "Connection failed: " << connection_result_str(connection_result);
return false;
}

return true;
}

std::future<uint64_t> wrapped_register_on_discover(DroneCore &dc)
{
auto future = _discovery_promise->get_future();

LogInfo() << "Waiting to discover device...";

dc.register_on_discover([this](uint64_t uuid) {
std::call_once(_discovery_flag, [this, uuid]() {
LogInfo() << "Device discovered [UUID: " << uuid << "]";
_discovery_promise->set_value(uuid);
});
});

return future;
}

std::once_flag _discovery_flag;
std::shared_ptr<std::promise<uint64_t>> _discovery_promise;
std::future<uint64_t> _discovery_future;
};

} // backend
} // dronecore
21 changes: 21 additions & 0 deletions grpc/server/test/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
cmake_minimum_required(VERSION 3.1)

add_executable(unit_tests_backend
connection_initiator_test.cpp
)

set_target_properties(unit_tests_backend PROPERTIES COMPILE_FLAGS ${warnings})

target_include_directories(unit_tests_backend
PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/../src
)

target_link_libraries(unit_tests_backend
backend
gtest
gmock
gmock_main
)

add_test(unit_tests unit_tests_backend)
Loading

0 comments on commit 2ac2e5c

Please sign in to comment.