Add camera server

examples/CMakeLists.txt

@@ -11,6 +11,7 @@ add_subdirectory(autopilot_server)
 add_subdirectory(battery)
 add_subdirectory(calibrate)
 add_subdirectory(camera)
+add_subdirectory(camera_server)
 add_subdirectory(camera_settings)
 add_subdirectory(fly_mission)
 add_subdirectory(fly_multiple_drones)

examples/camera_server/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+project(camera_server)
+
+add_executable(camera_server
+    camera_server.cpp
+)
+
+find_package(MAVSDK REQUIRED)
+
+target_link_libraries(camera_server
+    MAVSDK::mavsdk
+)
+
+if(NOT MSVC)
+    add_compile_options(camera_server PRIVATE -Wall -Wextra)
+else()
+    add_compile_options(camera_server PRIVATE -WX -W2)
+endif()

examples/camera_server/camera_server.cpp

@@ -0,0 +1,121 @@
+#include <future>
+
+#include <iostream>
+#include <map>
+#include <thread>
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/camera/camera.h>
+#include <mavsdk/plugins/camera_server/camera_server.h>
+
+using namespace mavsdk;
+
+using std::chrono::duration_cast;
+using std::chrono::seconds;
+using std::chrono::milliseconds;
+using std::chrono::system_clock;
+using std::this_thread::sleep_for;
+
+int main(int argc, char** argv)
+{
+    mavsdk::Mavsdk mavsdk;
+    mavsdk::Mavsdk::Configuration configuration(mavsdk::Mavsdk::Configuration::UsageType::Camera);
+    mavsdk.set_configuration(configuration);
+
+    // 14030 is the default camera port for PX4 SITL
+    auto result = mavsdk.add_any_connection("udp://:14030");
+    if (result == mavsdk::ConnectionResult::Success) {
+        std::cout << "Created camera server connection" << std::endl;
+    }
+
+    // map of system id to camera server instance
+    std::map<uint8_t, std::shared_ptr<mavsdk::CameraServer>> all_camera_servers{};
+
+    auto attach_camera_server = [&]() {
+        for (auto&& system : mavsdk.systems()) {
+            // if we have already created a camera server for this system, skip it
+            if (all_camera_servers.find(system->get_system_id()) != all_camera_servers.end()) {
+                continue;
+            }
+
+            // Create server plugin
+            auto camera_server = std::make_shared<mavsdk::CameraServer>(system);
+
+            all_camera_servers.insert({system->get_system_id(), camera_server});
+
+            // First add all subscriptions. This defines the camera capabilities.
+
+            camera_server->subscribe_take_photo(
+                [&all_camera_servers](CameraServer::Result result, int32_t index) {
+                    for (auto&& [key, value] : all_camera_servers) {
+                        value->set_in_progress(true);
+                    }
+
+                    std::cout << "taking a picture (" << +index << ")..." << std::endl;
+
+                    // TODO : actually capture image here
+                    // simulating with delay
+                    sleep_for(milliseconds(500));
+
+                    // TODO: populate with telemetry data
+                    auto position = CameraServer::Position{};
+                    auto attitude = CameraServer::Quaternion{};
+
+                    auto timestamp =
+                        duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
+                    auto success = true;
+
+                    for (auto&& [key, value] : all_camera_servers) {
+                        value->set_in_progress(false);
+                    }
+
+                    for (auto&& [key, value] : all_camera_servers) {
+                        value->respond_take_photo({
+                            .position = position,
+                            .attitude_quaternion = attitude,
+                            .time_utc_us = static_cast<uint64_t>(timestamp),
+                            .is_success = success,
+                            .index = index,
+                            .file_url = {},
+                        });
+                    }
+                });
+
+            // Then set the initial state of everything.
+
+            // TODO: this state is not guaranteed, e.g. a new system appears
+            // while a capture is in progress
+            camera_server->set_in_progress(false);
+
+            // Finally call set_information() to "activate" the camera plugin.
+
+            auto ret = camera_server->set_information({
+                .vendor_name = "MAVSDK",
+                .model_name = "Example Camera Server",
+                .firmware_version = "1.0.0",
+                .focal_length_mm = 3.0,
+                .horizontal_sensor_size_mm = 3.68,
+                .vertical_sensor_size_mm = 2.76,
+                .horizontal_resolution_px = 3280,
+                .vertical_resolution_px = 2464,
+            });
+
+            if (ret != CameraServer::Result::Success) {
+                std::cerr << "Failed to set camera info, exiting" << std::endl;
+                exit(1);
+            }
+
+            std::cout << "Connected to " << (system->is_standalone() ? "GCS" : "autopilot")
+                      << " system ID " << +system->get_system_id() << std::endl;
+        }
+    };
+
+    mavsdk.subscribe_on_new_system(attach_camera_server);
+    attach_camera_server();
+
+    while (true) {
+        // TODO: better way to wait forever?
+        sleep_for(seconds(1));
+    }
+
+    return 0;
+}

src/mavsdk/plugins/CMakeLists.txt

@@ -2,6 +2,7 @@ add_subdirectory(action)
 add_subdirectory(action_server)
 add_subdirectory(calibration)
 add_subdirectory(camera)
+add_subdirectory(camera_server)
 add_subdirectory(failure)
 add_subdirectory(follow_me)
 add_subdirectory(ftp)

src/mavsdk/plugins/camera_server/CMakeLists.txt

@@ -0,0 +1,15 @@
+target_sources(mavsdk
+    PRIVATE
+    camera_server.cpp
+    camera_server_impl.cpp
+)
+
+target_include_directories(mavsdk PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include/mavsdk>
+    )
+
+install(FILES
+    include/plugins/camera_server/camera_server.h
+    DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/mavsdk/plugins/camera_server
+)

src/mavsdk/plugins/camera_server/camera_server.cpp

@@ -0,0 +1,178 @@
+// WARNING: THIS FILE IS AUTOGENERATED! As such, it should not be edited.
+// Edits need to be made to the proto files
+// (see
+// https://github.com/mavlink/MAVSDK-Proto/blob/master/protos/camera_server/camera_server.proto)
+
+#include <iomanip>
+
+#include "camera_server_impl.h"
+#include "plugins/camera_server/camera_server.h"
+
+namespace mavsdk {
+
+using Information = CameraServer::Information;
+using Position = CameraServer::Position;
+using Quaternion = CameraServer::Quaternion;
+using CaptureInfo = CameraServer::CaptureInfo;
+
+CameraServer::CameraServer(System& system) :
+    PluginBase(),
+    _impl{std::make_unique<CameraServerImpl>(system)}
+{}
+
+CameraServer::CameraServer(std::shared_ptr<System> system) :
+    PluginBase(),
+    _impl{std::make_unique<CameraServerImpl>(system)}
+{}
+
+CameraServer::~CameraServer() {}
+
+CameraServer::Result CameraServer::set_information(Information information) const
+{
+    return _impl->set_information(information);
+}
+
+CameraServer::Result CameraServer::set_in_progress(bool in_progress) const
+{
+    return _impl->set_in_progress(in_progress);
+}
+
+void CameraServer::subscribe_take_photo(TakePhotoCallback callback)
+{
+    _impl->subscribe_take_photo(callback);
+}
+
+CameraServer::Result CameraServer::respond_take_photo(CaptureInfo capture_info) const
+{
+    return _impl->respond_take_photo(capture_info);
+}
+
+bool operator==(const CameraServer::Information& lhs, const CameraServer::Information& rhs)
+{
+    return (rhs.vendor_name == lhs.vendor_name) && (rhs.model_name == lhs.model_name) &&
+           (rhs.firmware_version == lhs.firmware_version) &&
+           ((std::isnan(rhs.focal_length_mm) && std::isnan(lhs.focal_length_mm)) ||
+            rhs.focal_length_mm == lhs.focal_length_mm) &&
+           ((std::isnan(rhs.horizontal_sensor_size_mm) &&
+             std::isnan(lhs.horizontal_sensor_size_mm)) ||
+            rhs.horizontal_sensor_size_mm == lhs.horizontal_sensor_size_mm) &&
+           ((std::isnan(rhs.vertical_sensor_size_mm) && std::isnan(lhs.vertical_sensor_size_mm)) ||
+            rhs.vertical_sensor_size_mm == lhs.vertical_sensor_size_mm) &&
+           (rhs.horizontal_resolution_px == lhs.horizontal_resolution_px) &&
+           (rhs.vertical_resolution_px == lhs.vertical_resolution_px) &&
+           (rhs.lens_id == lhs.lens_id) &&
+           (rhs.definition_file_version == lhs.definition_file_version) &&
+           (rhs.definition_file_uri == lhs.definition_file_uri);
+}
+
+std::ostream& operator<<(std::ostream& str, CameraServer::Information const& information)
+{
+    str << std::setprecision(15);
+    str << "information:" << '\n' << "{\n";
+    str << "    vendor_name: " << information.vendor_name << '\n';
+    str << "    model_name: " << information.model_name << '\n';
+    str << "    firmware_version: " << information.firmware_version << '\n';
+    str << "    focal_length_mm: " << information.focal_length_mm << '\n';
+    str << "    horizontal_sensor_size_mm: " << information.horizontal_sensor_size_mm << '\n';
+    str << "    vertical_sensor_size_mm: " << information.vertical_sensor_size_mm << '\n';
+    str << "    horizontal_resolution_px: " << information.horizontal_resolution_px << '\n';
+    str << "    vertical_resolution_px: " << information.vertical_resolution_px << '\n';
+    str << "    lens_id: " << information.lens_id << '\n';
+    str << "    definition_file_version: " << information.definition_file_version << '\n';
+    str << "    definition_file_uri: " << information.definition_file_uri << '\n';
+    str << '}';
+    return str;
+}
+
+bool operator==(const CameraServer::Position& lhs, const CameraServer::Position& rhs)
+{
+    return ((std::isnan(rhs.latitude_deg) && std::isnan(lhs.latitude_deg)) ||
+            rhs.latitude_deg == lhs.latitude_deg) &&
+           ((std::isnan(rhs.longitude_deg) && std::isnan(lhs.longitude_deg)) ||
+            rhs.longitude_deg == lhs.longitude_deg) &&
+           ((std::isnan(rhs.absolute_altitude_m) && std::isnan(lhs.absolute_altitude_m)) ||
+            rhs.absolute_altitude_m == lhs.absolute_altitude_m) &&
+           ((std::isnan(rhs.relative_altitude_m) && std::isnan(lhs.relative_altitude_m)) ||
+            rhs.relative_altitude_m == lhs.relative_altitude_m);
+}
+
+std::ostream& operator<<(std::ostream& str, CameraServer::Position const& position)
+{
+    str << std::setprecision(15);
+    str << "position:" << '\n' << "{\n";
+    str << "    latitude_deg: " << position.latitude_deg << '\n';
+    str << "    longitude_deg: " << position.longitude_deg << '\n';
+    str << "    absolute_altitude_m: " << position.absolute_altitude_m << '\n';
+    str << "    relative_altitude_m: " << position.relative_altitude_m << '\n';
+    str << '}';
+    return str;
+}
+
+bool operator==(const CameraServer::Quaternion& lhs, const CameraServer::Quaternion& rhs)
+{
+    return ((std::isnan(rhs.w) && std::isnan(lhs.w)) || rhs.w == lhs.w) &&
+           ((std::isnan(rhs.x) && std::isnan(lhs.x)) || rhs.x == lhs.x) &&
+           ((std::isnan(rhs.y) && std::isnan(lhs.y)) || rhs.y == lhs.y) &&
+           ((std::isnan(rhs.z) && std::isnan(lhs.z)) || rhs.z == lhs.z);
+}
+
+std::ostream& operator<<(std::ostream& str, CameraServer::Quaternion const& quaternion)
+{
+    str << std::setprecision(15);
+    str << "quaternion:" << '\n' << "{\n";
+    str << "    w: " << quaternion.w << '\n';
+    str << "    x: " << quaternion.x << '\n';
+    str << "    y: " << quaternion.y << '\n';
+    str << "    z: " << quaternion.z << '\n';
+    str << '}';
+    return str;
+}
+
+bool operator==(const CameraServer::CaptureInfo& lhs, const CameraServer::CaptureInfo& rhs)
+{
+    return (rhs.position == lhs.position) && (rhs.attitude_quaternion == lhs.attitude_quaternion) &&
+           (rhs.time_utc_us == lhs.time_utc_us) && (rhs.is_success == lhs.is_success) &&
+           (rhs.index == lhs.index) && (rhs.file_url == lhs.file_url);
+}
+
+std::ostream& operator<<(std::ostream& str, CameraServer::CaptureInfo const& capture_info)
+{
+    str << std::setprecision(15);
+    str << "capture_info:" << '\n' << "{\n";
+    str << "    position: " << capture_info.position << '\n';
+    str << "    attitude_quaternion: " << capture_info.attitude_quaternion << '\n';
+    str << "    time_utc_us: " << capture_info.time_utc_us << '\n';
+    str << "    is_success: " << capture_info.is_success << '\n';
+    str << "    index: " << capture_info.index << '\n';
+    str << "    file_url: " << capture_info.file_url << '\n';
+    str << '}';
+    return str;
+}
+
+std::ostream& operator<<(std::ostream& str, CameraServer::Result const& result)
+{
+    switch (result) {
+        case CameraServer::Result::Unknown:
+            return str << "Unknown";
+        case CameraServer::Result::Success:
+            return str << "Success";
+        case CameraServer::Result::InProgress:
+            return str << "In Progress";
+        case CameraServer::Result::Busy:
+            return str << "Busy";
+        case CameraServer::Result::Denied:
+            return str << "Denied";
+        case CameraServer::Result::Error:
+            return str << "Error";
+        case CameraServer::Result::Timeout:
+            return str << "Timeout";
+        case CameraServer::Result::WrongArgument:
+            return str << "Wrong Argument";
+        case CameraServer::Result::NoSystem:
+            return str << "No System";
+        default:
+            return str << "Unknown";
+    }
+}
+
+} // namespace mavsdk