diff --git a/include/mynteye/types.h b/include/mynteye/types.h index 7038ee9..b81a07f 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -345,6 +345,21 @@ struct MYNTEYE_API ImgData { timestamp = 0; exposure_time = 0; } + + ImgData() { + Reset(); + } + ImgData(const ImgData &other) { + frame_id = other.frame_id; + timestamp = other.timestamp; + exposure_time = other.exposure_time; + } + ImgData &operator=(const ImgData &other) { + frame_id = other.frame_id; + timestamp = other.timestamp; + exposure_time = other.exposure_time; + return *this; + } }; /** @@ -370,6 +385,10 @@ struct MYNTEYE_API ImuData { std::fill(gyro, gyro + 3, 0); temperature = 0; } + + ImuData() { + Reset(); + } }; MYNTEYE_END_NAMESPACE diff --git a/samples/device/camera.cc b/samples/device/camera.cc index 6f60058..c98e731 100644 --- a/samples/device/camera.cc +++ b/samples/device/camera.cc @@ -47,14 +47,22 @@ int main(int argc, char *argv[]) { Stream::LEFT, [&left_count](const device::StreamData &data) { ++left_count; VLOG(2) << Stream::LEFT << ", count: " << left_count; - UNUSED(data) + if (data.img) { + VLOG(2) << " frame_id: " << data.img->frame_id + << ", timestamp: " << data.img->timestamp + << ", exposure_time: " << data.img->exposure_time; + } }); std::size_t right_count = 0; device->SetStreamCallback( Stream::RIGHT, [&right_count](const device::StreamData &data) { ++right_count; VLOG(2) << Stream::RIGHT << ", count: " << right_count; - UNUSED(data) + if (data.img) { + VLOG(2) << " frame_id: " << data.img->frame_id + << ", timestamp: " << data.img->timestamp + << ", exposure_time: " << data.img->exposure_time; + } }); device->Start(Source::ALL); diff --git a/src/device/device.cc b/src/device/device.cc index 10b0af2..000e7c7 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -193,12 +193,14 @@ void Device::WaitForStreams() { std::vector Device::GetStreamDatas(const Stream &stream) { CHECK(video_streaming_); CHECK_NOTNULL(streams_); + std::lock_guard _(mtx_streams_); return streams_->GetStreamDatas(stream); } device::StreamData Device::GetLatestStreamData(const Stream &stream) { CHECK(video_streaming_); CHECK_NOTNULL(streams_); + std::lock_guard _(mtx_streams_); return streams_->GetLatestStreamData(stream); } @@ -247,6 +249,7 @@ void Device::StartVideoStreaming() { *device_, stream_request.width, stream_request.height, static_cast(stream_request.format), stream_request.fps, [this](const void *data) { + std::lock_guard _(mtx_streams_); streams_->PushStream(Capabilities::STEREO, data); if (HasStreamCallback(Stream::LEFT)) { auto &&stream_data = streams_->stream_datas(Stream::LEFT).back(); diff --git a/src/device/device.h b/src/device/device.h index b12438b..8d03c73 100644 --- a/src/device/device.h +++ b/src/device/device.h @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -112,6 +113,8 @@ class Device { std::map stream_config_requests_; + std::mutex mtx_streams_; + void ReadDeviceInfo(); void WriteImgIntrinsics(const ImgIntrinsics &intrinsics); diff --git a/src/internal/streams.cc b/src/internal/streams.cc index 7bf23fd..c6d94e3 100644 --- a/src/internal/streams.cc +++ b/src/internal/streams.cc @@ -4,6 +4,9 @@ #include #include +#include + +#include "internal/types.h" MYNTEYE_BEGIN_NAMESPACE @@ -11,9 +14,60 @@ namespace { void unpack_stereo_img_data( const void *data, const StreamRequest &request, ImgData &img) { // NOLINT - UNUSED(data) - UNUSED(request) - UNUSED(img) + CHECK_EQ(request.format, Format::YUYV); + + auto data_new = reinterpret_cast(data); + std::size_t data_n = + request.width * request.height * bytes_per_pixel(request.format); + auto data_end = data_new + data_n; + + // LOG(INFO) << "ImagePacket (raw): header=0x" << std::hex << + // static_cast(*(data_end - 1)) + // << ", size=0x" << std::hex << static_cast(*(data_end - 2)) + // << ", frame_id="<< std::dec << ((*(data_end - 3) << 8) + *(data_end - 4)) + // << ", timestamp="<< std::dec << ((*(data_end - 5) << 24) + (*(data_end - + // 6) << 16) + (*(data_end - 7) << 8) + *(data_end - 8)) + // << ", exposure_time="<< std::dec << ((*(data_end - 9) << 8) + *(data_end + // - 10)) + // << ", checksum=0x" << std::hex << static_cast(*(data_end - 11)); + + std::size_t packet_n = sizeof(ImagePacket); + // LOG(INFO) << "ImagePacket Size: " << packet_n; + std::vector packet(packet_n); + std::reverse_copy(data_end - packet_n, data_end, packet.begin()); + + ImagePacket img_packet(packet.data()); + // LOG(INFO) << "ImagePacket (new): header=0x" << std::hex << + // static_cast(img_packet.header) + // << ", size=0x" << std::hex << static_cast(img_packet.size) + // << ", frame_id="<< std::dec << img_packet.frame_id + // << ", timestamp="<< std::dec << img_packet.timestamp + // << ", exposure_time="<< std::dec << img_packet.exposure_time + // << ", checksum=0x" << std::hex << static_cast(img_packet.checksum); + + if (img_packet.header != 0x3B) { + LOG(WARNING) << "Image packet header must be 0x3B, but 0x" << std::hex + << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(img_packet.header) << " now"; + return; + } + + std::uint8_t checksum = 0; + for (std::size_t i = 1, n = packet_n - 2; i <= n; i++) { + checksum = (checksum ^ packet[i]); + } + if (checksum != img_packet.checksum) { + LOG(WARNING) << "Image packet checksum should be 0x" << std::hex + << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(img_packet.checksum) << ", but 0x" + << std::setw(2) << std::setfill('0') + << static_cast(img_packet.checksum) << " now"; + return; + } + + img.frame_id = img_packet.frame_id; + img.timestamp = img_packet.timestamp; + img.exposure_time = img_packet.exposure_time; } void unpack_left_img_pixels( @@ -85,7 +139,7 @@ void Streams::PushStream(const Capabilities &capability, const void *data) { 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; + *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); @@ -106,12 +160,12 @@ void Streams::WaitForStreams() { } Streams::stream_datas_t Streams::GetStreamDatas(const Stream &stream) { + std::unique_lock lock(mtx_); 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 lock(mtx_); stream_datas_t datas = stream_datas_map_.at(stream); stream_datas_map_[stream].clear(); return datas; @@ -155,7 +209,11 @@ 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(new ImgData{0, 0, 0}); + if (stream == Stream::LEFT || stream == Stream::RIGHT) { + data.img = std::make_shared(); + } else { + data.img = nullptr; + } data.frame = std::make_shared(request.width, request.height, format, nullptr); stream_datas_map_[stream].push_back(data); diff --git a/src/internal/types.h b/src/internal/types.h index d21dfdd..2890c48 100644 --- a/src/internal/types.h +++ b/src/internal/types.h @@ -125,6 +125,32 @@ struct DeviceInfo { std::uint16_t nominal_baseline; }; +/** + * @ingroup datatypes + * Image packet. + */ +#pragma pack(push, 1) +struct ImagePacket { + std::uint8_t header; + std::uint8_t size; + std::uint16_t frame_id; + std::uint32_t timestamp; + std::uint16_t exposure_time; + std::uint8_t checksum; + + ImagePacket() = default; + explicit ImagePacket(std::uint8_t *data) { + header = *data; + size = *(data + 1); + frame_id = (*(data + 2) << 8) + *(data + 3); + timestamp = (*(data + 4) << 24) + (*(data + 5) << 16) + (*(data + 6) << 8) + + *(data + 7); + exposure_time = (*(data + 8) << 8) + *(data + 9); + checksum = *(data + 10); + } +}; +#pragma pack(pop) + #undef MYNTEYE_PROPERTY MYNTEYE_END_NAMESPACE