tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Complete device & streams

Browse Source
This commit is contained in:
John Zhao
2018-04-08 22:35:49 +08:00
parent 81caf45dba
commit cfcb1ca531
15 changed files with 356 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
src/internal/callbacks.h
View File

@@ -3,6 +3,8 @@
#pragma once
#include <cstdint>
#include <algorithm>
#include <functional>
#include <memory>
#include <vector>
@@ -25,8 +27,13 @@ class 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<const std::uint8_t *>(data);
data_ = data_t(bytes, bytes + (width * height) * bytes_per_pixel(format));
std::size_t bytes_n = (width * height) * bytes_per_pixel(format);
if (data) {
const std::uint8_t *bytes = static_cast<const std::uint8_t *>(data);
data_ = data_t(bytes, bytes + bytes_n);
} else {
data_.assign(bytes_n, 0);
}
}
std::uint16_t width() const {
@@ -41,8 +48,22 @@ class Frame {
return format_;
}
const data_t &data() const {
return data_;
std::uint8_t *data() {
return data_.data();
}
const std::uint8_t *data() const {
return data_.data();
}
std::size_t size() const {
return data_.size();
}
Frame clone() const {
Frame frame(width_, height_, format_, nullptr);
std::copy(data_.begin(), data_.end(), frame.data_.begin());
return frame;
}
private:
@@ -54,12 +75,12 @@ class Frame {
};
struct MYNTEYE_API StreamData {
ImgData img;
std::shared_ptr<ImgData> img;
std::shared_ptr<Frame> frame;
};
struct MYNTEYE_API MotionData {
ImuData imu;
std::shared_ptr<ImuData> imu;
};
using StreamCallback = std::function<void(const StreamData &data)>;

135
src/internal/streams.cc
View File

@@ -3,14 +3,57 @@
#include <glog/logging.h>
#include <algorithm>
#include <chrono>
MYNTEYE_BEGIN_NAMESPACE
Streams::Streams()
: stream_capabilities_(
namespace {
void unpack_stereo_img_data(
const void *data, const StreamRequest &request, ImgData &img) { // NOLINT
UNUSED(data)
UNUSED(request)
UNUSED(img)
}
void unpack_left_img_pixels(
const void *data, const StreamRequest &request,
Streams::frame_t &frame) { // NOLINT
CHECK_EQ(request.format, Format::YUYV);
CHECK_EQ(frame.format(), Format::GREY);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t n = frame.width() * frame.height();
for (std::size_t i = 0; i < n; i++) {
frame.data()[i] = *(data_new + (i * 2));
}
}
void unpack_right_img_pixels(
const void *data, const StreamRequest &request,
Streams::frame_t &frame) { // NOLINT
CHECK_EQ(request.format, Format::YUYV);
CHECK_EQ(frame.format(), Format::GREY);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t n = frame.width() * frame.height();
for (std::size_t i = 0; i < n; i++) {
frame.data()[i] = *(data_new + (i * 2 + 1));
}
}
} // namespace
Streams::Streams(const std::vector<Stream> key_streams)
: key_streams_(key_streams),
stream_capabilities_(
{Capabilities::STEREO, Capabilities::COLOR, Capabilities::DEPTH,
Capabilities::POINTS, Capabilities::FISHEYE, Capabilities::INFRARED,
Capabilities::INFRARED2}) {
Capabilities::INFRARED2}),
unpack_img_data_map_(
{{Stream::LEFT, unpack_stereo_img_data},
{Stream::RIGHT, unpack_stereo_img_data}}),
unpack_img_pixels_map_(
{{Stream::LEFT, unpack_left_img_pixels},
{Stream::RIGHT, unpack_right_img_pixels}}) {
VLOG(2) << __func__;
}
@@ -23,6 +66,7 @@ void Streams::ConfigStream(
if (!IsStreamCapability(capability)) {
LOG(FATAL) << "Cannot config stream without stream capability";
}
VLOG(2) << "Config stream request of " << capability << ", " << request;
stream_config_requests_[capability] = request;
}
@@ -30,10 +74,56 @@ 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<frame_t>(GetStreamConfigRequest(capability), data);
// stream_datas_map_[Stream::LEFT];
// stream_datas_map_[Stream::RIGHT];
std::unique_lock<std::mutex> lock(mtx_);
auto &&request = GetStreamConfigRequest(capability);
switch (capability) {
case Capabilities::STEREO: {
// alloc
AllocStreamData(Stream::LEFT, request, Format::GREY);
AllocStreamData(Stream::RIGHT, request, Format::GREY);
auto &&left_data = stream_datas_map_[Stream::LEFT].back();
auto &&right_data = stream_datas_map_[Stream::RIGHT].back();
// unpack img data
unpack_img_data_map_[Stream::LEFT](data, request, *left_data.img);
right_data.img = left_data.img;
// unpack frame
unpack_img_pixels_map_[Stream::LEFT](data, request, *left_data.frame);
unpack_img_pixels_map_[Stream::RIGHT](data, request, *right_data.frame);
} break;
default:
LOG(FATAL) << "Not supported " << capability << " now";
}
if (HasKeyStreamDatas())
cv_.notify_one();
}
void Streams::WaitForStreams() {
std::unique_lock<std::mutex> lock(mtx_);
auto ready = std::bind(&Streams::HasKeyStreamDatas, this);
if (!ready() && !cv_.wait_for(lock, std::chrono::seconds(2), ready)) {
LOG(FATAL) << "Timeout waiting for key frames";
}
}
Streams::stream_datas_t Streams::GetStreamDatas(const Stream &stream) {
if (!HasStreamDatas(stream) || stream_datas_map_.at(stream).empty()) {
LOG(WARNING) << "There are stream datas of " << stream
<< ", do you first call WaitForStreams?";
return {};
}
std::unique_lock<std::mutex> lock(mtx_);
stream_datas_t datas = stream_datas_map_.at(stream);
stream_datas_map_[stream].clear();
return datas;
}
Streams::stream_data_t Streams::GetLatestStreamData(const Stream &stream) {
return GetStreamDatas(stream).back();
}
const Streams::stream_datas_t &Streams::stream_datas(
const Stream &stream) const {
return stream_datas_map_.at(stream);
}
bool Streams::IsStreamCapability(const Capabilities &capability) const {
@@ -56,8 +146,35 @@ 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];
void Streams::AllocStreamData(
const Stream &stream, const StreamRequest &request) {
AllocStreamData(stream, request, request.format);
}
void Streams::AllocStreamData(
const Stream &stream, const StreamRequest &request, const Format &format) {
static std::size_t stream_data_limits_max = 4;
stream_data_t data;
data.img = std::shared_ptr<ImgData>(new ImgData{0, 0, 0});
data.frame =
std::make_shared<frame_t>(request.width, request.height, format, nullptr);
stream_datas_map_[stream].push_back(data);
// If cached more then limits_max, drop the oldest one.
if (stream_datas_map_.at(stream).size() > stream_data_limits_max) {
auto &&datas = stream_datas_map_[stream];
datas.erase(datas.begin());
VLOG(2) << "Stream data of " << stream << " is dropped";
}
}
bool Streams::HasKeyStreamDatas() const {
for (auto &&s : key_streams_) {
if (!HasStreamDatas(s))
return false;
if (stream_datas_map_.at(s).empty())
return false;
}
return true;
}
MYNTEYE_END_NAMESPACE

42
src/internal/streams.h
View File

@@ -2,7 +2,10 @@
#define MYNTEYE_INTERNAL_STREAMS_H_
#pragma once
#include <condition_variable>
#include <functional>
#include <map>
#include <mutex>
#include <vector>
#include "mynteye/mynteye.h"
@@ -12,21 +15,19 @@
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<stream_data_t>;
Streams();
using unpack_img_data_t = std::function<void(
const void *data, const StreamRequest &request, ImgData &img)>; // NOLINT
using unpack_img_pixels_t = std::function<void(
const void *data, const StreamRequest &request,
frame_t &frame)>; // NOLINT
explicit Streams(const std::vector<Stream> key_streams);
~Streams();
void ConfigStream(
@@ -34,10 +35,12 @@ class Streams {
void PushStream(const Capabilities &capability, const void *data);
// void WaitForStreams() const;
void WaitForStreams();
// std::vector<StreamData> GetStreamData(const Stream &stream) const;
// StreamData GetLatestStreamData(const Stream &stream) const;
stream_datas_t GetStreamDatas(const Stream &stream);
stream_data_t GetLatestStreamData(const Stream &stream);
const stream_datas_t &stream_datas(const Stream &stream) const;
private:
bool IsStreamCapability(const Capabilities &capability) const;
@@ -47,12 +50,25 @@ class Streams {
const Capabilities &capability) const;
bool HasStreamDatas(const Stream &stream) const;
stream_datas_t &GetStreamDatas(const Stream &stream);
void AllocStreamData(const Stream &stream, const StreamRequest &request);
void AllocStreamData(
const Stream &stream, const StreamRequest &request, const Format &format);
bool HasKeyStreamDatas() const;
std::vector<Stream> key_streams_;
std::vector<Capabilities> stream_capabilities_;
std::map<Capabilities, StreamRequest> stream_config_requests_;
std::map<Stream, unpack_img_data_t> unpack_img_data_map_;
std::map<Stream, unpack_img_pixels_t> unpack_img_pixels_map_;
std::map<Stream, stream_datas_t> stream_datas_map_;
std::mutex mtx_;
std::condition_variable cv_;
};
MYNTEYE_END_NAMESPACE

3
src/internal/types.h
View File

@@ -32,6 +32,7 @@ class Version {
using size_t = std::size_t;
using value_t = std::uint8_t;
Version() = default;
Version(value_t major, value_t minor) : major_(major), minor_(minor) {}
explicit Version(const std::string &name)
: major_(parse_part(name, 0)), minor_(parse_part(name, 1)) {}
@@ -79,6 +80,7 @@ class HardwareVersion : public Version {
public:
using flag_t = std::bitset<8>;
HardwareVersion() = default;
HardwareVersion(value_t major, value_t minor, value_t flag = 0)
: Version(major, minor), flag_(flag) {}
explicit HardwareVersion(const std::string &name, value_t flag = 0)
@@ -95,6 +97,7 @@ class Type {
using size_t = std::size_t;
using value_t = std::uint16_t;
Type() = default;
Type(value_t vendor, value_t product) : vendor_(vendor), product_(product) {}
explicit Type(const std::string &name)
: vendor_(parse_part(name, 0, 2)), product_(parse_part(name, 2, 2)) {}