diff --git a/CMakeLists.txt b/CMakeLists.txt index cc2c310..ace55a8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,7 @@ cmake_minimum_required(VERSION 3.0) -project(mynteye VERSION 2.3.0 LANGUAGES C CXX) +project(mynteye VERSION 2.3.2 LANGUAGES C CXX) include(cmake/Common.cmake) @@ -219,6 +219,8 @@ if(WITH_API) src/mynteye/api/processor/depth_processor_ocv.cc src/mynteye/api/processor/rectify_processor_ocv.cc src/mynteye/api/config.cc + src/mynteye/api/correspondence.cc + src/mynteye/api/version_checker.cc ) if(WITH_CAM_MODELS) list(APPEND MYNTEYE_SRCS diff --git a/Makefile b/Makefile index 64699c2..f4057ff 100644 --- a/Makefile +++ b/Makefile @@ -130,7 +130,7 @@ endif # install -install: build +install: uninstall build @$(call echo,Make $@) ifeq ($(HOST_OS),Win) ifneq ($(HOST_NAME),MinGW) @@ -176,8 +176,8 @@ ifeq ($(HOST_OS),Mac) else @$(call cmake_build,./tools/_build) endif - - + + .PHONY: tools diff --git a/README.md b/README.md index e5b4fb6..2755f43 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # MYNT® EYE S SDK -[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.3.0-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK) +[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.3.2-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK) ## Overview @@ -17,11 +17,11 @@ Please follow the guide doc to install the SDK on different platforms. ## Documentations * [API Doc](https://github.com/slightech/MYNT-EYE-S-SDK/releases): API reference, some guides and data spec. - * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764152/mynt-eye-s-sdk-apidoc-2.3.0-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764156/mynt-eye-s-sdk-apidoc-2.3.0-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK/) - * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764160/mynt-eye-s-sdk-apidoc-2.3.0-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764173/mynt-eye-s-sdk-apidoc-2.3.0-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/api/mynt-eye-s-sdk-apidoc-2.3.0-zh-Hans/mynt-eye-s-sdk-apidoc-2.3.0-zh-Hans/index.html) + * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913110/mynt-eye-s-sdk-apidoc-2.3.2-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913111/mynt-eye-s-sdk-apidoc-2.3.2-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK/) + * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913112/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913113/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/api/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans/index.html) * [Guide Doc](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/releases): How to install and start using the SDK. - * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764143/mynt-eye-s-sdk-guide-2.3.0-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764145/mynt-eye-s-sdk-guide-2.3.0-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK-Guide/) - * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764150/mynt-eye-s-sdk-guide-2.3.0-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764163/mynt-eye-s-sdk-guide-2.3.0-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/sdk/mynt-eye-s-sdk-guide-2.3.0-zh-Hans/mynt-eye-s-sdk-guide-2.3.0-zh-Hans/index.html) + * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913052/mynt-eye-s-sdk-guide-2.3.2-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913053/mynt-eye-s-sdk-guide-2.3.2-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK-Guide/) + * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913054/mynt-eye-s-sdk-guide-2.3.2-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913056/mynt-eye-s-sdk-guide-2.3.2-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/sdk/mynt-eye-s-sdk-guide-2.3.2-zh-Hans/mynt-eye-s-sdk-guide-2.3.2-zh-Hans/index.html) > Supported languages: `en`, `zh-Hans`. diff --git a/doc/en/api.doxyfile b/doc/en/api.doxyfile index 9a6bb26..9400c74 100644 --- a/doc/en/api.doxyfile +++ b/doc/en/api.doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "MYNT EYE S SDK" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 2.3.0 +PROJECT_NUMBER = 2.3.2 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/doc/en/spec_hardware_info.md b/doc/en/spec_hardware_info.md index 7ad0981..16bf998 100644 --- a/doc/en/spec_hardware_info.md +++ b/doc/en/spec_hardware_info.md @@ -12,3 +12,5 @@ | Lens type | lens_type | - | × | √ Get/Set | 4 | vendor(2),product(2); default: 0 | | IMU type | imu_type | - | × | √ Get/Set | 4 | vendor(2),product(2); default: 0 | | Nominal baseline | nominal_baseline | - | × | √ Get/Set | 2 | unit: mm; default: 0 | +| Auxiliary chip version | auxiliary_chip_version | - | × | √ Get | 2 | major,minor | +| isp version | isp_version | - | × | √ Get | 2 | major,minor | diff --git a/doc/zh-Hans/api.doxyfile b/doc/zh-Hans/api.doxyfile index 312be3e..31d9a19 100644 --- a/doc/zh-Hans/api.doxyfile +++ b/doc/zh-Hans/api.doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "MYNT EYE S SDK" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 2.3.0 +PROJECT_NUMBER = 2.3.2 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/doc/zh-Hans/spec_hardware_info.md b/doc/zh-Hans/spec_hardware_info.md index e05f1aa..ba2dd41 100644 --- a/doc/zh-Hans/spec_hardware_info.md +++ b/doc/zh-Hans/spec_hardware_info.md @@ -12,6 +12,8 @@ | 镜头类型 | lens_type | - | × | √ Get/Set | 4 | vendor(2),product(2) ，未 Set 默认 0 | | IMU 类型 | imu_type | - | × | √ Get/Set | 4 | vendor(2),product(2) ，未 Set 默认 0 | | 基线长度 | nominal_baseline | - | × | √ Get/Set | 2 | 单位 mm ，未 set 默认 0 | +| 辅助芯片版本 | auxiliary_chip_version | - | × | √ Get | 2 | major,minor | +| ISP版本 | isp_version | - | × | √ Get | 2 | major,minor | * 描述符获取：指通用 USB 设备信息，可用工具查看。 * 拓展通道获取：指通过拓展通道（UVC Extension Unit）问硬件获取到的信息，需要读取。 diff --git a/include/mynteye/api/api.h b/include/mynteye/api/api.h index ea7a27f..d369ecd 100644 --- a/include/mynteye/api/api.h +++ b/include/mynteye/api/api.h @@ -30,6 +30,7 @@ MYNTEYE_BEGIN_NAMESPACE struct DeviceInfo; +class Correspondence; class Device; class Synthetic; @@ -91,6 +92,8 @@ class MYNTEYE_API API { using stream_callback_t = std::function; /** The api::MotionData callback. */ using motion_callback_t = std::function; + /** The enable/disable switch callback. */ + using stream_switch_callback_t = std::function; explicit API(std::shared_ptr device, CalibrationModel calib_model); virtual ~API(); @@ -280,11 +283,31 @@ class MYNTEYE_API API { * still support this stream. */ void EnableStreamData(const Stream &stream); + + /** + * Enable the data of stream. + * callback function will call before the father processor enable. + * when try_tag is true, the function will do nothing except callback. + */ + void EnableStreamData( + const Stream &stream, + stream_switch_callback_t callback, + bool try_tag = false); /** * Disable the data of stream. */ void DisableStreamData(const Stream &stream); + /** + * Disable the data of stream. + * callback function will call before the children processor disable. + * when try_tag is true, the function will do nothing except callback. + */ + void DisableStreamData( + const Stream &stream, + stream_switch_callback_t callback, + bool try_tag = false); + /** * Get the latest data of stream. */ @@ -305,6 +328,11 @@ class MYNTEYE_API API { */ std::vector GetMotionDatas(); + /** + * Enable motion datas with timestamp correspondence of some stream. + */ + void EnableTimestampCorrespondence(const Stream &stream); + /** * Enable the plugin. */ @@ -317,6 +345,10 @@ class MYNTEYE_API API { std::unique_ptr synthetic_; + std::unique_ptr correspondence_; + + motion_callback_t callback_; + void CheckImageParams(); }; diff --git a/include/mynteye/device/device.h b/include/mynteye/device/device.h index 30634b9..e188918 100644 --- a/include/mynteye/device/device.h +++ b/include/mynteye/device/device.h @@ -278,6 +278,10 @@ class MYNTEYE_API Device { */ std::vector GetStreamDatas(const Stream &stream); + /** + * Disable cache motion datas. + */ + void DisableMotionDatas(); /** * Enable cache motion datas. */ diff --git a/include/mynteye/device/types.h b/include/mynteye/device/types.h index 7ad1b75..7e156b0 100644 --- a/include/mynteye/device/types.h +++ b/include/mynteye/device/types.h @@ -162,6 +162,8 @@ struct MYNTEYE_API DeviceInfo { Type lens_type; Type imu_type; std::uint16_t nominal_baseline; + Version auxiliary_chip_version; + Version isp_version; }; #undef MYNTEYE_PROPERTY diff --git a/include/mynteye/types.h b/include/mynteye/types.h index b95d228..5af327e 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -122,6 +122,12 @@ enum class Info : std::uint8_t { IMU_TYPE, /** Nominal baseline */ NOMINAL_BASELINE, + /** Auxiliary chip version */ + AUXILIARY_CHIP_VERSION, + /** Isp version */ + ISP_VERSION, + /** SDK version*/ + SDK_VERSION, /** Last guard */ LAST }; diff --git a/samples/tutorials/CMakeLists.txt b/samples/tutorials/CMakeLists.txt index 911318b..60b01d2 100644 --- a/samples/tutorials/CMakeLists.txt +++ b/samples/tutorials/CMakeLists.txt @@ -105,6 +105,10 @@ if(PCL_FOUND) WITH_OPENCV WITH_PCL ) endif() +make_executable2(get_imu_correspondence + SRCS data/get_imu_correspondence.cc util/cv_painter.cc + WITH_OPENCV +) make_executable2(get_imu SRCS data/get_imu.cc util/cv_painter.cc WITH_OPENCV) make_executable2(get_from_callbacks SRCS data/get_from_callbacks.cc util/cv_painter.cc diff --git a/samples/tutorials/control/framerate.cc b/samples/tutorials/control/framerate.cc index d1226f5..41f1e5d 100644 --- a/samples/tutorials/control/framerate.cc +++ b/samples/tutorials/control/framerate.cc @@ -34,10 +34,6 @@ int main(int argc, char *argv[]) { // Set frame rate options for s1030 if (model == Model::STANDARD) { - // Attention: must set FRAME_RATE and IMU_FREQUENCY together, - // otherwise won't. - // succeed. - // FRAME_RATE values: 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60 api->SetOptionValue(Option::FRAME_RATE, 25); // IMU_FREQUENCY values: 100, 200, 250, 333, 500 diff --git a/samples/tutorials/data/get_device_info.cc b/samples/tutorials/data/get_device_info.cc index ddb3bd9..298d72e 100644 --- a/samples/tutorials/data/get_device_info.cc +++ b/samples/tutorials/data/get_device_info.cc @@ -28,6 +28,9 @@ int main(int argc, char *argv[]) { LOG(INFO) << "Lens type: " << api->GetInfo(Info::LENS_TYPE); LOG(INFO) << "IMU type: " << api->GetInfo(Info::IMU_TYPE); LOG(INFO) << "Nominal baseline: " << api->GetInfo(Info::NOMINAL_BASELINE); + LOG(INFO) << "Auxiliary chip version: " + << api->GetInfo(Info::AUXILIARY_CHIP_VERSION); + LOG(INFO) << "Nominal baseline: " << api->GetInfo(Info::ISP_VERSION); return 0; } diff --git a/samples/tutorials/data/get_imu_correspondence.cc b/samples/tutorials/data/get_imu_correspondence.cc new file mode 100644 index 0000000..9cec957 --- /dev/null +++ b/samples/tutorials/data/get_imu_correspondence.cc @@ -0,0 +1,85 @@ +// Copyright 2018 Slightech Co., Ltd. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include + +#include "mynteye/logger.h" +#include "mynteye/api/api.h" + +#include "util/cv_painter.h" + +MYNTEYE_USE_NAMESPACE + +int main(int argc, char *argv[]) { + auto &&api = API::Create(argc, argv); + if (!api) return 1; + + bool ok; + auto &&request = api->SelectStreamRequest(&ok); + if (!ok) return 1; + api->ConfigStreamRequest(request); + + // Enable motion datas with timestamp correspondence of some stream + api->EnableTimestampCorrespondence(Stream::LEFT); + + api->Start(Source::ALL); + + CVPainter painter; + + cv::namedWindow("frame"); + + std::uint64_t prev_img_stamp = 0; + std::uint64_t prev_imu_stamp = 0; + while (true) { + api->WaitForStreams(); + + auto &&left_data = api->GetStreamData(Stream::LEFT); + auto &&right_data = api->GetStreamData(Stream::RIGHT); + + auto img_stamp = left_data.img->timestamp; + LOG(INFO) << "Img timestamp: " << img_stamp + << ", diff_prev=" << (img_stamp - prev_img_stamp); + prev_img_stamp = img_stamp; + + cv::Mat img; + cv::hconcat(left_data.frame, right_data.frame, img); + + auto &&motion_datas = api->GetMotionDatas(); + LOG(INFO) << "Imu count: " << motion_datas.size(); + for (auto &&data : motion_datas) { + auto imu_stamp = data.imu->timestamp; + LOG(INFO) << "Imu timestamp: " << imu_stamp + << ", diff_prev=" << (imu_stamp - prev_imu_stamp) + << ", diff_img=" << (1.f + imu_stamp - img_stamp); + prev_imu_stamp = imu_stamp; + } + LOG(INFO); + + /* + painter.DrawImgData(img, *left_data.img); + if (!motion_datas.empty()) { + painter.DrawImuData(img, *motion_datas[0].imu); + } + */ + + cv::imshow("frame", img); + + char key = static_cast(cv::waitKey(1)); + if (key == 27 || key == 'q' || key == 'Q') { // ESC/Q + break; + } + } + + api->Stop(Source::ALL); + return 0; +} diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index 18e80cd..3f034be 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -22,9 +22,11 @@ #include #include "mynteye/logger.h" +#include "mynteye/api/correspondence.h" #include "mynteye/api/dl.h" #include "mynteye/api/plugin.h" #include "mynteye/api/synthetic.h" +#include "mynteye/api/version_checker.h" #include "mynteye/device/device.h" #include "mynteye/device/utils.h" @@ -208,7 +210,7 @@ std::vector get_plugin_paths() { } // namespace API::API(std::shared_ptr device, CalibrationModel calib_model) - : device_(device) { + : device_(device), correspondence_(nullptr) { VLOG(2) << __func__; // std::dynamic_pointer_cast(device_); synthetic_.reset(new Synthetic(this, calib_model)); @@ -221,7 +223,10 @@ API::~API() { std::shared_ptr API::Create(int argc, char *argv[]) { auto &&device = device::select(); if (!device) return nullptr; - return Create(argc, argv, device); + auto api = Create(argc, argv, device); + if (api && checkFirmwareVersion(api)) + return api; + return nullptr; } std::shared_ptr API::Create( @@ -260,7 +265,7 @@ std::shared_ptr API::Create(const std::shared_ptr &device) { } } else { LOG(ERROR) <<"no device!"; - api = std::make_shared(device, CalibrationModel::UNKNOW); + return nullptr; } return api; } @@ -323,6 +328,20 @@ std::shared_ptr API::GetInfo() const { } std::string API::GetInfo(const Info &info) const { + if (info == Info::SDK_VERSION) { + std::string info_path = + utils::get_sdk_install_dir(); + info_path.append(MYNTEYE_OS_SEP "share" \ + MYNTEYE_OS_SEP "mynteye" MYNTEYE_OS_SEP "build.info"); + + cv::FileStorage fs(info_path, cv::FileStorage::READ); + if (!fs.isOpened()) { + LOG(WARNING) << "build.info not found: " << info_path; + return "null"; + } + return fs["MYNTEYE_VERSION"]; + } + return device_->GetInfo(info); } @@ -377,10 +396,15 @@ void API::SetStreamCallback(const Stream &stream, stream_callback_t callback) { } void API::SetMotionCallback(motion_callback_t callback) { - static auto callback_ = callback; + if (correspondence_) { + correspondence_->SetMotionCallback(callback); + return; + } + callback_ = callback; if (callback_) { - device_->SetMotionCallback( - [](const device::MotionData &data) { callback_({data.imu}); }, true); + device_->SetMotionCallback([this](const device::MotionData &data) { + callback_({data.imu}); + }, true); } else { device_->SetMotionCallback(nullptr); } @@ -435,7 +459,11 @@ void API::Stop(const Source &source) { } void API::WaitForStreams() { - synthetic_->WaitForStreams(); + if (correspondence_) { + correspondence_->WaitForStreams(); + } else { + synthetic_->WaitForStreams(); + } } void API::EnableStreamData(const Stream &stream) { @@ -446,24 +474,69 @@ void API::DisableStreamData(const Stream &stream) { synthetic_->DisableStreamData(stream); } +void API::EnableStreamData( + const Stream &stream, stream_switch_callback_t callback, + bool try_tag) { + synthetic_->EnableStreamData(stream, callback, try_tag); +} +void API::DisableStreamData( + const Stream &stream, stream_switch_callback_t callback, + bool try_tag) { + synthetic_->DisableStreamData(stream, callback, try_tag); +} + api::StreamData API::GetStreamData(const Stream &stream) { - return synthetic_->GetStreamData(stream); + if (correspondence_ && correspondence_->Watch(stream)) { + return correspondence_->GetStreamData(stream); + } else { + return synthetic_->GetStreamData(stream); + } } std::vector API::GetStreamDatas(const Stream &stream) { - return synthetic_->GetStreamDatas(stream); + if (correspondence_ && correspondence_->Watch(stream)) { + return correspondence_->GetStreamDatas(stream); + } else { + return synthetic_->GetStreamDatas(stream); + } } void API::EnableMotionDatas(std::size_t max_size) { + if (correspondence_) return; // not cache them device_->EnableMotionDatas(max_size); } std::vector API::GetMotionDatas() { - std::vector datas; - for (auto &&data : device_->GetMotionDatas()) { - datas.push_back({data.imu}); + if (correspondence_) { + return correspondence_->GetMotionDatas(); + } else { + std::vector datas; + for (auto &&data : device_->GetMotionDatas()) { + datas.push_back({data.imu}); + } + return datas; + } +} + +void API::EnableTimestampCorrespondence(const Stream &stream) { + if (correspondence_ == nullptr) { + correspondence_.reset(new Correspondence(device_, stream)); + { + device_->DisableMotionDatas(); + if (callback_) { + correspondence_->SetMotionCallback(callback_); + callback_ = nullptr; + } + } + using namespace std::placeholders; // NOLINT + device_->SetMotionCallback( + std::bind(&Correspondence::OnMotionDataCallback, + correspondence_.get(), _1), + true); + synthetic_->SetStreamDataListener( + std::bind(&Correspondence::OnStreamDataCallback, + correspondence_.get(), _1, _2)); } - return datas; } void API::EnablePlugin(const std::string &path) { diff --git a/src/mynteye/api/correspondence.cc b/src/mynteye/api/correspondence.cc new file mode 100644 index 0000000..d4af6cd --- /dev/null +++ b/src/mynteye/api/correspondence.cc @@ -0,0 +1,277 @@ +// Copyright 2018 Slightech Co., Ltd. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "mynteye/api/correspondence.h" + +#include "mynteye/device/device.h" +#include "mynteye/logger.h" + +MYNTEYE_BEGIN_NAMESPACE + +Correspondence::Correspondence(const std::shared_ptr &device, + const Stream &stream) + : device_(device), stream_(stream), ready_image_timestamp_(0) { + VLOG(2) << __func__; + // set matched stream to be watched too, + // aim to make stream and matched stream correspondence + if (stream_ == Stream::LEFT) { + stream_match_ = Stream::RIGHT; + } else if (stream_ == Stream::RIGHT) { + stream_match_ = Stream::LEFT; + } else if (stream_ == Stream::LEFT_RECTIFIED) { + stream_match_ = Stream::RIGHT_RECTIFIED; + } else if (stream_ == Stream::RIGHT_RECTIFIED) { + stream_match_ = Stream::LEFT_RECTIFIED; + } else { + stream_match_ = Stream::LAST; + } + EnableStreamMatch(); + + auto framerate = device_->GetOptionValue(Option::FRAME_RATE); + stream_interval_us_ = 1000000.f / framerate; + stream_interval_us_half_ = 0.5f * stream_interval_us_; + VLOG(2) << "framerate: " << framerate + << ", interval_us: " << stream_interval_us_; +} + +Correspondence::~Correspondence() { + VLOG(2) << __func__; +} + +bool Correspondence::Watch(const Stream &stream) const { + if (stream == stream_) return true; + if (stream_match_enabled_ && stream == stream_match_) return true; + return false; +} + +void Correspondence::OnStreamDataCallback( + const Stream &stream, const api::StreamData &data) { + if (!Watch(stream)) { + return; // unwatched + } + // LOG(INFO) << __func__ << ", " << stream + // << ", id: " << data.frame_id << ", stamp: " << data.img->timestamp; + // if (data.img == nullptr) { + // LOG(FATAL) << "stream data image info is empty!"; + // } + std::lock_guard _(mtx_stream_datas_); + if (stream == stream_) { + stream_datas_.push_back(std::move(data)); + } else if (/*stream_match_enabled_ && */stream == stream_match_) { + stream_datas_match_.push_back(std::move(data)); + } + NotifyStreamDataReady(); +} + +void Correspondence::OnMotionDataCallback(const device::MotionData &data) { + // LOG(INFO) << __func__ << ", id: " << data.imu->frame_id + // << ", stamp: " << data.imu->timestamp; + { + std::lock_guard _(mtx_motion_datas_); + motion_datas_.push_back(data); + } + if (motion_callback_) { + motion_callback_({data.imu}); + } +} + +void Correspondence::SetMotionCallback(API::motion_callback_t callback) { + // LOG(INFO) << __func__; + motion_callback_ = callback; +} + +void Correspondence::WaitForStreams() { + if (stream_ == Stream::LEFT || stream_ == Stream::RIGHT) { + // Wait native stream ready, avoid get these stream empty + // Todo: determine native stream according to device + WaitStreamDataReady(); + return; + } + device_->WaitForStreams(); +} + +api::StreamData Correspondence::GetStreamData(const Stream &stream) { + auto datas = GetStreamDatas(stream); + return datas.empty() ? api::StreamData{} : datas.back(); +} + +std::vector Correspondence::GetStreamDatas( + const Stream &stream) { + if (!Watch(stream)) { + LOG(ERROR) << "Get unwatched stream data of " << stream; + return {}; + } + + std::lock_guard _(mtx_stream_datas_); + static std::uint32_t stream_count_ = 0; + static std::uint32_t stream_match_count_ = 0; + + if (stream == stream_) { + auto datas = GetReadyStreamData(false); + + if (stream_count_ < 10) { + ++stream_count_; + } else { + // get stream, but not get matched stream, disable it + if (stream_match_count_ == 0) { + DisableStreamMatch(); + } + } + + return datas; + } else if (/*stream_match_enabled_ && */stream == stream_match_) { + auto datas = GetReadyStreamData(true); + + if (stream_match_count_ < 10) { + ++stream_match_count_; + } + + return datas; + } + + return {}; +} + +std::vector Correspondence::GetMotionDatas() { + return GetReadyMotionDatas(); +} + +void Correspondence::EnableStreamMatch() { + stream_match_enabled_ = true; +} + +void Correspondence::DisableStreamMatch() { + stream_match_enabled_ = false; + stream_datas_match_.clear(); +} + +void Correspondence::WaitStreamDataReady() { + std::unique_lock lock(mtx_stream_datas_); + auto ready = std::bind(&Correspondence::IsStreamDataReady, this); + bool ok = cond_stream_datas_.wait_for(lock, std::chrono::seconds(3), ready); + if (!ok) { + LOG(FATAL) << "Timeout waiting for key frames. Please use USB 3.0, and not " + "in virtual machine."; + } +} + +void Correspondence::NotifyStreamDataReady() { + cond_stream_datas_.notify_one(); +} + +bool Correspondence::IsStreamDataReady() { + if (stream_datas_.empty()) return false; + if (stream_match_enabled_) { + if (stream_datas_match_.empty()) return false; + } + if (motion_datas_.empty()) return false; + + std::uint64_t img_stamp = 0; + std::uint64_t img_macth_stamp = 0; + { + std::lock_guard _(mtx_stream_datas_); + auto data = stream_datas_.front(); + if (data.img == nullptr) { + LOG(FATAL) << "stream data image info is empty!"; + } + img_stamp = data.img->timestamp; + + if (stream_match_enabled_) { + img_macth_stamp = stream_datas_match_.front().img->timestamp; + } + } + std::uint64_t imu_stamp = 0; + { + std::lock_guard _(mtx_motion_datas_); + auto data = motion_datas_.back(); + if (data.imu == nullptr) { + LOG(FATAL) << "motion data imu info is empty!"; + } + imu_stamp = data.imu->timestamp; + } + + if (stream_match_enabled_) { + return img_stamp + stream_interval_us_half_ < imu_stamp + && img_macth_stamp + stream_interval_us_half_ < imu_stamp; + } else { + return img_stamp + stream_interval_us_half_ < imu_stamp; + } +} + +std::vector Correspondence::GetReadyStreamData(bool matched) { + std::uint64_t imu_stamp = 0; + { + std::lock_guard _(mtx_motion_datas_); + if (motion_datas_.empty()) { + LOG(WARNING) << "motion data is unexpected empty!" + "\n\n Please ensure Start(Source::MOTION_TRACKING) " + "or Start(Source::ALL)\n"; + std::lock_guard _(mtx_stream_datas_); + return std::move(matched ? stream_datas_match_ : stream_datas_); + } + imu_stamp = motion_datas_.back().imu->timestamp; + } + std::lock_guard _(mtx_stream_datas_); + + std::vector &datas = + matched ? stream_datas_match_ : stream_datas_; + + // LOG(INFO) << "datas.size: " << datas.size() << ", matched: " << matched; + std::vector result; + + for (auto it = datas.begin(); it != datas.end(); ) { + // LOG(INFO) << "data.id: " << it->frame_id; + auto img_stamp = it->img->timestamp; + if (img_stamp + stream_interval_us_half_ < imu_stamp) { + // LOG(INFO) << "data.id: " << it->frame_id << " > result"; + result.push_back(std::move(*it)); + it = datas.erase(it); + } else { + // ++it; + break; + } + } + // LOG(INFO) << "datas.size: " << datas.size() + // << ", result.size: " << result.size(); + + if (!matched && !result.empty()) { + // last match stream timestamp + ready_image_timestamp_ = result.back().img->timestamp; + } + return result; +} + +std::vector Correspondence::GetReadyMotionDatas() { + if (ready_image_timestamp_ == 0) return {}; + std::lock_guard _(mtx_motion_datas_); + + std::vector result; + + auto &&datas = motion_datas_; + for (auto it = datas.begin(); it != datas.end(); ) { + auto imu_stamp = it->imu->timestamp; + if (imu_stamp < ready_image_timestamp_ - stream_interval_us_half_) { + it = datas.erase(it); + } else if (imu_stamp > ready_image_timestamp_ + stream_interval_us_half_) { + // ++it; + break; + } else { + result.push_back({it->imu}); + it = datas.erase(it); + } + } + + return result; +} + +MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/correspondence.h b/src/mynteye/api/correspondence.h new file mode 100644 index 0000000..df822e0 --- /dev/null +++ b/src/mynteye/api/correspondence.h @@ -0,0 +1,80 @@ +// Copyright 2018 Slightech Co., Ltd. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef MYNTEYE_API_CONFIG_H_ +#define MYNTEYE_API_CONFIG_H_ +#pragma once + +#include +#include +#include +#include +#include + +#include "mynteye/api/api.h" +#include "mynteye/device/callbacks.h" + +MYNTEYE_BEGIN_NAMESPACE + +class Correspondence { + public: + Correspondence(const std::shared_ptr &device, const Stream &stream); + ~Correspondence(); + + bool Watch(const Stream &stream) const; + + void OnStreamDataCallback(const Stream &stream, const api::StreamData &data); + void OnMotionDataCallback(const device::MotionData &data); + + void SetMotionCallback(API::motion_callback_t callback); + + void WaitForStreams(); + api::StreamData GetStreamData(const Stream &stream); + std::vector GetStreamDatas(const Stream &stream); + std::vector GetMotionDatas(); + + private: + void EnableStreamMatch(); + void DisableStreamMatch(); + + void WaitStreamDataReady(); + void NotifyStreamDataReady(); + + bool IsStreamDataReady(); + + std::vector GetReadyStreamData(bool matched); + std::vector GetReadyMotionDatas(); + + std::shared_ptr device_; + Stream stream_; + Stream stream_match_; + std::atomic_bool stream_match_enabled_; + + float stream_interval_us_; + float stream_interval_us_half_; + + API::motion_callback_t motion_callback_; + std::vector motion_datas_; + std::recursive_mutex mtx_motion_datas_; + + std::vector stream_datas_; + std::vector stream_datas_match_; + std::recursive_mutex mtx_stream_datas_; + std::condition_variable_any cond_stream_datas_; + + std::uint64_t ready_image_timestamp_; +}; + +MYNTEYE_END_NAMESPACE + +#endif // MYNTEYE_API_CONFIG_H_ diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index b68cfd4..654d30e 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -74,6 +74,44 @@ void process_childs( } } +// ObjMat/ObjMat2 > api::StreamData + +api::StreamData obj_data_first(const ObjMat2 *obj) { + return {obj->first_data, obj->first, nullptr, obj->first_id}; +} + +api::StreamData obj_data_second(const ObjMat2 *obj) { + return {obj->second_data, obj->second, nullptr, obj->second_id}; +} + +api::StreamData obj_data(const ObjMat *obj) { + return {obj->data, obj->value, nullptr, obj->id}; +} + +api::StreamData obj_data_first(const std::shared_ptr &obj) { + return {obj->first_data, obj->first, nullptr, obj->first_id}; +} + +api::StreamData obj_data_second(const std::shared_ptr &obj) { + return {obj->second_data, obj->second, nullptr, obj->second_id}; +} + +api::StreamData obj_data(const std::shared_ptr &obj) { + return {obj->data, obj->value, nullptr, obj->id}; +} + +// api::StreamData > ObjMat/ObjMat2 + +ObjMat data_obj(const api::StreamData &data) { + return ObjMat{data.frame, data.frame_id, data.img}; +} + +ObjMat2 data_obj(const api::StreamData &first, const api::StreamData &second) { + return ObjMat2{ + first.frame, first.frame_id, first.img, + second.frame, second.frame_id, second.img}; +} + } // namespace void Synthetic::InitCalibInfo() { @@ -105,7 +143,8 @@ Synthetic::Synthetic(API *api, CalibrationModel calib_model) : api_(api), plugin_(nullptr), calib_model_(calib_model), - calib_default_tag_(false) { + calib_default_tag_(false), + stream_data_listener_(nullptr) { VLOG(2) << __func__; CHECK_NOTNULL(api_); InitCalibInfo(); @@ -121,6 +160,10 @@ Synthetic::~Synthetic() { } } +void Synthetic::SetStreamDataListener(stream_data_listener_t listener) { + stream_data_listener_ = listener; +} + void Synthetic::NotifyImageParamsChanged() { if (!calib_default_tag_) { intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT); @@ -195,27 +238,6 @@ bool Synthetic::checkControlDateWithStream(const Stream& stream) const { return false; } -void Synthetic::EnableStreamData(const Stream &stream) { - // Activate processors of synthetic stream - auto processor = getProcessorWithStream(stream); - iterate_processor_CtoP_before(processor, - [](std::shared_ptr proce){ - auto streams = proce->getTargetStreams(); - int act_tag = 0; - for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) { - if (proce->target_streams_[i].enabled_mode_ == MODE_LAST) { - act_tag++; - proce->target_streams_[i].enabled_mode_ = MODE_SYNTHETIC; - } - } - if (act_tag > 0 && !proce->IsActivated()) { - // std::cout << proce->Name() << " Active now" << std::endl; - proce->Activate(); - } - }); -} - - bool Synthetic::Supports(const Stream &stream) const { return checkControlDateWithStream(stream); } @@ -228,16 +250,45 @@ Synthetic::mode_t Synthetic::SupportsMode(const Stream &stream) const { return MODE_LAST; } -void Synthetic::DisableStreamData(const Stream &stream) { +void Synthetic::EnableStreamData( + const Stream &stream, stream_switch_callback_t callback, + bool try_tag) { + // Activate processors of synthetic stream + auto processor = getProcessorWithStream(stream); + iterate_processor_CtoP_before(processor, + [callback, try_tag](std::shared_ptr proce){ + auto streams = proce->getTargetStreams(); + int act_tag = 0; + for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) { + if (proce->target_streams_[i].enabled_mode_ == MODE_LAST) { + callback(proce->target_streams_[i].stream); + if (!try_tag) { + act_tag++; + proce->target_streams_[i].enabled_mode_ = MODE_SYNTHETIC; + } + } + } + if (act_tag > 0 && !proce->IsActivated()) { + // std::cout << proce->Name() << " Active now" << std::endl; + proce->Activate(); + } + }); +} +void Synthetic::DisableStreamData( + const Stream &stream, stream_switch_callback_t callback, + bool try_tag) { auto processor = getProcessorWithStream(stream); iterate_processor_PtoC_before(processor, - [](std::shared_ptr proce){ + [callback, try_tag](std::shared_ptr proce){ auto streams = proce->getTargetStreams(); int act_tag = 0; for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) { if (proce->target_streams_[i].enabled_mode_ == MODE_SYNTHETIC) { - act_tag++; - proce->target_streams_[i].enabled_mode_ = MODE_LAST; + callback(proce->target_streams_[i].stream); + if (!try_tag) { + act_tag++; + proce->target_streams_[i].enabled_mode_ = MODE_LAST; + } } } if (act_tag > 0 && proce->IsActivated()) { @@ -247,6 +298,20 @@ void Synthetic::DisableStreamData(const Stream &stream) { }); } +void Synthetic::EnableStreamData(const Stream &stream) { + EnableStreamData(stream, [](const Stream &stream){ + // std::cout << stream << "enabled in callback" << std::endl; + MYNTEYE_UNUSED(stream); + }, false); +} + +void Synthetic::DisableStreamData(const Stream &stream) { + DisableStreamData(stream, [](const Stream &stream){ + // std::cout << stream << "disabled in callback" << std::endl; + MYNTEYE_UNUSED(stream); + }, false); +} + bool Synthetic::IsStreamDataEnabled(const Stream &stream) const { if (checkControlDateWithStream(stream)) { auto data = getControlDateWithStream(stream); @@ -335,7 +400,7 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) { if (out != nullptr) { auto &&output = Object::Cast(out); if (output != nullptr) { - return {output->data, output->value, nullptr, output->id}; + return obj_data(output); } VLOG(2) << "Rectify not ready now"; } @@ -349,15 +414,9 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) { for (auto it : streams) { if (it.stream == stream) { if (num == 1) { - return {output->first_data, - output->first, - nullptr, - output->first_id}; + return obj_data_first(output); } else { - return {output->second_data, - output->second, - nullptr, - output->second_id}; + return obj_data_second(output); } } num++; @@ -452,66 +511,60 @@ bool Synthetic::IsStreamEnabledSynthetic(const Stream &stream) const { void Synthetic::InitProcessors() { std::shared_ptr rectify_processor = nullptr; -#ifdef WITH_CAM_MODELS - std::shared_ptr rectify_processor_imp = nullptr; -#endif - cv::Mat Q; - if (calib_model_ == CalibrationModel::PINHOLE) { - auto &&rectify_processor_ocv = - std::make_shared(intr_left_, intr_right_, extr_, - RECTIFY_PROC_PERIOD); - Q = rectify_processor_ocv->Q; - rectify_processor = rectify_processor_ocv; -#ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - rectify_processor_imp = - std::make_shared(intr_left_, intr_right_, extr_, - RECTIFY_PROC_PERIOD); - rectify_processor = rectify_processor_imp; -#endif - } else { - LOG(ERROR) << "Unknow calib model type in device: " - << calib_model_ << ", use default pinhole model"; - auto &&rectify_processor_ocv = - std::make_shared(intr_left_, intr_right_, extr_, - RECTIFY_PROC_PERIOD); - rectify_processor = rectify_processor_ocv; - } + std::shared_ptr points_processor = nullptr; + std::shared_ptr depth_processor = nullptr; + auto &&disparity_processor = std::make_shared(DisparityComputingMethod::SGBM, DISPARITY_PROC_PERIOD); auto &&disparitynormalized_processor = std::make_shared( DISPARITY_NORM_PROC_PERIOD); - std::shared_ptr points_processor = nullptr; - if (calib_model_ == CalibrationModel::PINHOLE) { + + auto root_processor = + std::make_shared(ROOT_PROC_PERIOD); + + if (calib_model_ == CalibrationModel::PINHOLE) { + // PINHOLE + auto &&rectify_processor_ocv = + std::make_shared(intr_left_, intr_right_, extr_, + RECTIFY_PROC_PERIOD); + rectify_processor = rectify_processor_ocv; points_processor = std::make_shared( - Q, POINTS_PROC_PERIOD); + rectify_processor_ocv->Q, POINTS_PROC_PERIOD); + depth_processor = std::make_shared(DEPTH_PROC_PERIOD); + + root_processor->AddChild(rectify_processor); + rectify_processor->AddChild(disparity_processor); + disparity_processor->AddChild(disparitynormalized_processor); + disparity_processor->AddChild(points_processor); + points_processor->AddChild(depth_processor); #ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + // KANNALA_BRANDT + auto rectify_processor_imp = + std::make_shared(intr_left_, intr_right_, extr_, + RECTIFY_PROC_PERIOD); + rectify_processor = rectify_processor_imp; points_processor = std::make_shared( rectify_processor_imp -> getCalibInfoPair(), POINTS_PROC_PERIOD); -#endif - } else { - points_processor = std::make_shared( - Q, POINTS_PROC_PERIOD); - } - std::shared_ptr depth_processor = nullptr; - if (calib_model_ == CalibrationModel::PINHOLE) { - depth_processor = std::make_shared(DEPTH_PROC_PERIOD); -#ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { depth_processor = std::make_shared( rectify_processor_imp -> getCalibInfoPair(), DEPTH_PROC_PERIOD); + + root_processor->AddChild(rectify_processor); + rectify_processor->AddChild(disparity_processor); + disparity_processor->AddChild(disparitynormalized_processor); + disparity_processor->AddChild(depth_processor); + depth_processor->AddChild(points_processor); #endif } else { - depth_processor = std::make_shared(DEPTH_PROC_PERIOD); + // UNKNOW + LOG(ERROR) << "Unknow calib model type in device: " + << calib_model_; + return; } - auto root_processor = - std::make_shared(ROOT_PROC_PERIOD); - root_processor->AddChild(rectify_processor); rectify_processor->addTargetStreams( {Stream::LEFT_RECTIFIED, Mode::MODE_LAST, Mode::MODE_LAST, nullptr}); @@ -559,30 +612,14 @@ void Synthetic::InitProcessors() { depth_processor->SetPostProcessCallback( std::bind(&Synthetic::OnDepthPostProcess, this, _1)); - if (calib_model_ == CalibrationModel::PINHOLE) { - // PINHOLE - rectify_processor->AddChild(disparity_processor); - disparity_processor->AddChild(disparitynormalized_processor); - disparity_processor->AddChild(points_processor); - points_processor->AddChild(depth_processor); - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - // KANNALA_BRANDT - rectify_processor->AddChild(disparity_processor); - disparity_processor->AddChild(disparitynormalized_processor); - disparity_processor->AddChild(depth_processor); - depth_processor->AddChild(points_processor); - } else { - // UNKNOW - LOG(ERROR) << "Unknow calib model type in device: " - << calib_model_; - } - - processor_ = rectify_processor; + processor_ = root_processor; } void Synthetic::ProcessNativeStream( const Stream &stream, const api::StreamData &data) { + NotifyStreamData(stream, data); if (stream == Stream::LEFT || stream == Stream::RIGHT) { + std::unique_lock lk(mtx_left_right_ready_); static api::StreamData left_data, right_data; if (stream == Stream::LEFT) { left_data = data; @@ -603,9 +640,7 @@ void Synthetic::ProcessNativeStream( << calib_model_ << ", use default pinhole model"; processor = find_processor(processor_); } - processor->Process(ObjMat2{ - left_data.frame, left_data.frame_id, left_data.img, - right_data.frame, right_data.frame_id, right_data.img}); + processor->Process(data_obj(left_data, right_data)); } return; } @@ -627,34 +662,28 @@ void Synthetic::ProcessNativeStream( name = RectifyProcessor::NAME; #endif } - process_childs( - processor_, name, ObjMat2{ - left_rect_data.frame, left_rect_data.frame_id, left_rect_data.img, - right_rect_data.frame, right_rect_data.frame_id, - right_rect_data.img}); + process_childs(processor_, name, + data_obj(left_rect_data, right_rect_data)); } return; } switch (stream) { case Stream::DISPARITY: { - process_childs(processor_, DisparityProcessor::NAME, - ObjMat{data.frame, data.frame_id, data.img}); + process_childs(processor_, DisparityProcessor::NAME, data_obj(data)); } break; case Stream::DISPARITY_NORMALIZED: { process_childs(processor_, DisparityNormalizedProcessor::NAME, - ObjMat{data.frame, data.frame_id, data.img}); + data_obj(data)); } break; case Stream::POINTS: { if (calib_model_ == CalibrationModel::PINHOLE) { // PINHOLE - process_childs(processor_, PointsProcessorOCV::NAME, - ObjMat{data.frame, data.frame_id, data.img}); + process_childs(processor_, PointsProcessorOCV::NAME, data_obj(data)); #ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { // KANNALA_BRANDT - process_childs(processor_, PointsProcessor::NAME, - ObjMat{data.frame, data.frame_id, data.img}); + process_childs(processor_, PointsProcessor::NAME, data_obj(data)); #endif } else { // UNKNOW @@ -665,13 +694,11 @@ void Synthetic::ProcessNativeStream( case Stream::DEPTH: { if (calib_model_ == CalibrationModel::PINHOLE) { // PINHOLE - process_childs(processor_, DepthProcessorOCV::NAME, - ObjMat{data.frame, data.frame_id, data.img}); + process_childs(processor_, DepthProcessorOCV::NAME, data_obj(data)); #ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { // KANNALA_BRANDT - process_childs(processor_, DepthProcessor::NAME, - ObjMat{data.frame, data.frame_id, data.img}); + process_childs(processor_, DepthProcessor::NAME, data_obj(data)); #endif } else { // UNKNOW @@ -737,51 +764,51 @@ bool Synthetic::OnDepthProcess( void Synthetic::OnRectifyPostProcess(Object *const out) { const ObjMat2 *output = Object::Cast(out); + NotifyStreamData(Stream::LEFT_RECTIFIED, obj_data_first(output)); + NotifyStreamData(Stream::RIGHT_RECTIFIED, obj_data_second(output)); if (HasStreamCallback(Stream::LEFT_RECTIFIED)) { auto data = getControlDateWithStream(Stream::LEFT_RECTIFIED); - data.stream_callback( - {output->first_data, output->first, nullptr, output->first_id}); + data.stream_callback(obj_data_first(output)); } if (HasStreamCallback(Stream::RIGHT_RECTIFIED)) { auto data = getControlDateWithStream(Stream::RIGHT_RECTIFIED); - data.stream_callback( - {output->second_data, output->second, nullptr, output->second_id}); + data.stream_callback(obj_data_second(output)); } } void Synthetic::OnDisparityPostProcess(Object *const out) { const ObjMat *output = Object::Cast(out); + NotifyStreamData(Stream::DISPARITY, obj_data(output)); if (HasStreamCallback(Stream::DISPARITY)) { auto data = getControlDateWithStream(Stream::DISPARITY); - data.stream_callback( - {output->data, output->value, nullptr, output->id}); + data.stream_callback(obj_data(output)); } } void Synthetic::OnDisparityNormalizedPostProcess(Object *const out) { const ObjMat *output = Object::Cast(out); + NotifyStreamData(Stream::DISPARITY_NORMALIZED, obj_data(output)); if (HasStreamCallback(Stream::DISPARITY_NORMALIZED)) { auto data = getControlDateWithStream(Stream::DISPARITY_NORMALIZED); - data.stream_callback( - {output->data, output->value, nullptr, output->id}); + data.stream_callback(obj_data(output)); } } void Synthetic::OnPointsPostProcess(Object *const out) { const ObjMat *output = Object::Cast(out); + NotifyStreamData(Stream::POINTS, obj_data(output)); if (HasStreamCallback(Stream::POINTS)) { auto data = getControlDateWithStream(Stream::POINTS); - data.stream_callback( - {output->data, output->value, nullptr, output->id}); + data.stream_callback(obj_data(output)); } } void Synthetic::OnDepthPostProcess(Object *const out) { const ObjMat *output = Object::Cast(out); + NotifyStreamData(Stream::DEPTH, obj_data(output)); if (HasStreamCallback(Stream::DEPTH)) { auto data = getControlDateWithStream(Stream::DEPTH); - data.stream_callback( - {output->data, output->value, nullptr, output->id}); + data.stream_callback(obj_data(output)); } } @@ -796,4 +823,11 @@ void Synthetic::SetDisparityComputingMethodType( LOG(ERROR) << "ERROR: no suited processor for disparity computing."; } +void Synthetic::NotifyStreamData( + const Stream &stream, const api::StreamData &data) { + if (stream_data_listener_) { + stream_data_listener_(stream, data); + } +} + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/synthetic.h b/src/mynteye/api/synthetic.h index e39088b..f1c3d1d 100644 --- a/src/mynteye/api/synthetic.h +++ b/src/mynteye/api/synthetic.h @@ -19,6 +19,7 @@ #include #include #include +#include #include "mynteye/api/api.h" #include "mynteye/api/config.h" @@ -34,6 +35,9 @@ struct Object; class Synthetic { public: using stream_callback_t = API::stream_callback_t; + using stream_data_listener_t = + std::function; + using stream_switch_callback_t = API::stream_switch_callback_t; typedef enum Mode { MODE_NATIVE, // Native stream @@ -51,6 +55,8 @@ class Synthetic { explicit Synthetic(API *api, CalibrationModel calib_model); ~Synthetic(); + void SetStreamDataListener(stream_data_listener_t listener); + void NotifyImageParamsChanged(); bool Supports(const Stream &stream) const; @@ -58,6 +64,11 @@ class Synthetic { void EnableStreamData(const Stream &stream); void DisableStreamData(const Stream &stream); + + void EnableStreamData( + const Stream &stream, stream_switch_callback_t callback, bool try_tag); + void DisableStreamData( + const Stream &stream, stream_switch_callback_t callback, bool try_tag); bool IsStreamDataEnabled(const Stream &stream) const; void SetStreamCallback(const Stream &stream, stream_callback_t callback); @@ -125,6 +136,8 @@ class Synthetic { void OnPointsPostProcess(Object *const out); void OnDepthPostProcess(Object *const out); + void NotifyStreamData(const Stream &stream, const api::StreamData &data); + API *api_; std::shared_ptr processor_; @@ -132,6 +145,7 @@ class Synthetic { std::shared_ptr plugin_; CalibrationModel calib_model_; + std::mutex mtx_left_right_ready_; std::shared_ptr intr_left_; std::shared_ptr intr_right_; @@ -139,6 +153,8 @@ class Synthetic { bool calib_default_tag_; std::vector> processors_; + + stream_data_listener_t stream_data_listener_; }; class SyntheticProcessorPart { diff --git a/src/mynteye/api/version_checker.cc b/src/mynteye/api/version_checker.cc new file mode 100644 index 0000000..723bbf0 --- /dev/null +++ b/src/mynteye/api/version_checker.cc @@ -0,0 +1,136 @@ +// Copyright 2018 Slightech Co., Ltd. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "mynteye/api/version_checker.h" +#include "mynteye/device/utils.h" +#include "mynteye/logger.h" +#include "mynteye/types.h" + +MYNTEYE_BEGIN_NAMESPACE + +typedef struct { + const std::string device_type; + const std::string sdk_version; + const std::string firmware_version; + const std::string status; +}firmware_version_match_table_unit; + +const char* ERRO_DESCRIPTION_F = + "Please update the firmware at first"; +const char* ERRO_DESCRIPTION_S = + "Please update the SDK at first"; +const char* WARN_DESCRIPTION_F = + "We suggest that you should update the firmware"; +const char* WARN_DESCRIPTION_S = + "We suggest that you should update the SDK"; +const char* PASS_DESCRIPTION = "pass"; + +/** firmware/sdk version matched table */ +/**----device type-----sdk version---firmware version-----pass tag-----*/ +static const firmware_version_match_table_unit FSVM_TABLE[] ={ +/** S1030 */ + {"MYNT-EYE-S1030", ">2.3.0", ">2.2.0", PASS_DESCRIPTION}, + {"MYNT-EYE-S1030", ">2.3.0", "2.2.0", WARN_DESCRIPTION_F}, + {"MYNT-EYE-S1030", ">2.3.0", "<2.2.0", ERRO_DESCRIPTION_F}, + {"MYNT-EYE-S1030", "<2.3.1", "<2.2.0", WARN_DESCRIPTION_S}, +/** S2100 */ + {"MYNT-EYE-S2100", ">2.3.0", "1.0", PASS_DESCRIPTION}, + {"MYNT-EYE-S2100", "<2.3.1", "1.0", ERRO_DESCRIPTION_S}, +/** S210A */ + {"MYNT-EYE-S210A", ">2.3.0", "1.0", PASS_DESCRIPTION}, + {"MYNT-EYE-S210A", "<2.3.1", "1.0", ERRO_DESCRIPTION_S}, +}; + +void getVersion(const std::string &str, char *version) { + std::string st1(""); + int j = 0; + for (size_t i = 0; i < str.size(); i++) { + if (str[i] == '.') { + version[j++] = atoi(st1.c_str()); + st1 = ""; + } else { + st1 += str[i]; + } + } + version[j++] = atoi(st1.c_str()); +} + +bool conditionMatch(const std::string& condition, const std::string& target) { + char version[4] = {0}; + char version_c[4] = {0}; + getVersion(target, version); + int tag_c = 0; + std::string condition_c; + if (condition[0] == '>') { + tag_c = 1; + condition_c = condition.substr(1); + } else if (condition[0] == '<') { + tag_c = -1; + condition_c = condition.substr(1); + } else { + tag_c = 0; + condition_c = condition; + } + getVersion(condition_c, version_c); + int tag_big = memcmp(version, version_c, 4); + if (tag_big * tag_c > 0 || (tag_big == 0 && tag_c == 0)) return true; + return false; +} + +enum STATUS_UNIT { + ST_PASS, + ST_ERRO_F, + ST_ERRO_S, + ST_NOT_PASS +}; + +STATUS_UNIT checkUnit(const std::string& sdkv, + const std::string& devn, + const std::string& firmv, + const firmware_version_match_table_unit& condition) { + if (condition.device_type == devn && + conditionMatch(condition.sdk_version, sdkv) && + conditionMatch(condition.firmware_version, firmv)) { + if (condition.status == ERRO_DESCRIPTION_F) return ST_ERRO_F; + if (condition.status == ERRO_DESCRIPTION_S) return ST_ERRO_S; + if (condition.status == WARN_DESCRIPTION_F || + condition.status == WARN_DESCRIPTION_S) { + LOG(WARNING) << condition.status; + } + return ST_PASS; + } + return ST_NOT_PASS; +} + +bool checkFirmwareVersion(const std::shared_ptr api) { + auto sdkv = api->GetInfo(Info::SDK_VERSION); + auto devn = api->GetInfo(Info::DEVICE_NAME); + auto firmv = api->GetInfo(Info::FIRMWARE_VERSION); + + for (size_t i =0; + i < sizeof(FSVM_TABLE)/sizeof(firmware_version_match_table_unit); + i++) { + auto res = checkUnit(sdkv, devn, firmv, FSVM_TABLE[i]); + if (res == ST_PASS) { + return true; + } else if (res == ST_ERRO_S || res == ST_ERRO_F) { + LOG(ERROR) << FSVM_TABLE[i].status; + return false; + } + } + LOG(ERROR) << ERRO_DESCRIPTION_S; + return false; +} + +MYNTEYE_END_NAMESPACE + diff --git a/src/mynteye/api/version_checker.h b/src/mynteye/api/version_checker.h new file mode 100644 index 0000000..6921975 --- /dev/null +++ b/src/mynteye/api/version_checker.h @@ -0,0 +1,25 @@ +// Copyright 2018 Slightech Co., Ltd. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef MYNTEYE_API_VERSION_CHECKER_H_ +#define MYNTEYE_API_VERSION_CHECKER_H_ +#pragma once + +#include +#include "mynteye/api/api.h" + +MYNTEYE_BEGIN_NAMESPACE +bool checkFirmwareVersion(const std::shared_ptr api); +MYNTEYE_END_NAMESPACE + +#endif // MYNTEYE_API_VERSION_CHECKER_H_ diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc index 0794aa7..a51555e 100644 --- a/src/mynteye/device/channel/channels.cc +++ b/src/mynteye/device/channel/channels.cc @@ -460,6 +460,7 @@ bool Channels::GetFiles( while (i < end) { std::uint8_t file_id = *(data + i); std::uint16_t file_size = bytes::_from_data(data + i + 1); + LOG(INFO) << "GetFiles:data_size : " << file_size; VLOG(2) << "GetFiles id: " << static_cast(file_id) << ", size: " << file_size; i += 3; diff --git a/src/mynteye/device/channel/file_channel.cc b/src/mynteye/device/channel/file_channel.cc index 5c24777..7a4526e 100644 --- a/src/mynteye/device/channel/file_channel.cc +++ b/src/mynteye/device/channel/file_channel.cc @@ -32,6 +32,7 @@ std::size_t FileChannel::GetDeviceInfoFromData( const std::uint8_t *data, const std::uint16_t &data_size, device_info_t *info) { auto n = dev_info_parser_->GetFromData(data, data_size, info); + LOG(INFO) << "GetDeviceInfoFromData:data_size : " << data_size; auto spec_version = info->spec_version; img_params_parser_->SetSpecVersion(spec_version); imu_params_parser_->SetSpecVersion(spec_version); @@ -113,6 +114,22 @@ std::size_t DeviceInfoParser::GetFromData( info->nominal_baseline = bytes::_from_data(data + i); i += 2; + if (info->spec_version >= Version(1, 2)) { + // auxiliary_chip_version, 2 + info->auxiliary_chip_version.set_major(data[i]); + info->auxiliary_chip_version.set_minor(data[i + 1]); + i += 2; + // isp_version, 2 + info->isp_version.set_major(data[i]); + info->isp_version.set_minor(data[i + 1]); + i += 2; + } else { + info->auxiliary_chip_version.set_major(0); + info->auxiliary_chip_version.set_minor(0); + info->isp_version.set_major(0); + info->isp_version.set_minor(0); + } + // get other infos according to spec_version MYNTEYE_UNUSED(data_size) @@ -155,6 +172,17 @@ std::size_t DeviceInfoParser::SetToData( bytes::_to_data(info->nominal_baseline, data + i); i += 2; + if (info->spec_version >= Version(1, 2)) { + // auxiliary_chip_version, 2 + data[i] = info->auxiliary_chip_version.major(); + data[i + 1] = info->auxiliary_chip_version.minor(); + i += 2; + // isp_version, 2 + data[i] = info->isp_version.major(); + data[i + 1] = info->isp_version.minor(); + i += 2; + } + // set other infos according to spec_version // others @@ -181,7 +209,7 @@ std::size_t ImgParamsParser::GetFromData( return GetFromData_v1_0(data, data_size, img_params); } // s210a old params - if (spec_version_ == Version(1, 1) && data_size == 404) { + if (spec_version_ >= Version(1, 1) && data_size == 404) { return GetFromData_v1_1(data, data_size, img_params); } // get img params with new version format @@ -406,7 +434,7 @@ std::size_t ImuParamsParser::GetFromData( return GetFromData_old(data, data_size, imu_params); } // s210a old params - if (spec_version_ == Version(1, 1) && data_size == 384) { + if (spec_version_ >= Version(1, 1) && data_size == 384) { return GetFromData_old(data, data_size, imu_params); } // get imu params with new version format diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index 1f7a1d4..3b5d42f 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -234,6 +234,10 @@ std::string Device::GetInfo(const Info &info) const { return device_info_->imu_type.to_string(); case Info::NOMINAL_BASELINE: return std::to_string(device_info_->nominal_baseline); + case Info::AUXILIARY_CHIP_VERSION: + return device_info_->auxiliary_chip_version.to_string(); + case Info::ISP_VERSION: + return device_info_->isp_version.to_string(); default: LOG(WARNING) << "Unknown device info"; return ""; @@ -349,6 +353,9 @@ OptionInfo Device::GetOptionInfo(const Option &option) const { std::int32_t Device::GetOptionValue(const Option &option) const { if (!Supports(option)) { + if (option == Option::FRAME_RATE) { + return GetStreamRequest().fps; + } LOG(WARNING) << "Unsupported option: " << option; return -1; } @@ -466,6 +473,11 @@ std::vector Device::GetStreamDatas(const Stream &stream) { return streams_->GetStreamDatas(stream); } +void Device::DisableMotionDatas() { + CHECK_NOTNULL(motions_); + motions_->DisableMotionDatas(); +} + void Device::EnableMotionDatas() { EnableMotionDatas(std::numeric_limits::max()); } diff --git a/src/mynteye/device/motions.cc b/src/mynteye/device/motions.cc index 71c40ab..d7ebf93 100644 --- a/src/mynteye/device/motions.cc +++ b/src/mynteye/device/motions.cc @@ -66,7 +66,10 @@ void Motions::SetMotionCallback(motion_callback_t callback) { std::lock_guard _(mtx_datas_); motion_data_t data = {imu}; - if (motion_datas_enabled_) { + if (motion_datas_enabled_ && motion_datas_max_size_ > 0) { + if (motion_datas_.size() >= motion_datas_max_size_) { + motion_datas_.erase(motion_datas_.begin()); + } motion_datas_.push_back(data); } @@ -98,13 +101,21 @@ void Motions::StopMotionTracking() { } } +void Motions::DisableMotionDatas() { + std::lock_guard _(mtx_datas_); + motion_datas_enabled_ = false; + motion_datas_max_size_ = 0; + motion_datas_.clear(); +} + void Motions::EnableMotionDatas(std::size_t max_size) { if (max_size <= 0) { LOG(WARNING) << "Could not enable motion datas with max_size <= 0"; return; } + std::lock_guard _(mtx_datas_); motion_datas_enabled_ = true; - motion_datas_max_size = max_size; + motion_datas_max_size_ = max_size; } Motions::motion_datas_t Motions::GetMotionDatas() { diff --git a/src/mynteye/device/motions.h b/src/mynteye/device/motions.h index 9bb50d3..8149a68 100644 --- a/src/mynteye/device/motions.h +++ b/src/mynteye/device/motions.h @@ -42,6 +42,7 @@ class Motions { void StartMotionTracking(); void StopMotionTracking(); + void DisableMotionDatas(); void EnableMotionDatas(std::size_t max_size); motion_datas_t GetMotionDatas(); @@ -52,7 +53,7 @@ class Motions { motion_datas_t motion_datas_; bool motion_datas_enabled_; - std::size_t motion_datas_max_size; + std::size_t motion_datas_max_size_; bool is_imu_tracking; diff --git a/src/mynteye/types.cc b/src/mynteye/types.cc index 38747b6..4bc83f1 100644 --- a/src/mynteye/types.cc +++ b/src/mynteye/types.cc @@ -92,6 +92,8 @@ const char *to_string(const Info &value) { CASE(LENS_TYPE) CASE(IMU_TYPE) CASE(NOMINAL_BASELINE) + CASE(AUXILIARY_CHIP_VERSION) + CASE(ISP_VERSION) default: CHECK(is_valid(value)); return "Info::UNKNOWN"; diff --git a/test/types_test.cc b/test/types_test.cc index cc2dfa8..28a292c 100644 --- a/test/types_test.cc +++ b/test/types_test.cc @@ -53,6 +53,8 @@ TEST(Info, VerifyToString) { EXPECT_STREQ("Info::LENS_TYPE", to_string(Info::LENS_TYPE)); EXPECT_STREQ("Info::IMU_TYPE", to_string(Info::IMU_TYPE)); EXPECT_STREQ("Info::NOMINAL_BASELINE", to_string(Info::NOMINAL_BASELINE)); + EXPECT_STREQ("Info::AUXILIARY_CHIP_VERSION", to_string(Info::AUXILIARY_CHIP_VERSION)); + EXPECT_STREQ("Info::ISP_VERSION", to_string(Info::ISP_VERSION)); } TEST(Option, VerifyToString) { diff --git a/tools/writer/config/S210A/device.info b/tools/writer/config/S210A/device.info index 6bce65d..f887695 100644 --- a/tools/writer/config/S210A/device.info +++ b/tools/writer/config/S210A/device.info @@ -2,9 +2,11 @@ --- device_name: MYNT-EYE-S210A serial_number: "07C40D1C0009071F" -firmware_version: "0.1" +firmware_version: "1.1" hardware_version: "1.0" -spec_version: "1.1" -lens_type: "0000" -imu_type: "0000" +spec_version: "1.2" +lens_type: "0001" +imu_type: "0001" nominal_baseline: 0 +auxiliary_chip_version: "1.0" +isp_version: "1.0" diff --git a/tools/writer/device_writer.cc b/tools/writer/device_writer.cc index cb57712..96d9f77 100644 --- a/tools/writer/device_writer.cc +++ b/tools/writer/device_writer.cc @@ -52,7 +52,11 @@ bool DeviceWriter::WriteDeviceInfo(const dev_info_t &info) { << ", spec_version: " << dev_info->spec_version.to_string() << ", lens_type: " << dev_info->lens_type.to_string() << ", imu_type: " << dev_info->imu_type.to_string() - << ", nominal_baseline: " << dev_info->nominal_baseline << "}"; + << ", nominal_baseline: " << dev_info->nominal_baseline + << ", auxiliary_chip_version: " + << dev_info->auxiliary_chip_version.to_string() + << ", isp_version: " + << dev_info->isp_version.to_string()<< "}"; return true; } else { LOG(ERROR) << "Write device info failed"; @@ -215,6 +219,8 @@ bool DeviceWriter::SaveDeviceInfo( fs << "lens_type" << info.lens_type.to_string(); fs << "imu_type" << info.imu_type.to_string(); fs << "nominal_baseline" << info.nominal_baseline; + fs << "auxiliary_chip_version" << info.auxiliary_chip_version.to_string(); + fs << "isp_version" << info.isp_version.to_string(); // save other infos according to spec_version fs.release(); return true; diff --git a/wrappers/python/src/mynteye_py.cc b/wrappers/python/src/mynteye_py.cc index 5c23cf3..b27a044 100644 --- a/wrappers/python/src/mynteye_py.cc +++ b/wrappers/python/src/mynteye_py.cc @@ -104,7 +104,7 @@ struct MYNTEYE_API MotionData { ImuData imu; bool operator==(const MotionData &other) const { - return imu.timestamp == other.imu.timestamp; + return imu.timestamp == other.imu.timestamp; } }; @@ -247,6 +247,8 @@ BOOST_PYTHON_MODULE(mynteye_py) { .value("LENS_TYPE", Info::LENS_TYPE) .value("IMU_TYPE", Info::IMU_TYPE) .value("NOMINAL_BASELINE", Info::NOMINAL_BASELINE) + .value("AUXILIARY_CHIP_VERSION", Info::AUXILIARY_CHIP_VERSION) + .value("ISP_VERSION", Info::ISP_VERSION) #ifdef ENUM_EXPORT_VALUES .export_values() #endif diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch index 8a571db..020e254 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -153,7 +153,7 @@ - + @@ -347,6 +347,10 @@ - 'image_transport/compressedDepth' - + + + - 'image_transport/compressedDepth' + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/scripts/get_device_info.py b/wrappers/ros/src/mynt_eye_ros_wrapper/scripts/get_device_info.py index db4989b..7aeeeed 100755 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/scripts/get_device_info.py +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/scripts/get_device_info.py @@ -54,6 +54,8 @@ def main(): 'LENS_TYPE': GetInfoRequest.LENS_TYPE, 'IMU_TYPE': GetInfoRequest.IMU_TYPE, 'NOMINAL_BASELINE': GetInfoRequest.NOMINAL_BASELINE, + 'AUXILIARY_CHIP_VERSION': GetInfoRequest.AUXILIARY_CHIP_VERSION, + 'ISP_VERSION': GetInfoRequest.ISP_VERSION, } for k, v in get_device_info(**keys).items(): print('{}: {}'.format(k, v)) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index d9048da..75b381b 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -315,7 +315,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { for (auto &&it = mono_topics.begin(); it != mono_topics.end(); ++it) { auto &&topic = mono_topics[it->first]; if (it->first == Stream::LEFT || it->first == Stream::RIGHT) { - mono_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1); + mono_publishers_[it->first] = it_mynteye.advertise(topic, 1); } NODELET_INFO_STREAM("Advertized on topic " << topic); } @@ -407,6 +407,12 @@ class ROSWrapperNodelet : public nodelet::Nodelet { case Request::NOMINAL_BASELINE: res.value = api_->GetInfo(Info::NOMINAL_BASELINE); break; + case Request::AUXILIARY_CHIP_VERSION: + res.value = api_->GetInfo(Info::AUXILIARY_CHIP_VERSION); + break; + case Request::ISP_VERSION: + res.value = api_->GetInfo(Info::ISP_VERSION); + break; case Request::IMG_INTRINSICS: { auto intri_left = api_->GetIntrinsicsBase(Stream::LEFT); @@ -554,75 +560,41 @@ class ROSWrapperNodelet : public nodelet::Nodelet { return true; } - void SetIsPublished(const Stream &stream) { - is_published_[stream] = false; - switch (stream) { - case Stream::LEFT_RECTIFIED: { - if (is_published_[Stream::RIGHT_RECTIFIED]) { - SetIsPublished(Stream::RIGHT_RECTIFIED); - } - if (is_published_[Stream::DISPARITY]) { - SetIsPublished(Stream::DISPARITY); - } - } break; - case Stream::RIGHT_RECTIFIED: { - if (is_published_[Stream::LEFT_RECTIFIED]) { - SetIsPublished(Stream::LEFT_RECTIFIED); - } - if (is_published_[Stream::DISPARITY]) { - SetIsPublished(Stream::DISPARITY); - } - } break; - case Stream::DISPARITY: { - if (is_published_[Stream::DISPARITY_NORMALIZED]) { - SetIsPublished(Stream::DISPARITY_NORMALIZED); - } - if (is_published_[Stream::POINTS]) { - SetIsPublished(Stream::POINTS); - } - } break; - case Stream::DISPARITY_NORMALIZED: { - } break; - case Stream::POINTS: { - if (is_published_[Stream::DEPTH]) { - SetIsPublished(Stream::DEPTH); - } - } break; - case Stream::DEPTH: { - } break; - default: - return; + void publishData( + const Stream &stream, const api::StreamData &data, std::uint32_t seq, + ros::Time stamp) { + if (stream == Stream::LEFT || stream == Stream::RIGHT) { + return; + } else if (stream == Stream::POINTS) { + publishPoints(data, seq, stamp); + } else { + publishCamera(stream, data, seq, stamp); } } - void publishPoint(const Stream &stream) { - auto &&points_num = points_publisher_.getNumSubscribers(); - if (points_num == 0 && is_published_[stream]) { - SetIsPublished(stream); - api_->DisableStreamData(stream); - } else if (points_num > 0 && !is_published_[Stream::POINTS]) { - api_->EnableStreamData(Stream::POINTS); - api_->SetStreamCallback( - Stream::POINTS, [this](const api::StreamData &data) { - // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); - ros::Time stamp = checkUpTimeStamp( - data.img->timestamp, Stream::POINTS); - static std::size_t count = 0; - ++count; - publishPoints(data, count, stamp); - }); - is_published_[Stream::POINTS] = true; + int getStreamSubscribers(const Stream &stream) { + if (stream == Stream::POINTS) { + return points_publisher_.getNumSubscribers(); } + auto pub = camera_publishers_[stream]; + if (pub) + return pub.getNumSubscribers(); + return -1; } void publishOthers(const Stream &stream) { - auto stream_num = camera_publishers_[stream].getNumSubscribers(); - if (stream_num == 0 && is_published_[stream]) { - // Stop computing when was not subcribed - SetIsPublished(stream); - api_->DisableStreamData(stream); - } else if (stream_num > 0 && !is_published_[stream]) { - // Start computing and publishing when was subcribed + // std::cout << stream << "===============================" << std::endl; + // int enable_tag = 0; + // api_->EnableStreamData(stream, [&](const Stream &stream) { + // enable_tag += getStreamSubscribers(stream); + // std::cout << "EnableStreamData callback test" + // << stream << "|| Sum:" + // << getStreamSubscribers(stream) << std::endl; + // }, true); + if (getStreamSubscribers(stream) > 0 && !is_published_[stream]) { + // std::cout << stream + // <<" enableStreamData tag = 0 return" << std::endl; + // std::cout << "enable " << stream << std::endl; api_->EnableStreamData(stream); api_->SetStreamCallback( stream, [this, stream](const api::StreamData &data) { @@ -631,81 +603,121 @@ class ROSWrapperNodelet : public nodelet::Nodelet { data.img->timestamp, stream); static std::size_t count = 0; ++count; - publishCamera(stream, data, count, stamp); + publishData(stream, data, count, stamp); }); is_published_[stream] = true; + return; + } + + int disable_tag = 0; + api_->DisableStreamData(stream, [&](const Stream &stream) { + disable_tag += getStreamSubscribers(stream); + // std::cout << "DisableStreamData callback test: " + // << stream << "|| Sum:"<< getStreamSubscribers(stream) << std::endl; + }, true); + if (disable_tag == 0 && is_published_[stream]) { + api_->DisableStreamData(stream, [&](const Stream &stream) { + // std::cout << "disable " << stream << std::endl; + api_->SetStreamCallback(stream, nullptr); + is_published_[stream] = false; + }); + return; } } void publishTopics() { - if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 || - mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) && - !is_published_[Stream::LEFT]) { - api_->SetStreamCallback( - Stream::LEFT, [&](const api::StreamData &data) { - ++left_count_; - if (left_count_ > 10) { - // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); - ros::Time stamp = checkUpTimeStamp( - data.img->timestamp, Stream::LEFT); + std::vector all_streams{ + Stream::RIGHT, + Stream::LEFT, + Stream::LEFT_RECTIFIED, + Stream::RIGHT_RECTIFIED, + Stream::DISPARITY, + Stream::DISPARITY_NORMALIZED, + Stream::POINTS, + Stream::DEPTH + }; - // static double img_time_prev = -1; - // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION << - // ros_time_beg - // << ", img_time_elapsed: " << FULL_PRECISION - // << ((data.img->timestamp - img_time_beg) * 0.00001f) - // << ", img_time_diff: " << FULL_PRECISION - // << ((img_time_prev < 0) ? 0 - // : (data.img->timestamp - img_time_prev) * 0.01f) << " - // ms"); - // img_time_prev = data.img->timestamp; - publishCamera(Stream::LEFT, data, left_count_, stamp); - publishMono(Stream::LEFT, data, left_count_, stamp); - NODELET_DEBUG_STREAM( - Stream::LEFT << ", count: " << left_count_ - << ", frame_id: " << data.img->frame_id - << ", timestamp: " << data.img->timestamp - << ", exposure_time: " << data.img->exposure_time); - } - }); - left_time_beg_ = ros::Time::now().toSec(); - is_published_[Stream::LEFT] = true; + static int sum = 0; + int sum_c = 0; + for (auto &&stream : all_streams) { + sum_c += getStreamSubscribers(stream); } - if ((camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 || - mono_publishers_[Stream::RIGHT].getNumSubscribers() > 0) && - !is_published_[Stream::RIGHT]) { - api_->SetStreamCallback( - Stream::RIGHT, [&](const api::StreamData &data) { - ++right_count_; - if (right_count_ > 10) { - // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); - ros::Time stamp = checkUpTimeStamp( - data.img->timestamp, Stream::RIGHT); - publishCamera(Stream::RIGHT, data, right_count_, stamp); - publishMono(Stream::RIGHT, data, right_count_, stamp); - NODELET_DEBUG_STREAM( - Stream::RIGHT << ", count: " << right_count_ - << ", frame_id: " << data.img->frame_id - << ", timestamp: " << data.img->timestamp - << ", exposure_time: " << data.img->exposure_time); - } - }); - right_time_beg_ = ros::Time::now().toSec(); - is_published_[Stream::RIGHT] = true; - } - - std::vector other_streams{ - Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, - Stream::DISPARITY, Stream::DISPARITY_NORMALIZED, - Stream::POINTS, Stream::DEPTH}; - - for (auto &&stream : other_streams) { - if (stream != Stream::POINTS) { - publishOthers(stream); + if (sum_c != sum) { + if (sum_c == 0) { + api_->Stop(Source::VIDEO_STREAMING); + for (auto &&stream : all_streams) { + is_published_[stream] = false; + } + api_->Start(Source::VIDEO_STREAMING); } else { - publishPoint(stream); + if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 || + mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) && + !is_published_[Stream::LEFT]) { + api_->SetStreamCallback( + Stream::LEFT, [&](const api::StreamData &data) { + ++left_count_; + if (left_count_ > 10) { + // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); + ros::Time stamp = checkUpTimeStamp( + data.img->timestamp, Stream::LEFT); + + // static double img_time_prev = -1; + // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION << + // ros_time_beg + // << ", img_time_elapsed: " << FULL_PRECISION + // << ((data.img->timestamp - img_time_beg) * 0.00001f) + // << ", img_time_diff: " << FULL_PRECISION + // << ((img_time_prev < 0) ? 0 + // : (data.img->timestamp - img_time_prev) * 0.01f) << " + // ms"); + // img_time_prev = data.img->timestamp; + publishCamera(Stream::LEFT, data, left_count_, stamp); + publishMono(Stream::LEFT, data, left_count_, stamp); + NODELET_DEBUG_STREAM( + Stream::LEFT << ", count: " << left_count_ + << ", frame_id: " << data.img->frame_id + << ", timestamp: " << data.img->timestamp + << ", exposure_time: " << data.img->exposure_time); + } + }); + left_time_beg_ = ros::Time::now().toSec(); + is_published_[Stream::LEFT] = true; + } + + if ((camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 || + mono_publishers_[Stream::RIGHT].getNumSubscribers() > 0) && + !is_published_[Stream::RIGHT]) { + api_->SetStreamCallback( + Stream::RIGHT, [&](const api::StreamData &data) { + ++right_count_; + if (right_count_ > 10) { + // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); + ros::Time stamp = checkUpTimeStamp( + data.img->timestamp, Stream::RIGHT); + publishCamera(Stream::RIGHT, data, right_count_, stamp); + publishMono(Stream::RIGHT, data, right_count_, stamp); + NODELET_DEBUG_STREAM( + Stream::RIGHT << ", count: " << right_count_ + << ", frame_id: " << data.img->frame_id + << ", timestamp: " << data.img->timestamp + << ", exposure_time: " << data.img->exposure_time); + } + }); + right_time_beg_ = ros::Time::now().toSec(); + is_published_[Stream::RIGHT] = true; + } + + std::vector other_streams{ + Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, + Stream::DISPARITY, Stream::DISPARITY_NORMALIZED, + Stream::POINTS, Stream::DEPTH + }; + for (auto &&stream : other_streams) { + publishOthers(stream); + } } + sum = sum_c; } if (!is_motion_published_) { @@ -822,9 +834,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { cv::cvtColor(data.frame, mono, CV_RGB2GRAY); auto &&msg = cv_bridge::CvImage(header, enc::MONO8, mono).toImageMsg(); pthread_mutex_unlock(&mutex_data_); - auto &&info = getCameraInfo(stream); - info->header.stamp = msg->header.stamp; - mono_publishers_[stream].publish(msg, info); + mono_publishers_[stream].publish(msg); } void publishPoints( @@ -1284,7 +1294,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet { } for (int i = 0; i < p.rows; i++) { for (int j = 0; j < p.cols; j++) { - camera_info->P.at(i * p.cols + j) = p.at(i, j); + int scale = (i == 2 && j == 2)?1:1000; + camera_info->P.at(i * p.cols + j) = p.at(i, j) / scale; } } @@ -1510,7 +1521,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { std::map image_encodings_; // mono: LEFT, RIGHT - std::map mono_publishers_; + std::map mono_publishers_; // pointcloud: POINTS ros::Publisher points_publisher_; diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/srv/GetInfo.srv b/wrappers/ros/src/mynt_eye_ros_wrapper/srv/GetInfo.srv index 3e2efe3..a917450 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/srv/GetInfo.srv +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/srv/GetInfo.srv @@ -6,10 +6,12 @@ uint32 SPEC_VERSION=4 uint32 LENS_TYPE=5 uint32 IMU_TYPE=6 uint32 NOMINAL_BASELINE=7 -uint32 IMG_INTRINSICS=8 -uint32 IMG_EXTRINSICS_RTOL=9 -uint32 IMU_INTRINSICS=10 -uint32 IMU_EXTRINSICS=11 +uint32 AUXILIARY_CHIP_VERSION=8 +uint32 ISP_VERSION=9 +uint32 IMG_INTRINSICS=10 +uint32 IMG_EXTRINSICS_RTOL=11 +uint32 IMU_INTRINSICS=12 +uint32 IMU_EXTRINSICS=13 uint32 key --- string value