diff --git a/CMakeLists.txt b/CMakeLists.txt index 6baad02..8eb6253 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -92,6 +92,7 @@ endif() set(MYNTEYE_SRCS ${UVC_SRC} src/internal/config.cc + src/internal/streams.cc src/internal/strings.cc src/internal/types.cc src/public/types.cc diff --git a/include/mynteye/types.h b/include/mynteye/types.h index f518f9d..89e4aaf 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -212,6 +212,8 @@ inline std::ostream &operator<<(std::ostream &os, const Format &value) { return os << to_string(value); } +std::size_t bytes_per_pixel(const Format &value); + /** * Stream request. */ @@ -224,6 +226,14 @@ struct MYNTEYE_API StreamRequest { Format format; /** frames per second */ std::uint16_t fps; + + bool operator==(const StreamRequest &other) const { + return width == other.width && height == other.height && + format == other.format && fps == other.fps; + } + bool operator!=(const StreamRequest &other) const { + return !(*this == other); + } }; /** diff --git a/src/device/device.cc b/src/device/device.cc index 637806a..a2d4d65 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -2,8 +2,12 @@ #include +#include +#include + #include "device/device_s.h" #include "internal/config.h" +#include "internal/streams.h" #include "internal/strings.h" #include "internal/types.h" #include "uvc/uvc.h" @@ -11,7 +15,11 @@ MYNTEYE_BEGIN_NAMESPACE Device::Device(const Model &model, std::shared_ptr device) - : model_(model), device_(device) { + : video_streaming_(false), + motion_tracking_(false), + model_(model), + device_(device), + streams_(nullptr) { VLOG(2) << __func__; ReadDeviceInfo(); } @@ -31,20 +39,45 @@ std::shared_ptr Device::Create( } bool Device::Supports(const Stream &stream) const { - auto &&supports = stream_supports_map.at(Model::STANDARD); + auto &&supports = stream_supports_map.at(model_); return supports.find(stream) != supports.end(); } bool Device::Supports(const Capabilities &capability) const { - auto &&supports = capabilities_supports_map.at(Model::STANDARD); + auto &&supports = capabilities_supports_map.at(model_); return supports.find(capability) != supports.end(); } bool Device::Supports(const Option &option) const { - auto &&supports = option_supports_map.at(Model::STANDARD); + auto &&supports = option_supports_map.at(model_); return supports.find(option) != supports.end(); } +const std::vector &Device::GetStreamRequests( + const Capabilities &capability) const { + if (!Supports(capability)) { + LOG(FATAL) << "Unsupported capability: " << to_string(capability); + } + try { + auto &&cap_requests = stream_requests_map.at(model_); + return cap_requests.at(capability); + } catch (const std::out_of_range &e) { + LOG(FATAL) << "Stream request of " << capability << " of " << model_ + << " not found"; + } +} + +void Device::ConfigStreamRequest( + const Capabilities &capability, const StreamRequest &request) { + auto &&requests = GetStreamRequests(capability); + if (std::find(requests.cbegin(), requests.cend(), request) == + requests.cend()) { + LOG(FATAL) << "Config stream request of " << capability + << " is not accpected"; + } + stream_config_requests_[capability] = request; +} + std::shared_ptr Device::GetInfo() const { return device_info_; } @@ -139,26 +172,86 @@ void Device::Stop(const Source &source) { } } -StreamRequest Device::GetStreamRequest(const Capabilities &capability) const { - if (!Supports(capability)) { - LOG(FATAL) << "Unsupported capability: " << to_string(capability); +const StreamRequest &Device::GetStreamRequest(const Capabilities &capability) { + try { + return stream_config_requests_[capability]; + } catch (const std::out_of_range &e) { + auto &&requests = GetStreamRequests(capability); + if (requests.size() == 1) { + VLOG(2) << "Get the only one stream request of " << capability; + return requests[0]; + } else { + LOG(FATAL) << "Please config the stream request of " << capability; + } } - auto &&requests = stream_requests_map.at(Model::STANDARD); - return requests.at(capability); } -void Device::StartVideoStreaming() {} +void Device::StartVideoStreaming() { + if (video_streaming_) { + LOG(WARNING) << "Cannot start video streaming without first stopping it"; + return; + } -void Device::StopVideoStreaming() {} + streams_ = std::make_shared(); + + // if stream capabilities are supported with subdevices of device_ + /* + Capabilities stream_capabilities[] = { + Capabilities::STEREO, + Capabilities::COLOR, + Capabilities::DEPTH, + Capabilities::POINTS, + Capabilities::FISHEYE, + Capabilities::INFRARED, + Capabilities::INFRARED2 + }; + for (auto &&capability : stream_capabilities) { + } + */ + if (Supports(Capabilities::STEREO)) { + // do stream request selection if more than one request of each stream + auto &&stream_request = GetStreamRequest(Capabilities::STEREO); + streams_->ConfigStream(Capabilities::STEREO, stream_request); + uvc::set_device_mode( + *device_, stream_request.width, stream_request.height, + static_cast(stream_request.format), stream_request.fps, + [this](const void *data) { + streams_->PushStream(Capabilities::STEREO, data); + // ... + }); + } else { + LOG(FATAL) << "Not any stream capabilities are supported by this device"; + } + + uvc::start_streaming(*device_, 0); + video_streaming_ = true; +} + +void Device::StopVideoStreaming() { + if (!video_streaming_) { + LOG(WARNING) << "Cannot stop video streaming without first starting it"; + return; + } + stop_streaming(*device_); + video_streaming_ = false; +} void Device::StartMotionTracking() { if (!Supports(Capabilities::IMU)) { - LOG(FATAL) << "IMU is not supported by this device"; + LOG(FATAL) << "IMU capability is not supported by this device"; + } + if (motion_tracking_) { + LOG(WARNING) << "Cannot start motion tracking without first stopping it"; + return; } // TODO(JohnZhao) } void Device::StopMotionTracking() { + if (!motion_tracking_) { + LOG(WARNING) << "Cannot stop motion tracking without first starting it"; + return; + } // TODO(JohnZhao) } diff --git a/src/device/device.h b/src/device/device.h index e0391cc..7b2d369 100644 --- a/src/device/device.h +++ b/src/device/device.h @@ -5,6 +5,7 @@ #include #include #include +#include #include "mynteye/mynteye.h" #include "mynteye/types.h" @@ -21,6 +22,8 @@ struct device; struct DeviceInfo; +class Streams; + class Device { public: using stream_callback_t = device::StreamCallback; @@ -42,6 +45,11 @@ class Device { bool Supports(const Capabilities &capability) const; bool Supports(const Option &option) const; + const std::vector &GetStreamRequests( + const Capabilities &capability) const; + void ConfigStreamRequest( + const Capabilities &capability, const StreamRequest &request); + std::shared_ptr GetInfo() const; std::string GetInfo(const Info &info) const; @@ -66,7 +74,7 @@ class Device { return device_info_; } - StreamRequest GetStreamRequest(const Capabilities &capability) const; + const StreamRequest &GetStreamRequest(const Capabilities &capability); virtual void StartVideoStreaming(); virtual void StopVideoStreaming(); @@ -74,6 +82,9 @@ class Device { virtual void StartMotionTracking(); virtual void StopMotionTracking(); + bool video_streaming_; + bool motion_tracking_; + private: Model model_; std::shared_ptr device_; @@ -87,6 +98,10 @@ class Device { stream_callbacks_t stream_callbacks_; motion_callback_t motion_callback_; + std::shared_ptr streams_; + + std::map stream_config_requests_; + void ReadDeviceInfo(); void WriteImgIntrinsics(const ImgIntrinsics &intrinsics); diff --git a/src/internal/callbacks.h b/src/internal/callbacks.h index ec973f3..694eab2 100644 --- a/src/internal/callbacks.h +++ b/src/internal/callbacks.h @@ -2,7 +2,10 @@ #define MYNTEYE_INTERNAL_CALLBACKS_H_ #pragma once +#include #include +#include +#include #include "mynteye/mynteye.h" #include "mynteye/types.h" @@ -11,8 +14,48 @@ MYNTEYE_BEGIN_NAMESPACE namespace device { +class Frame { + public: + using data_t = std::vector; + + Frame(const StreamRequest &request, const void *data) + : Frame(request.width, request.height, request.format, data) {} + + Frame( + std::uint16_t width, std::uint16_t height, Format format, + const void *data) + : width_(width), height_(height), format_(format) { + const std::uint8_t *bytes = static_cast(data); + data_ = data_t(bytes, bytes + (width * height) * bytes_per_pixel(format)); + } + + std::uint16_t width() const { + return width_; + } + + std::uint16_t height() const { + return height_; + } + + Format format() const { + return format_; + } + + const data_t &data() const { + return data_; + } + + private: + std::uint16_t width_; + std::uint16_t height_; + Format format_; + + data_t data_; +}; + struct MYNTEYE_API StreamData { ImgData img; + std::shared_ptr frame; }; struct MYNTEYE_API MotionData { diff --git a/src/internal/config.cc b/src/internal/config.cc index 061b97b..8946b3c 100644 --- a/src/internal/config.cc +++ b/src/internal/config.cc @@ -15,7 +15,9 @@ const std::map option_supports_map = { Option::MAX_EXPOSURE_TIME, Option::DESIRED_BRIGHTNESS, Option::IR_CONTROL, Option::HDR_MODE, Option::ZERO_DRIFT_CALIBRATION, Option::ERASE_CHIP}}}; -const std::map stream_requests_map = { - {Model::STANDARD, {{Capabilities::STEREO, {752, 480, Format::YUYV, 25}}}}}; +const std::map> + stream_requests_map = { + {Model::STANDARD, + {{Capabilities::STEREO, {{752, 480, Format::YUYV, 25}}}}}}; MYNTEYE_END_NAMESPACE diff --git a/src/internal/config.h b/src/internal/config.h index d4febe9..e0441f1 100644 --- a/src/internal/config.h +++ b/src/internal/config.h @@ -19,9 +19,10 @@ extern const std::map stream_supports_map; extern const std::map capabilities_supports_map; extern const std::map option_supports_map; -using StreamRequests = std::map; +using StreamRequests = std::vector; -extern const std::map stream_requests_map; +extern const std::map> + stream_requests_map; MYNTEYE_END_NAMESPACE diff --git a/src/internal/streams.cc b/src/internal/streams.cc new file mode 100644 index 0000000..8949c89 --- /dev/null +++ b/src/internal/streams.cc @@ -0,0 +1,63 @@ +#include "internal/streams.h" + +#include + +#include + +MYNTEYE_BEGIN_NAMESPACE + +Streams::Streams() + : stream_capabilities_( + {Capabilities::STEREO, Capabilities::COLOR, Capabilities::DEPTH, + Capabilities::POINTS, Capabilities::FISHEYE, Capabilities::INFRARED, + Capabilities::INFRARED2}) { + VLOG(2) << __func__; +} + +Streams::~Streams() { + VLOG(2) << __func__; +} + +void Streams::ConfigStream( + const Capabilities &capability, const StreamRequest &request) { + if (!IsStreamCapability(capability)) { + LOG(FATAL) << "Cannot config stream without stream capability"; + } + stream_config_requests_[capability] = request; +} + +void Streams::PushStream(const Capabilities &capability, const void *data) { + if (!HasStreamConfigRequest(capability)) { + LOG(FATAL) << "Cannot push stream without stream config request"; + } + auto frame = + std::make_shared(GetStreamConfigRequest(capability), data); + // stream_datas_map_[Stream::LEFT]; + // stream_datas_map_[Stream::RIGHT]; +} + +bool Streams::IsStreamCapability(const Capabilities &capability) const { + return std::find( + stream_capabilities_.begin(), stream_capabilities_.end(), + capability) != stream_capabilities_.end(); +} + +bool Streams::HasStreamConfigRequest(const Capabilities &capability) const { + return stream_config_requests_.find(capability) != + stream_config_requests_.end(); +} + +const StreamRequest &Streams::GetStreamConfigRequest( + const Capabilities &capability) const { + return stream_config_requests_.at(capability); +} + +bool Streams::HasStreamDatas(const Stream &stream) const { + return stream_datas_map_.find(stream) != stream_datas_map_.end(); +} + +Streams::stream_datas_t &Streams::GetStreamDatas(const Stream &stream) { + return stream_datas_map_[stream]; +} + +MYNTEYE_END_NAMESPACE diff --git a/src/internal/streams.h b/src/internal/streams.h new file mode 100644 index 0000000..aa14c67 --- /dev/null +++ b/src/internal/streams.h @@ -0,0 +1,60 @@ +#ifndef MYNTEYE_INTERNAL_STREAMS_H_ // NOLINT +#define MYNTEYE_INTERNAL_STREAMS_H_ +#pragma once + +#include +#include + +#include "mynteye/mynteye.h" +#include "mynteye/types.h" + +#include "internal/callbacks.h" + +MYNTEYE_BEGIN_NAMESPACE + +namespace streams { + +class Stream { + public: +}; + +} // namesapce streams + +class Streams { + public: + using frame_t = device::Frame; + using stream_data_t = device::StreamData; + using stream_datas_t = std::vector; + + Streams(); + ~Streams(); + + void ConfigStream( + const Capabilities &capability, const StreamRequest &request); + + void PushStream(const Capabilities &capability, const void *data); + + // void WaitForStreams() const; + + // std::vector GetStreamData(const Stream &stream) const; + // StreamData GetLatestStreamData(const Stream &stream) const; + + private: + bool IsStreamCapability(const Capabilities &capability) const; + bool HasStreamConfigRequest(const Capabilities &capability) const; + + const StreamRequest &GetStreamConfigRequest( + const Capabilities &capability) const; + + bool HasStreamDatas(const Stream &stream) const; + stream_datas_t &GetStreamDatas(const Stream &stream); + + std::vector stream_capabilities_; + std::map stream_config_requests_; + + std::map stream_datas_map_; +}; + +MYNTEYE_END_NAMESPACE + +#endif // MYNTEYE_INTERNAL_STREAMS_H_ NOLINT diff --git a/src/public/types.cc b/src/public/types.cc index ef10981..edd3755 100644 --- a/src/public/types.cc +++ b/src/public/types.cc @@ -128,4 +128,13 @@ const char *to_string(const Format &value) { #undef CASE } +std::size_t bytes_per_pixel(const Format &value) { + switch (value) { + case Format::YUYV: + return 2; + default: + LOG(FATAL) << "Unknown format"; + } +} + MYNTEYE_END_NAMESPACE