camera: status, mode, capture, status, tests

This adds more camera features and some integration tests for it.

integration_tests/CMakeLists.txt

@@ -33,6 +33,10 @@ add_executable(integration_tests_runner
     gimbal.cpp
     transition_multicopter_fixedwing.cpp
     follow_me.cpp
+    camera_take_photo.cpp
+    camera_take_photo_interval.cpp
+    camera_mode.cpp
+    camera_test_helpers.cpp
 )
 
 include_directories(
@@ -54,6 +58,7 @@ target_link_libraries(integration_tests_runner
     dronecore_info
     dronecore_gimbal
     dronecore_follow_me
+    dronecore_camera
     gtest
     gtest_main
     gmock

integration_tests/camera_mode.cpp

@@ -0,0 +1,66 @@
+#include <iostream>
+#include <future>
+#include "integration_test_helper.h"
+#include "global_include.h"
+#include "dronecore.h"
+#include "camera_test_helpers.h"
+
+
+using namespace dronecore;
+
+
+TEST(CameraTest, ChangeMode)
+{
+    DroneCore dc;
+
+    ConnectionResult ret = dc.add_udp_connection();
+    ASSERT_EQ(ret, ConnectionResult::SUCCESS);
+
+    // Wait for device to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    Device &device = dc.device();
+    auto camera = std::make_shared<Camera>(device);
+
+    set_mode(camera, Camera::Mode::PHOTO);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    Camera::Mode new_mode = get_mode(camera);
+    EXPECT_EQ(new_mode, Camera::Mode::PHOTO);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    set_mode(camera, Camera::Mode::PHOTO_SURVEY);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    new_mode = get_mode(camera);
+    EXPECT_EQ(new_mode, Camera::Mode::PHOTO_SURVEY);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    set_mode(camera, Camera::Mode::VIDEO);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    new_mode = get_mode(camera);
+    EXPECT_EQ(new_mode, Camera::Mode::VIDEO);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    set_mode(camera, Camera::Mode::PHOTO_SURVEY);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    new_mode = get_mode(camera);
+    EXPECT_EQ(new_mode, Camera::Mode::PHOTO_SURVEY);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    set_mode(camera, Camera::Mode::PHOTO);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    new_mode = get_mode(camera);
+    EXPECT_EQ(new_mode, Camera::Mode::PHOTO);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    set_mode(camera, Camera::Mode::VIDEO);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    new_mode = get_mode(camera);
+    EXPECT_EQ(new_mode, Camera::Mode::VIDEO);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    set_mode(camera, Camera::Mode::PHOTO);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    new_mode = get_mode(camera);
+    EXPECT_EQ(new_mode, Camera::Mode::PHOTO);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+}

integration_tests/camera_take_photo.cpp

@@ -0,0 +1,106 @@
+#include <iostream>
+#include <functional>
+#include <vector>
+#include <atomic>
+#include <future>
+#include "integration_test_helper.h"
+#include "global_include.h"
+#include "dronecore.h"
+#include "camera_test_helpers.h"
+
+using namespace dronecore;
+using namespace std::placeholders; // for `_1`
+
+static void receive_camera_result(Camera::Result result);
+
+static void receive_capture_info(Camera::CaptureInfo capture_info);
+
+static std::atomic<bool> _received_result {false};
+static std::atomic<bool> _received_capture_info {false};
+
+
+TEST(CameraTest, TakePhoto)
+{
+    DroneCore dc;
+
+    ConnectionResult ret = dc.add_udp_connection();
+    ASSERT_EQ(ret, ConnectionResult::SUCCESS);
+
+    // Wait for device to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    Device &device = dc.device();
+    auto camera = std::make_shared<Camera>(device);
+
+    // We want to take the picture in photo mode.
+    set_mode(camera, Camera::Mode::PHOTO);
+
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+
+    camera->capture_info_async(std::bind(&receive_capture_info, _1));
+
+    camera->take_photo_async(std::bind(&receive_camera_result, _1));
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    EXPECT_TRUE(_received_capture_info);
+}
+
+TEST(CameraTest, TakeMultiplePhotos)
+{
+    DroneCore dc;
+
+    const int num_photos_to_take = 3;
+
+    ConnectionResult ret = dc.add_udp_connection();
+    ASSERT_EQ(ret, ConnectionResult::SUCCESS);
+
+    // Wait for device to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    Device &device = dc.device();
+    auto camera = std::make_shared<Camera>(device);
+
+    // We want to take the picture in photo mode.
+    set_mode(camera, Camera::Mode::PHOTO);
+
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+
+    camera->capture_info_async(std::bind(&receive_capture_info, _1));
+
+    for (unsigned i = 0; i < num_photos_to_take; ++i) {
+        camera->take_photo_async(std::bind(&receive_camera_result, _1));
+        LogDebug() << "taking picture: " << i;
+        std::this_thread::sleep_for(std::chrono::milliseconds(1500));
+        EXPECT_TRUE(_received_capture_info);
+        _received_capture_info = false;
+    }
+}
+
+void receive_camera_result(Camera::Result result)
+{
+    _received_result = true;
+    EXPECT_EQ(result, Camera::Result::SUCCESS);
+}
+
+void receive_capture_info(Camera::CaptureInfo capture_info)
+{
+    LogInfo() << "New capture at "
+              << capture_info.position.latitude_deg << ", "
+              << capture_info.position.longitude_deg << ", "
+              << capture_info.position.absolute_altitude_m << " m, "
+              << capture_info.position.relative_altitude_m << " m (relative to home).";
+    LogInfo() << "Time: "
+              << capture_info.time_utc_us << " us.";
+    LogInfo() << "Attitude: "
+              << capture_info.quaternion.w << ", "
+              << capture_info.quaternion.x << ", "
+              << capture_info.quaternion.y << ", "
+              << capture_info.quaternion.z << ".";
+    LogInfo() << "Result: "
+              << (capture_info.success ? "success" : "fail") << ".";
+    LogInfo() << "Saved to "
+              << capture_info.file_url << " ("
+              << capture_info.index << ").";
+
+    _received_capture_info = true;
+}

integration_tests/camera_take_photo_interval.cpp

@@ -0,0 +1,85 @@
+#include <iostream>
+#include <functional>
+#include <vector>
+#include <atomic>
+#include <future>
+#include "integration_test_helper.h"
+#include "global_include.h"
+#include "dronecore.h"
+#include "camera_test_helpers.h"
+
+using namespace dronecore;
+using namespace std::placeholders; // for `_1`
+
+static void receive_camera_result(Camera::Result result);
+
+static void check_interval_on(std::shared_ptr<Camera> camera, bool on);
+
+static std::atomic<bool> _received_result {false};
+
+
+TEST(CameraTest, TakePhotoInterval)
+{
+    DroneCore dc;
+
+    ConnectionResult ret = dc.add_udp_connection();
+    ASSERT_EQ(ret, ConnectionResult::SUCCESS);
+
+    // Wait for device to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    Device &device = dc.device();
+    auto camera = std::make_shared<Camera>(device);
+
+    // We want to take the pictures in photo mode.
+    set_mode(camera, Camera::Mode::PHOTO);
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+
+    check_interval_on(camera, false);
+
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+
+    _received_result = false;
+    camera->start_photo_interval_async(2.0f, std::bind(&receive_camera_result, _1));
+
+    // Wait for 3 photos
+    std::this_thread::sleep_for(std::chrono::seconds(7));
+
+    check_interval_on(camera, true);
+
+    // Wait for another photo
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    // Then, stop it again.
+    camera->stop_photo_interval_async(std::bind(&receive_camera_result, _1));
+
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+    check_interval_on(camera, false);
+
+    EXPECT_TRUE(_received_result);
+}
+
+void receive_camera_result(Camera::Result result)
+{
+    _received_result = true;
+    EXPECT_EQ(result, Camera::Result::SUCCESS);
+}
+
+
+void check_interval_on(std::shared_ptr<Camera> camera, bool on)
+{
+    auto prom = std::make_shared<std::promise<void>>();
+    auto ret = prom->get_future();
+
+    // Check if status is correct
+    camera->get_status_async([prom, on](Camera::Result result, Camera::Status status) {
+        EXPECT_EQ(result, Camera::Result::SUCCESS);
+        EXPECT_EQ(status.photo_interval_on, on);
+        prom->set_value();
+    });
+
+    // Block now for a while to wait for result.
+    auto status = ret.wait_for(std::chrono::seconds(1));
+
+    EXPECT_EQ(status, std::future_status::ready);
+}

integration_tests/camera_test_helpers.cpp

@@ -0,0 +1,65 @@
+#include <memory>
+#include <future>
+#include "integration_test_helper.h"
+#include "camera_test_helpers.h"
+
+using namespace dronecore;
+
+Camera::Mode get_mode(std::shared_ptr<Camera> camera)
+{
+    struct PromiseResult {
+        Camera::Result result;
+        Camera::Mode mode;
+    };
+
+    auto prom = std::make_shared<std::promise<PromiseResult>>();
+    auto ret = prom->get_future();
+
+    // Let's get the mode first
+    camera->get_mode_async([prom](Camera::Result result, Camera::Mode mode) {
+        PromiseResult pr {};
+        pr.result = result;
+        pr.mode = mode;
+        prom->set_value(pr);
+    });
+
+    // Block now for a while to wait for result.
+    auto status = ret.wait_for(std::chrono::seconds(7));
+
+    EXPECT_EQ(status, std::future_status::ready);
+
+    if (status == std::future_status::ready) {
+        PromiseResult new_ret = ret.get();
+        EXPECT_EQ(new_ret.result, Camera::Result::SUCCESS);
+        EXPECT_NE(new_ret.mode, Camera::Mode::UNKNOWN);
+        return new_ret.mode;
+    } else {
+        return Camera::Mode::UNKNOWN;
+    }
+}
+
+void set_mode(std::shared_ptr<Camera> camera, Camera::Mode mode)
+{
+    // FIXME: this should not be required.
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+
+    auto prom = std::make_shared<std::promise<void>>();
+    auto ret = prom->get_future();
+
+    // Let's get the mode first
+    camera->set_mode_async(mode, [mode, prom](Camera::Result result,
+    Camera::Mode mode_got) {
+        EXPECT_EQ(result, Camera::Result::SUCCESS);
+        EXPECT_EQ(mode, mode_got);
+        prom->set_value();
+    });
+
+    // Block now for a while to wait for result.
+    auto status = ret.wait_for(std::chrono::seconds(10));
+
+    EXPECT_EQ(status, std::future_status::ready);
+
+    if (status == std::future_status::ready) {
+        ret.get();
+    }
+}

integration_tests/camera_test_helpers.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#include "dronecore.h"
+#include "plugins/camera/camera.h"
+
+dronecore::Camera::Mode get_mode(std::shared_ptr<dronecore::Camera> camera);
+void set_mode(std::shared_ptr<dronecore::Camera> camera, dronecore::Camera::Mode mode);

plugins/camera/camera.cpp

@@ -73,6 +73,16 @@ void Camera::get_mode_async(const mode_callback_t &callback)
     _impl->get_mode_async(callback);
 }
 
+void Camera::get_status_async(get_status_callback_t callback)
+{
+    _impl->get_status_async(callback);
+}
+
+void Camera::capture_info_async(capture_info_callback_t callback)
+{
+    _impl->capture_info_async(callback);
+}
+
 const char *Camera::result_str(Result result)
 {
     switch (result) {