Merge pull request #2088 from tbago/add_more_camera_function

More camera server functionality

examples/camera_server/camera_client.cpp

@@ -1,10 +1,13 @@
 #include <chrono>
 #include <future>
 #include <iostream>
+#include <thread>
 
 #include <mavsdk/mavsdk.h>
 #include <mavsdk/plugins/camera/camera.h>
 
+static void do_camera_operation(mavsdk::Camera& camera);
+
 int main(int argc, const char* argv[])
 {
     // we run client plugins to act as the GCS
@@ -47,8 +50,50 @@ int main(int argc, const char* argv[])
         std::cout << info << std::endl;
     });
 
-    auto take_photo_result = camera.take_photo();
-    std::cout << "Take phto return : " << take_photo_result << std::endl;
+    camera.subscribe_status([](mavsdk::Camera::Status status) {
+        std::cout << "Camera status:" << std::endl;
+        std::cout << status << std::endl;
+    });
+
+    do_camera_operation(camera);
 
+    // for test subscribe camera status, so don't end the process
+    while (true) {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
     return 0;
 }
+
+void do_camera_operation(mavsdk::Camera& camera)
+{
+    // switch to photo mode to take photo
+    auto operation_result = camera.set_mode(mavsdk::Camera::Mode::Photo);
+    std::cout << "Set camera to photo mode result : " << operation_result << std::endl;
+
+    operation_result = camera.take_photo();
+    std::cout << "Take photo result : " << operation_result << std::endl;
+
+    // switch to video mode to recording video
+    operation_result = camera.set_mode(mavsdk::Camera::Mode::Video);
+    std::cout << "Set camera to video mode result : " << operation_result << std::endl;
+
+    operation_result = camera.start_video();
+    std::cout << "Start video result : " << operation_result << std::endl;
+
+    operation_result = camera.stop_video();
+    std::cout << "Stop video result : " << operation_result << std::endl;
+
+    // the camera can have multi video stream so may need the stream id
+    operation_result = camera.start_video_streaming(2);
+    std::cout << "start video streaming result : " << operation_result << std::endl;
+
+    operation_result = camera.stop_video_streaming(2);
+    std::cout << "stop video streaming result : " << operation_result << std::endl;
+
+    // format the storage with special storage id test
+    operation_result = camera.format_storage(11);
+    std::cout << "format storage result : " << operation_result << std::endl;
+
+    operation_result = camera.reset_settings();
+    std::cout << "Reset camera settings result : " << operation_result << std::endl;
+}

examples/camera_server/camera_server.cpp

@@ -1,8 +1,12 @@
 #include <iostream>
 #include <thread>
+#include <chrono>
+#include <future>
 #include <mavsdk/mavsdk.h>
 #include <mavsdk/plugins/camera_server/camera_server.h>
 
+static void subscribe_camera_operation(mavsdk::CameraServer& camera_server);
+
 int main(int argc, char** argv)
 {
     mavsdk::Mavsdk mavsdk{mavsdk::Mavsdk::Configuration{mavsdk::Mavsdk::ComponentType::Camera}};
@@ -18,10 +22,54 @@ int main(int argc, char** argv)
     auto camera_server = mavsdk::CameraServer{mavsdk.server_component()};
 
     // First add all subscriptions. This defines the camera capabilities.
+    subscribe_camera_operation(camera_server);
+
+    // 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,
+        .lens_id = 0,
+        .definition_file_version = 0, // TODO: add this
+        .definition_file_uri = "", // TODO: implement this using MAVLink FTP
+    });
+
+    if (ret != mavsdk::CameraServer::Result::Success) {
+        std::cerr << "Failed to set camera info, exiting" << std::endl;
+        return 2;
+    }
 
+    // works as a server and never quit
+    while (true) {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+
+    return 0;
+}
+
+// sample for camera current status
+std::chrono::steady_clock::time_point start_video_time;
+bool is_recording_video = false;
+bool is_capture_in_progress = false;
+int32_t image_count = 0;
+
+static void subscribe_camera_operation(mavsdk::CameraServer& camera_server)
+{
     camera_server.subscribe_take_photo([&camera_server](int32_t index) {
         camera_server.set_in_progress(true);
 
+        is_capture_in_progress = true;
+
         std::cout << "taking a picture (" << +index << ")..." << std::endl;
 
         // TODO : actually capture image here
@@ -36,11 +84,11 @@ int main(int argc, char** argv)
                              std::chrono::system_clock::now().time_since_epoch())
                              .count();
         auto success = true;
-
+        image_count++;
         camera_server.set_in_progress(false);
 
         camera_server.respond_take_photo(
-            mavsdk::CameraServer::TakePhotoFeedback::Ok,
+            mavsdk::CameraServer::CameraFeedback::Ok,
             mavsdk::CameraServer::CaptureInfo{
                 .position = position,
                 .attitude_quaternion = attitude,
@@ -49,39 +97,82 @@ int main(int argc, char** argv)
                 .index = index,
                 .file_url = {},
             });
+
+        is_capture_in_progress = false;
     });
 
-    // Then set the initial state of everything.
+    camera_server.subscribe_start_video([&camera_server](int32_t stream_id) {
+        std::cout << "Start video record" << std::endl;
+        is_recording_video = true;
+        start_video_time = std::chrono::steady_clock::now();
+        camera_server.respond_start_video(mavsdk::CameraServer::CameraFeedback::Ok);
+    });
 
-    // TODO: this state is not guaranteed, e.g. a new system appears
-    // while a capture is in progress
-    camera_server.set_in_progress(false);
+    camera_server.subscribe_stop_video([&camera_server](int32_t stream_id) {
+        std::cout << "Stop video record" << std::endl;
+        is_recording_video = false;
+        camera_server.respond_stop_video(mavsdk::CameraServer::CameraFeedback::Ok);
+    });
 
-    // Finally call set_information() to "activate" the camera plugin.
+    camera_server.subscribe_start_video_streaming([&camera_server](int32_t stream_id) {
+        std::cout << "Start video streaming " << stream_id << std::endl;
+        camera_server.respond_start_video_streaming(mavsdk::CameraServer::CameraFeedback::Ok);
+    });
 
-    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,
-        .lens_id = 0,
-        .definition_file_version = 0, // TODO: add this
-        .definition_file_uri = "", // TODO: implement this using MAVLink FTP
+    camera_server.subscribe_stop_video_streaming([&camera_server](int32_t stream_id) {
+        std::cout << "Stop video streaming " << stream_id << std::endl;
+        camera_server.respond_stop_video_streaming(mavsdk::CameraServer::CameraFeedback::Ok);
     });
 
-    if (ret != mavsdk::CameraServer::Result::Success) {
-        std::cerr << "Failed to set camera info, exiting" << std::endl;
-        return 2;
-    }
+    camera_server.subscribe_set_mode([&camera_server](mavsdk::CameraServer::Mode mode) {
+        std::cout << "Set camera mode " << mode << std::endl;
+        camera_server.respond_set_mode(mavsdk::CameraServer::CameraFeedback::Ok);
+    });
 
-    // works as a server and never quit
-    while (true) {
-        std::this_thread::sleep_for(std::chrono::seconds(1));
-    }
+    camera_server.subscribe_storage_information([&camera_server](int32_t storage_id) {
+        mavsdk::CameraServer::StorageInformation storage_information;
+        constexpr int kTotalStorage = 4 * 1024 * 1024;
+        storage_information.total_storage_mib = kTotalStorage;
+        storage_information.used_storage_mib = 100;
+        storage_information.available_storage_mib =
+            kTotalStorage - storage_information.used_storage_mib;
+        storage_information.storage_status =
+            mavsdk::CameraServer::StorageInformation::StorageStatus::Formatted;
+        storage_information.storage_type =
+            mavsdk::CameraServer::StorageInformation::StorageType::Microsd;
+
+        camera_server.respond_storage_information(
+            mavsdk::CameraServer::CameraFeedback::Ok, storage_information);
+    });
 
-    return 0;
-}
+    camera_server.subscribe_capture_status([&camera_server](int32_t reserved) {
+        mavsdk::CameraServer::CaptureStatus capture_status;
+        capture_status.image_count = image_count;
+        capture_status.image_status =
+            is_capture_in_progress ?
+                mavsdk::CameraServer::CaptureStatus::ImageStatus::CaptureInProgress :
+                mavsdk::CameraServer::CaptureStatus::ImageStatus::Idle;
+        capture_status.video_status =
+            is_recording_video ?
+                mavsdk::CameraServer::CaptureStatus::VideoStatus::CaptureInProgress :
+                mavsdk::CameraServer::CaptureStatus::VideoStatus::Idle;
+        auto current_time = std::chrono::steady_clock::now();
+        if (is_recording_video) {
+            capture_status.recording_time_s =
+                std::chrono::duration_cast<std::chrono::seconds>(current_time - start_video_time)
+                    .count();
+        }
+        camera_server.respond_capture_status(
+            mavsdk::CameraServer::CameraFeedback::Ok, capture_status);
+    });
+
+    camera_server.subscribe_format_storage([&camera_server](int storage_id) {
+        std::cout << "format storage with id : " << storage_id << std::endl;
+        camera_server.respond_format_storage(mavsdk::CameraServer::CameraFeedback::Ok);
+    });
+
+    camera_server.subscribe_reset_settings([&camera_server](int camera_id) {
+        std::cout << "reset camera settings" << std::endl;
+        camera_server.respond_reset_settings(mavsdk::CameraServer::CameraFeedback::Ok);
+    });
+}

src/integration_tests/CMakeLists.txt

@@ -18,6 +18,7 @@ add_executable(integration_tests_runner
     camera_take_photo.cpp
     camera_take_photo_interval.cpp
     camera_format.cpp
+    camera_reset_settings.cpp
     camera_test_helpers.cpp
     connection.cpp
     follow_me.cpp

src/integration_tests/camera_format.cpp

@@ -22,5 +22,5 @@ TEST(CameraTest, Format)
 
     auto camera = std::make_shared<Camera>(system);
 
-    EXPECT_EQ(Camera::Result::Success, camera->format_storage());
+    EXPECT_EQ(Camera::Result::Success, camera->format_storage(1));
 }

src/integration_tests/camera_reset_settings.cpp

@@ -0,0 +1,26 @@
+#include "integration_test_helper.h"
+#include "mavsdk.h"
+#include <iostream>
+#include <functional>
+#include <atomic>
+#include "plugins/camera/camera.h"
+
+using namespace mavsdk;
+
+TEST(CameraTest, ResetSettings)
+{
+    Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}};
+
+    ConnectionResult ret = mavsdk.add_udp_connection();
+    ASSERT_EQ(ret, ConnectionResult::Success);
+
+    // Wait for system to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    auto system = mavsdk.systems().at(0);
+    ASSERT_TRUE(system->has_camera());
+
+    auto camera = std::make_shared<Camera>(system);
+
+    EXPECT_EQ(Camera::Result::Success, camera->reset_settings());
+}

src/integration_tests/camera_take_photo.cpp

@@ -25,7 +25,7 @@ TEST(CameraTest, TakePhotoSingle)
         info.index = index;
         info.is_success = true;
 
-        camera_server.respond_take_photo(CameraServer::TakePhotoFeedback::Ok, info);
+        camera_server.respond_take_photo(CameraServer::CameraFeedback::Ok, info);
     });
 
     // Wait for system to connect via heartbeat.

src/mavsdk/core/mavlink_mission_transfer_client.cpp

@@ -275,7 +275,8 @@ void MavlinkMissionTransferClient::UploadWorkItem::send_count()
                 _target_system_id,
                 MAV_COMP_ID_AUTOPILOT1,
                 _items.size(),
-                _type);
+                _type,
+                0);
             return message;
         })) {
         _timeout_handler.remove(_cookie);
@@ -303,7 +304,8 @@ void MavlinkMissionTransferClient::UploadWorkItem::send_cancel_and_finish()
                 _target_system_id,
                 MAV_COMP_ID_AUTOPILOT1,
                 MAV_MISSION_OPERATION_CANCELLED,
-                _type);
+                _type,
+                0);
             return message;
         })) {
         callback_and_reset(Result::ConnectionError);
@@ -350,7 +352,8 @@ void MavlinkMissionTransferClient::UploadWorkItem::process_mission_request(
                     request_message.sysid,
                     request_message.compid,
                     MAV_MISSION_UNSUPPORTED,
-                    _type);
+                    _type,
+                    0);
                 return message;
             })) {
             _timeout_handler.remove(_cookie);
@@ -670,7 +673,8 @@ void MavlinkMissionTransferClient::DownloadWorkItem::send_ack_and_finish()
                 _target_system_id,
                 MAV_COMP_ID_AUTOPILOT1,
                 MAV_MISSION_ACCEPTED,
-                _type);
+                _type,
+                0);
             return message;
         })) {
         callback_and_reset(Result::ConnectionError);
@@ -693,7 +697,8 @@ void MavlinkMissionTransferClient::DownloadWorkItem::send_cancel_and_finish()
                 _target_system_id,
                 MAV_COMP_ID_AUTOPILOT1,
                 MAV_MISSION_OPERATION_CANCELLED,
-                _type);
+                _type,
+                0);
             return message;
         })) {
         callback_and_reset(Result::ConnectionError);

src/mavsdk/core/mavlink_mission_transfer_client_test.cpp

@@ -83,7 +83,8 @@ mavlink_message_t make_mission_count(unsigned count)
         target_address.system_id,
         target_address.component_id,
         count,
-        MAV_MISSION_TYPE_MISSION);
+        MAV_MISSION_TYPE_MISSION,
+        0);
     return message;
 }
 
@@ -419,7 +420,8 @@ mavlink_message_t make_mission_ack(uint8_t type, uint8_t result)
         target_address.system_id,
         target_address.component_id,
         result,
-        type);
+        type,
+        0);
     return message;
 }
 
@@ -1695,7 +1697,7 @@ mavlink_message_t make_mission_current(uint16_t seq)
 {
     mavlink_message_t message;
     mavlink_msg_mission_current_pack(
-        own_address.system_id, own_address.component_id, &message, seq, 0, 0, 0);
+        own_address.system_id, own_address.component_id, &message, seq, 0, 0, 0, 0, 0, 0);
     return message;
 }