Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

camera: update mavlink and remove stream settings #756

Merged
merged 2 commits into from
Jun 12, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/backend/proto
Submodule proto updated from 56fb54 to 3385c8
14 changes: 0 additions & 14 deletions src/backend/src/plugins/camera/camera_service_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -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<rpc::camera::VideoStreamSettings> translateVideoStreamSettings(
const dronecode_sdk::Camera::VideoStreamSettings video_stream_settings)
{
Expand Down
51 changes: 0 additions & 51 deletions src/backend/test/camera_service_impl_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<dronecode_sdk::rpc::camera::VideoStreamSettings>
CameraServiceImplTest::createArbitraryRPCVideoStreamSettings() const
{
Expand All @@ -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();
Expand Down Expand Up @@ -572,30 +545,6 @@ void CameraServiceImplTest::checkSendsVideoStreamInfo(
}
}

TEST_F(CameraServiceImplTest, sendsMultipleVideoStreamInfos)
{
std::vector<dronecode_sdk::Camera::VideoStreamInfo> 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();
Expand Down
5 changes: 0 additions & 5 deletions src/plugins/camera/camera.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
36 changes: 4 additions & 32 deletions src/plugins/camera/camera_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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{};
Expand Down Expand Up @@ -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<std::mutex> lock(_video_stream_info.mutex);
Expand Down Expand Up @@ -953,8 +922,11 @@ void CameraImpl::process_video_information(const mavlink_message_t &message)

{
std::lock_guard<std::mutex> lock(_video_stream_info.mutex);
// TODO: use stream_id and count
_video_stream_info.info.status =
static_cast<Camera::VideoStreamInfo::Status>(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;
Expand Down
4 changes: 0 additions & 4 deletions src/plugins/camera/camera_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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<CameraDefinition> _camera_definition{};
Expand Down
33 changes: 1 addition & 32 deletions src/plugins/camera/include/plugins/camera/camera.h
Original file line number Diff line number Diff line change
Expand Up @@ -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. */
Expand All @@ -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.
*/
Expand Down
2 changes: 1 addition & 1 deletion src/third_party/mavlink/include/mavlink/v2.0
Submodule v2.0 updated 347 files