From 3e320b4ca455e2bb401e1f23f56b7b226d86c0d1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 4 Jun 2019 15:59:03 +0200 Subject: [PATCH 1/2] camera: update mavlink and remove stream settings It turns out we can no longer set streams using MAVLink but we can only receive the information. --- src/plugins/camera/camera.cpp | 5 --- src/plugins/camera/camera_impl.cpp | 36 +++---------------- src/plugins/camera/camera_impl.h | 4 --- .../camera/include/plugins/camera/camera.h | 33 +---------------- src/third_party/mavlink/include/mavlink/v2.0 | 2 +- 5 files changed, 6 insertions(+), 74 deletions(-) diff --git a/src/plugins/camera/camera.cpp b/src/plugins/camera/camera.cpp index a1482b1dae..642edbc37f 100644 --- a/src/plugins/camera/camera.cpp +++ b/src/plugins/camera/camera.cpp @@ -72,11 +72,6 @@ Camera::Result Camera::stop_video_streaming() return _impl->stop_video_streaming(); } -void Camera::set_video_stream_settings(const VideoStreamSettings &settings) -{ - return _impl->set_video_stream_settings(settings); -} - Camera::Result Camera::get_video_stream_info(VideoStreamInfo &info) { return _impl->get_video_stream_info(info); diff --git a/src/plugins/camera/camera_impl.cpp b/src/plugins/camera/camera_impl.cpp index f36eabe4e5..d272b8ba85 100644 --- a/src/plugins/camera/camera_impl.cpp +++ b/src/plugins/camera/camera_impl.cpp @@ -307,28 +307,6 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_stop_video_streaming() return cmd_stop_video_streaming; } -mavlink_message_t -CameraImpl::make_message_set_video_stream_settings(const Camera::VideoStreamSettings &settings) -{ - mavlink_message_t msg; - - mavlink_msg_set_video_stream_settings_pack(_parent->get_own_system_id(), - _parent->get_own_component_id(), - &msg, - _parent->get_system_id(), - _camera_id + MAV_COMP_ID_CAMERA, - _camera_id + - MAV_COMP_ID_CAMERA, // TODO: Is it right ? - settings.frame_rate_hz, - settings.horizontal_resolution_pix, - settings.vertical_resolution_pix, - settings.bit_rate_b_s, - settings.rotation_deg, - settings.uri.c_str()); - - return msg; -} - MAVLinkCommands::CommandLong CameraImpl::make_command_request_video_stream_info() { MAVLinkCommands::CommandLong cmd_req_video_stream_info{}; @@ -465,15 +443,6 @@ Camera::Information CameraImpl::get_information() return _information.data; } -void CameraImpl::set_video_stream_settings(const Camera::VideoStreamSettings &settings) -{ - auto msg = make_message_set_video_stream_settings(settings); - - if (!_parent->send_message(msg)) { - LogErr() << "Failed to set Video stream settings"; - } -} - Camera::Result CameraImpl::start_video_streaming() { std::lock_guard lock(_video_stream_info.mutex); @@ -953,8 +922,11 @@ void CameraImpl::process_video_information(const mavlink_message_t &message) { std::lock_guard lock(_video_stream_info.mutex); + // TODO: use stream_id and count _video_stream_info.info.status = - static_cast(received_video_info.status); + (received_video_info.flags & VIDEO_STREAM_STATUS_FLAGS_RUNNING ? + Camera::VideoStreamInfo::Status::IN_PROGRESS : + Camera::VideoStreamInfo::Status::NOT_RUNNING); auto &video_stream_info = _video_stream_info.info.settings; video_stream_info.frame_rate_hz = received_video_info.framerate; video_stream_info.horizontal_resolution_pix = received_video_info.resolution_h; diff --git a/src/plugins/camera/camera_impl.h b/src/plugins/camera/camera_impl.h index f13f8bff46..4db44335a6 100644 --- a/src/plugins/camera/camera_impl.h +++ b/src/plugins/camera/camera_impl.h @@ -37,7 +37,6 @@ class CameraImpl : public PluginImplBase { Camera::Information get_information(); - void set_video_stream_settings(const Camera::VideoStreamSettings &settings); Camera::Result get_video_stream_info(Camera::VideoStreamInfo &info); void get_video_stream_info_async(const Camera::get_video_stream_info_callback_t callback); void subscribe_video_stream_info(const Camera::subscribe_video_stream_info_callback_t callback); @@ -156,9 +155,6 @@ class CameraImpl : public PluginImplBase { MAVLinkCommands::CommandLong make_command_start_video_streaming(); MAVLinkCommands::CommandLong make_command_stop_video_streaming(); - mavlink_message_t - make_message_set_video_stream_settings(const Camera::VideoStreamSettings &settings); - MAVLinkCommands::CommandLong make_command_request_video_stream_info(); std::unique_ptr _camera_definition{}; diff --git a/src/plugins/camera/include/plugins/camera/camera.h b/src/plugins/camera/include/plugins/camera/camera.h index fc034e4048..abd39a2246 100644 --- a/src/plugins/camera/include/plugins/camera/camera.h +++ b/src/plugins/camera/include/plugins/camera/camera.h @@ -306,10 +306,8 @@ class Camera : public PluginBase { /** * @brief Type for video stream settings. * - * Application may call set_video_stream_settings() before starting - * video streaming to tell Camera server to use these settings during video streaming. * - * @sa set_video_stream_settings(), get_video_stream_info(), start_video_streaming(). + * @sa get_video_stream_info(), start_video_streaming(). */ struct VideoStreamSettings { float frame_rate_hz = 0.f; /**< @brief Frames per second. */ @@ -318,37 +316,8 @@ class Camera : public PluginBase { uint32_t bit_rate_b_s = 0u; /**< @brief Bit rate in bits per second. */ uint16_t rotation_deg = 0u; /**< @brief Video image rotation clockwise (0-359 degrees). */ std::string uri{}; /**< @brief Video stream URI. */ - - /** - * @brief Sets to highest possible settings for Resolution, framerate. - */ - void set_highest() - { - frame_rate_hz = FRAME_RATE_HIGHEST; - horizontal_resolution_pix = RESOLUTION_H_HIGHEST; - vertical_resolution_pix = RESOLUTION_V_HIGHEST; - // FIXME: Should bit_rate_b_s be part of set_highest() ? - bit_rate_b_s = BIT_RATE_AUTO; - } - - constexpr static const float FRAME_RATE_HIGHEST = - -1.0f; /**< @brief Highest possible framerate. */ - constexpr static const uint16_t RESOLUTION_H_HIGHEST = - UINT16_MAX; /**< @brief Highest possible horizontal resolution. */ - constexpr static const uint16_t RESOLUTION_V_HIGHEST = - UINT16_MAX; /**< @brief Highest possible vertical resolution. */ - constexpr static const uint32_t BIT_RATE_AUTO = - 0; /**< @brief Auto settings for Bit rate. */ }; - /** - * @brief Sets video stream settings. - * - * @param settings Reference to custom video stream settings which include resolution, - * framerate, bitrate, etc. - */ - void set_video_stream_settings(const VideoStreamSettings &settings); - /** * @brief Information about video stream. */ diff --git a/src/third_party/mavlink/include/mavlink/v2.0 b/src/third_party/mavlink/include/mavlink/v2.0 index fb2c4f8198..a66f06dd30 160000 --- a/src/third_party/mavlink/include/mavlink/v2.0 +++ b/src/third_party/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit fb2c4f819819e2348285bc9e03ab711ceb6c4cc1 +Subproject commit a66f06dd300338aef7da306b403941bd9c0dd3a8 From 261fb54c9003844e07e4019fe0eb273874a72ace Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Jun 2019 10:18:12 +0200 Subject: [PATCH 2/2] backend: remove set video stream setting This functionality was removed from MAVLink. --- src/backend/proto | 2 +- .../src/plugins/camera/camera_service_impl.h | 14 ----- src/backend/test/camera_service_impl_test.cpp | 51 ------------------- 3 files changed, 1 insertion(+), 66 deletions(-) diff --git a/src/backend/proto b/src/backend/proto index 56fb54e3fe..3385c8b9b4 160000 --- a/src/backend/proto +++ b/src/backend/proto @@ -1 +1 @@ -Subproject commit 56fb54e3fe8bd0c076bce72168bee05f397ecbd1 +Subproject commit 3385c8b9b411bfe47df95c7c9e10bb7930ee5298 diff --git a/src/backend/src/plugins/camera/camera_service_impl.h b/src/backend/src/plugins/camera/camera_service_impl.h index fd0b5e9c98..3c48a20413 100644 --- a/src/backend/src/plugins/camera/camera_service_impl.h +++ b/src/backend/src/plugins/camera/camera_service_impl.h @@ -188,20 +188,6 @@ class CameraServiceImpl final : public rpc::camera::CameraService::Service { return grpc::Status::OK; } - grpc::Status - SetVideoStreamSettings(grpc::ServerContext * /* context */, - const rpc::camera::SetVideoStreamSettingsRequest *request, - rpc::camera::SetVideoStreamSettingsResponse * /* response */) override - { - if (request != nullptr) { - const auto video_stream_settings = - translateRPCVideoStreamSettings(request->video_stream_settings()); - _camera.set_video_stream_settings(video_stream_settings); - } - - return grpc::Status::OK; - } - static std::unique_ptr translateVideoStreamSettings( const dronecode_sdk::Camera::VideoStreamSettings video_stream_settings) { diff --git a/src/backend/test/camera_service_impl_test.cpp b/src/backend/test/camera_service_impl_test.cpp index 5d2d1a6622..05f7a54feb 100644 --- a/src/backend/test/camera_service_impl_test.cpp +++ b/src/backend/test/camera_service_impl_test.cpp @@ -440,21 +440,6 @@ TEST_F(CameraServiceImplTest, sendsMultipleModes) checkSendsModes(modes); } -TEST_F(CameraServiceImplTest, setVideoStreamSettingsDoesNotFailWithAllNullParams) -{ - _camera_service.SetVideoStreamSettings(nullptr, nullptr, nullptr); -} - -TEST_F(CameraServiceImplTest, setVideoStreamSettingsDoesNotFailWithNullResponse) -{ - dronecode_sdk::rpc::camera::SetVideoStreamSettingsRequest request; - - auto rpc_settings = createArbitraryRPCVideoStreamSettings(); - request.set_allocated_video_stream_settings(rpc_settings.release()); - - _camera_service.SetVideoStreamSettings(nullptr, &request, nullptr); -} - std::unique_ptr CameraServiceImplTest::createArbitraryRPCVideoStreamSettings() const { @@ -470,18 +455,6 @@ CameraServiceImplTest::createArbitraryRPCVideoStreamSettings() const return rpc_settings; } -TEST_F(CameraServiceImplTest, setsVideoStreamSettingsCorrectly) -{ - auto rpc_video_stream_settings = createArbitraryRPCVideoStreamSettings(); - const auto expected_video_stream_settings = - CameraServiceImpl::translateRPCVideoStreamSettings(*rpc_video_stream_settings); - EXPECT_CALL(_camera, set_video_stream_settings(expected_video_stream_settings)).Times(1); - dronecode_sdk::rpc::camera::SetVideoStreamSettingsRequest request; - request.set_allocated_video_stream_settings(rpc_video_stream_settings.release()); - - _camera_service.SetVideoStreamSettings(nullptr, &request, nullptr); -} - TEST_F(CameraServiceImplTest, registersToVideoStreamInfo) { auto rpc_video_stream_info = createArbitraryRPCVideoStreamInfo(); @@ -572,30 +545,6 @@ void CameraServiceImplTest::checkSendsVideoStreamInfo( } } -TEST_F(CameraServiceImplTest, sendsMultipleVideoStreamInfos) -{ - std::vector video_info_events; - - dronecode_sdk::Camera::VideoStreamSettings settings1; - settings1.set_highest(); - video_info_events.push_back(createVideoStreamInfo( - settings1, dronecode_sdk::Camera::VideoStreamInfo::Status::NOT_RUNNING)); - - dronecode_sdk::Camera::VideoStreamSettings settings2; - video_info_events.push_back(createVideoStreamInfo( - settings2, dronecode_sdk::Camera::VideoStreamInfo::Status::IN_PROGRESS)); - - checkSendsVideoStreamInfo(video_info_events); -} - -dronecode_sdk::Camera::VideoStreamInfo CameraServiceImplTest::createVideoStreamInfo( - const dronecode_sdk::Camera::VideoStreamSettings settings, - const dronecode_sdk::Camera::VideoStreamInfo::Status status) const -{ - dronecode_sdk::Camera::VideoStreamInfo video_stream_info{settings, status}; - return video_stream_info; -} - TEST_F(CameraServiceImplTest, registersToCaptureInfo) { auto rpc_capture_info = createArbitraryRPCCaptureInfo();