From a22eed8c1d7367a27b1457936b741b93d902deec Mon Sep 17 00:00:00 2001 From: TinyOh Date: Wed, 20 Feb 2019 16:12:38 +0800 Subject: [PATCH 01/28] fix(windows): ProcessNativeStream change --- src/mynteye/api/synthetic.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index b68cfd4..a73d280 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -584,10 +584,13 @@ void Synthetic::ProcessNativeStream( const Stream &stream, const api::StreamData &data) { if (stream == Stream::LEFT || stream == Stream::RIGHT) { static api::StreamData left_data, right_data; + cv::Mat tmp1, tmp2; if (stream == Stream::LEFT) { left_data = data; + tmp1 = left_data.frame.clone(); } else if (stream == Stream::RIGHT) { right_data = data; + tmp2 = right_data.frame.clone(); } if (left_data.img && right_data.img && left_data.img->frame_id == right_data.img->frame_id) { @@ -604,8 +607,8 @@ void Synthetic::ProcessNativeStream( 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}); + tmp1, left_data.frame_id, left_data.img, + tmp2, right_data.frame_id, right_data.img}); } return; } From 055c6a23d079a3b877e6596f1e5a22041a9745b7 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Wed, 20 Feb 2019 16:31:32 +0800 Subject: [PATCH 02/28] fix(thread): ProcessNativeStream --- src/mynteye/api/synthetic.cc | 8 +++----- src/mynteye/api/synthetic.h | 2 ++ 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index a73d280..1830e67 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -583,14 +583,12 @@ void Synthetic::InitProcessors() { void Synthetic::ProcessNativeStream( const Stream &stream, const api::StreamData &data) { if (stream == Stream::LEFT || stream == Stream::RIGHT) { + std::unique_lock lk(mtx_left_right_ready_); static api::StreamData left_data, right_data; - cv::Mat tmp1, tmp2; if (stream == Stream::LEFT) { left_data = data; - tmp1 = left_data.frame.clone(); } else if (stream == Stream::RIGHT) { right_data = data; - tmp2 = right_data.frame.clone(); } if (left_data.img && right_data.img && left_data.img->frame_id == right_data.img->frame_id) { @@ -607,8 +605,8 @@ void Synthetic::ProcessNativeStream( processor = find_processor(processor_); } processor->Process(ObjMat2{ - tmp1, left_data.frame_id, left_data.img, - tmp2, right_data.frame_id, right_data.img}); + left_data.frame, left_data.frame_id, left_data.img, + right_data.frame, right_data.frame_id, right_data.img}); } return; } diff --git a/src/mynteye/api/synthetic.h b/src/mynteye/api/synthetic.h index e39088b..29a9548 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" @@ -132,6 +133,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_; From fd8616f475bf1ff563a92ff9d5512cc82bd7562f Mon Sep 17 00:00:00 2001 From: John Zhao Date: Thu, 21 Feb 2019 22:17:53 +0800 Subject: [PATCH 03/28] fix(motions): fix max size and add disable method --- src/mynteye/device/motions.cc | 15 +++++++++++++-- src/mynteye/device/motions.h | 3 ++- 2 files changed, 15 insertions(+), 3 deletions(-) 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; From 0fb3610744b0d393af47bc2aad6cbbf3f3673f65 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Thu, 21 Feb 2019 22:19:52 +0800 Subject: [PATCH 04/28] feat(device): add disable motion datas --- include/mynteye/device/device.h | 4 ++++ src/mynteye/device/device.cc | 5 +++++ 2 files changed, 9 insertions(+) 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/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index 344eea7..651baab 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -466,6 +466,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()); } From c6aa8d93caed5a9a5ee0f700b6c2cd580024854a Mon Sep 17 00:00:00 2001 From: John Zhao Date: Thu, 21 Feb 2019 22:21:17 +0800 Subject: [PATCH 05/28] feat(synthetic): add stream data listener --- src/mynteye/api/synthetic.cc | 117 +++++++++++++++++++++++------------ src/mynteye/api/synthetic.h | 8 +++ 2 files changed, 84 insertions(+), 41 deletions(-) diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index 1830e67..99ee18a 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); @@ -335,7 +378,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 +392,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++; @@ -582,6 +619,7 @@ void Synthetic::InitProcessors() { 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; @@ -604,9 +642,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; } @@ -628,34 +664,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 @@ -666,13 +696,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 @@ -738,51 +766,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)); } } @@ -797,4 +825,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 29a9548..5de5bca 100644 --- a/src/mynteye/api/synthetic.h +++ b/src/mynteye/api/synthetic.h @@ -35,6 +35,8 @@ struct Object; class Synthetic { public: using stream_callback_t = API::stream_callback_t; + using stream_data_listener_t = + std::function; typedef enum Mode { MODE_NATIVE, // Native stream @@ -52,6 +54,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; @@ -126,6 +130,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_; @@ -141,6 +147,8 @@ class Synthetic { bool calib_default_tag_; std::vector> processors_; + + stream_data_listener_t stream_data_listener_; }; class SyntheticProcessorPart { From 73cca48f57b5793c2ca830f951c431582222fc74 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Thu, 21 Feb 2019 22:37:55 +0800 Subject: [PATCH 06/28] feat(api): add timestamp correspondence --- CMakeLists.txt | 1 + include/mynteye/api/api.h | 10 +++ samples/tutorials/CMakeLists.txt | 4 + .../tutorials/data/get_imu_correspondence.cc | 77 +++++++++++++++++++ src/mynteye/api/api.cc | 48 ++++++++++-- src/mynteye/api/correspondence.cc | 64 +++++++++++++++ src/mynteye/api/correspondence.h | 49 ++++++++++++ 7 files changed, 245 insertions(+), 8 deletions(-) create mode 100644 samples/tutorials/data/get_imu_correspondence.cc create mode 100644 src/mynteye/api/correspondence.cc create mode 100644 src/mynteye/api/correspondence.h diff --git a/CMakeLists.txt b/CMakeLists.txt index cc2c310..de270c0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -219,6 +219,7 @@ 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 ) if(WITH_CAM_MODELS) list(APPEND MYNTEYE_SRCS diff --git a/include/mynteye/api/api.h b/include/mynteye/api/api.h index ea7a27f..caece74 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; @@ -305,6 +306,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 +323,10 @@ class MYNTEYE_API API { std::unique_ptr synthetic_; + std::unique_ptr correspondence_; + + motion_callback_t callback_; + void CheckImageParams(); }; 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/data/get_imu_correspondence.cc b/samples/tutorials/data/get_imu_correspondence.cc new file mode 100644 index 0000000..a38b459 --- /dev/null +++ b/samples/tutorials/data/get_imu_correspondence.cc @@ -0,0 +1,77 @@ +// 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"); + + while (true) { + api->WaitForStreams(); + + auto &&left_data = api->GetStreamData(Stream::LEFT); + auto &&right_data = api->GetStreamData(Stream::RIGHT); + + LOG(INFO) << "Img frame_id: " << left_data.img->frame_id + << ", timestamp: " << left_data.img->timestamp; + + cv::Mat img; + cv::hconcat(left_data.frame, right_data.frame, img); + + auto &&motion_datas = api->GetMotionDatas(); + for (auto &&data : motion_datas) { + LOG(INFO) << "Imu frame_id: " << data.imu->frame_id + << ", timestamp: " << data.imu->timestamp; + } + 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..136cac9 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -22,6 +22,7 @@ #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" @@ -208,7 +209,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)); @@ -377,10 +378,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); } @@ -455,15 +461,41 @@ std::vector API::GetStreamDatas(const Stream &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..344e7e7 --- /dev/null +++ b/src/mynteye/api/correspondence.cc @@ -0,0 +1,64 @@ +// 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) { + VLOG(2) << __func__; +} + +Correspondence::~Correspondence() { + VLOG(2) << __func__; +} + +void Correspondence::OnStreamDataCallback( + const Stream &stream, const api::StreamData &data) { + // LOG(INFO) << __func__ << ", " << stream + // << ", id: " << data.frame_id << ", stamp: " << data.img->timestamp; +} + +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; +} + +std::vector Correspondence::GetMotionDatas() { + std::lock_guard _(mtx_motion_datas_); + std::vector datas; + for (auto &&data : motion_datas_) { + datas.push_back({data.imu}); + } + motion_datas_.clear(); + return datas; +} + +MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/correspondence.h b/src/mynteye/api/correspondence.h new file mode 100644 index 0000000..3191e36 --- /dev/null +++ b/src/mynteye/api/correspondence.h @@ -0,0 +1,49 @@ +// 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 "mynteye/api.h" +#include "mynteye/device/callbacks.h" + +MYNTEYE_BEGIN_NAMESPACE + +class Correspondence { + public: + Correspondence(const std::shared_ptr &device, const Stream &stream); + ~Correspondence(); + + void OnStreamDataCallback(const Stream &stream, const api::StreamData &data); + void OnMotionDataCallback(const device::MotionData &data); + + void SetMotionCallback(API::motion_callback_t callback); + + std::vector GetMotionDatas(); + + private: + std::shared_ptr device_; + Stream stream_; + API::motion_callback_t motion_callback_; + std::vector motion_datas_; + std::mutex mtx_motion_datas_; +}; + +MYNTEYE_END_NAMESPACE + +#endif // MYNTEYE_API_CONFIG_H_ From ab52fd5280a550bb519495c8cbfe27d8c01cfd9d Mon Sep 17 00:00:00 2001 From: John Zhao Date: Fri, 22 Feb 2019 12:15:18 +0800 Subject: [PATCH 07/28] feat(correspondence): wait img when imu correspondence --- src/mynteye/api/api.cc | 18 +++- src/mynteye/api/correspondence.cc | 141 +++++++++++++++++++++++++++++- src/mynteye/api/correspondence.h | 27 +++++- 3 files changed, 180 insertions(+), 6 deletions(-) diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index 136cac9..d52f198 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -441,7 +441,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) { @@ -453,11 +457,19 @@ void API::DisableStreamData(const Stream &stream) { } 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) { diff --git a/src/mynteye/api/correspondence.cc b/src/mynteye/api/correspondence.cc index 344e7e7..5da3030 100644 --- a/src/mynteye/api/correspondence.cc +++ b/src/mynteye/api/correspondence.cc @@ -22,23 +22,59 @@ Correspondence::Correspondence(const std::shared_ptr &device, const Stream &stream) : device_(device), stream_(stream) { 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) { // LOG(INFO) << __func__ << ", " << stream // << ", id: " << data.frame_id << ", stamp: " << data.img->timestamp; + if (!Watch(stream)) { + return; // unwatched + } + std::lock_guard _(mtx_stream_datas_); + if (stream == stream_) { + stream_datas_.push_back(data); + } else if (/*stream_match_enabled_ && */stream == stream_match_) { + stream_datas_match_.push_back(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_); + std::lock_guard _(mtx_motion_datas_); motion_datas_.push_back(data); } if (motion_callback_) { @@ -51,8 +87,60 @@ void Correspondence::SetMotionCallback(API::motion_callback_t callback) { 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 = std::move(stream_datas_); + + 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 = std::move(stream_datas_match_); + + if (stream_match_count_ < 10) { + ++stream_match_count_; + } + + return datas; + } + + return {}; +} + std::vector Correspondence::GetMotionDatas() { - std::lock_guard _(mtx_motion_datas_); + std::lock_guard _(mtx_motion_datas_); std::vector datas; for (auto &&data : motion_datas_) { datas.push_back({data.imu}); @@ -61,4 +149,53 @@ std::vector Correspondence::GetMotionDatas() { return datas; } +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 (motion_datas_.empty()) return false; + + std::uint64_t img_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; + } + 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; + } + + return img_stamp + stream_interval_us_half_ < imu_stamp; +} + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/correspondence.h b/src/mynteye/api/correspondence.h index 3191e36..c415731 100644 --- a/src/mynteye/api/correspondence.h +++ b/src/mynteye/api/correspondence.h @@ -15,6 +15,7 @@ #define MYNTEYE_API_CONFIG_H_ #pragma once +#include #include #include #include @@ -29,19 +30,43 @@ class Correspondence { 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::shared_ptr device_; Stream stream_; + Stream stream_match_; + bool stream_match_enabled_; + + float stream_interval_us_; + float stream_interval_us_half_; + API::motion_callback_t motion_callback_; std::vector motion_datas_; - std::mutex mtx_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_; }; MYNTEYE_END_NAMESPACE From 85acd7b91557086b14ad25769fb8067ba6ab4547 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Fri, 22 Feb 2019 14:00:23 +0800 Subject: [PATCH 08/28] feat(correspondence): add get correspondence data --- .../tutorials/data/get_imu_correspondence.cc | 16 +++- src/mynteye/api/correspondence.cc | 88 ++++++++++++++++--- src/mynteye/api/correspondence.h | 5 ++ src/mynteye/device/device.cc | 3 + 4 files changed, 94 insertions(+), 18 deletions(-) diff --git a/samples/tutorials/data/get_imu_correspondence.cc b/samples/tutorials/data/get_imu_correspondence.cc index a38b459..9cec957 100644 --- a/samples/tutorials/data/get_imu_correspondence.cc +++ b/samples/tutorials/data/get_imu_correspondence.cc @@ -38,22 +38,30 @@ int main(int argc, char *argv[]) { 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); - LOG(INFO) << "Img frame_id: " << left_data.img->frame_id - << ", timestamp: " << left_data.img->timestamp; + 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) { - LOG(INFO) << "Imu frame_id: " << data.imu->frame_id - << ", timestamp: " << data.imu->timestamp; + 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); diff --git a/src/mynteye/api/correspondence.cc b/src/mynteye/api/correspondence.cc index 5da3030..30f9176 100644 --- a/src/mynteye/api/correspondence.cc +++ b/src/mynteye/api/correspondence.cc @@ -20,7 +20,7 @@ MYNTEYE_BEGIN_NAMESPACE Correspondence::Correspondence(const std::shared_ptr &device, const Stream &stream) - : device_(device), 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 @@ -56,16 +56,19 @@ bool Correspondence::Watch(const Stream &stream) const { void Correspondence::OnStreamDataCallback( const Stream &stream, const api::StreamData &data) { - // LOG(INFO) << __func__ << ", " << stream - // << ", id: " << data.frame_id << ", stamp: " << data.img->timestamp; 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(data); + stream_datas_.push_back(std::move(data)); } else if (/*stream_match_enabled_ && */stream == stream_match_) { - stream_datas_match_.push_back(data); + stream_datas_match_.push_back(std::move(data)); } NotifyStreamDataReady(); } @@ -114,7 +117,7 @@ std::vector Correspondence::GetStreamDatas( static std::uint32_t stream_match_count_ = 0; if (stream == stream_) { - auto datas = std::move(stream_datas_); + auto datas = GetReadyStreamData(false); if (stream_count_ < 10) { ++stream_count_; @@ -127,7 +130,7 @@ std::vector Correspondence::GetStreamDatas( return datas; } else if (/*stream_match_enabled_ && */stream == stream_match_) { - auto datas = std::move(stream_datas_match_); + auto datas = GetReadyStreamData(true); if (stream_match_count_ < 10) { ++stream_match_count_; @@ -140,13 +143,7 @@ std::vector Correspondence::GetStreamDatas( } std::vector Correspondence::GetMotionDatas() { - std::lock_guard _(mtx_motion_datas_); - std::vector datas; - for (auto &&data : motion_datas_) { - datas.push_back({data.imu}); - } - motion_datas_.clear(); - return datas; + return GetReadyMotionDatas(); } void Correspondence::EnableStreamMatch() { @@ -198,4 +195,67 @@ bool Correspondence::IsStreamDataReady() { 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!"; + return {}; + } + 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 index c415731..3f69874 100644 --- a/src/mynteye/api/correspondence.h +++ b/src/mynteye/api/correspondence.h @@ -51,6 +51,9 @@ class Correspondence { bool IsStreamDataReady(); + std::vector GetReadyStreamData(bool matched); + std::vector GetReadyMotionDatas(); + std::shared_ptr device_; Stream stream_; Stream stream_match_; @@ -67,6 +70,8 @@ class Correspondence { 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 diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index 651baab..497d5cd 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -349,6 +349,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; } From 3796c190408a355848100e54b74078ae54878f5d Mon Sep 17 00:00:00 2001 From: TinyOh Date: Fri, 22 Feb 2019 18:20:47 +0800 Subject: [PATCH 09/28] fix(ros): record close bug --- include/mynteye/api/api.h | 22 ++ src/mynteye/api/api.cc | 11 + src/mynteye/api/synthetic.cc | 72 +++-- src/mynteye/api/synthetic.h | 6 + .../launch/mynteye.launch | 6 +- .../src/wrapper_nodelet.cc | 258 +++++++++--------- 6 files changed, 223 insertions(+), 152 deletions(-) diff --git a/include/mynteye/api/api.h b/include/mynteye/api/api.h index ea7a27f..e9b3cc1 100644 --- a/include/mynteye/api/api.h +++ b/include/mynteye/api/api.h @@ -91,6 +91,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 +282,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. */ diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index 18e80cd..0f6801e 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -446,6 +446,17 @@ 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); } diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index 1830e67..fc7771e 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -195,27 +195,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 +207,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 +255,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); diff --git a/src/mynteye/api/synthetic.h b/src/mynteye/api/synthetic.h index 29a9548..fbd80ac 100644 --- a/src/mynteye/api/synthetic.h +++ b/src/mynteye/api/synthetic.h @@ -35,6 +35,7 @@ struct Object; class Synthetic { public: using stream_callback_t = API::stream_callback_t; + using stream_switch_callback_t = API::stream_switch_callback_t; typedef enum Mode { MODE_NATIVE, // Native stream @@ -59,6 +60,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); 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..d6ada67 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -347,6 +347,10 @@ - 'image_transport/compressedDepth' - + + + - 'image_transport/compressedDepth' + + 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..72b5541 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 @@ -554,75 +554,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 +597,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_) { From 84145cd35cc1747d61252a391139497971d9d10c Mon Sep 17 00:00:00 2001 From: John Zhao Date: Fri, 22 Feb 2019 14:40:03 +0800 Subject: [PATCH 10/28] fix(correspondence): fix include header --- src/mynteye/api/correspondence.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mynteye/api/correspondence.h b/src/mynteye/api/correspondence.h index 3f69874..e2cbd98 100644 --- a/src/mynteye/api/correspondence.h +++ b/src/mynteye/api/correspondence.h @@ -20,7 +20,7 @@ #include #include -#include "mynteye/api.h" +#include "mynteye/api/api.h" #include "mynteye/device/callbacks.h" MYNTEYE_BEGIN_NAMESPACE From 6d0fa3a3ca7697597bf829a72e365d6a09d2e301 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Fri, 22 Feb 2019 14:57:20 +0800 Subject: [PATCH 11/28] fix(correspondence): improve warning if not start motion tracking --- src/mynteye/api/correspondence.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/mynteye/api/correspondence.cc b/src/mynteye/api/correspondence.cc index 30f9176..6ebc7da 100644 --- a/src/mynteye/api/correspondence.cc +++ b/src/mynteye/api/correspondence.cc @@ -200,8 +200,11 @@ std::vector Correspondence::GetReadyStreamData(bool matched) { { std::lock_guard _(mtx_motion_datas_); if (motion_datas_.empty()) { - LOG(WARNING) << "motion data is unexpected empty!"; - return {}; + 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; } From 7bc74f4d1ba45c2d92706f440adeacd41000bcfe Mon Sep 17 00:00:00 2001 From: kalman Date: Fri, 22 Feb 2019 15:09:06 +0800 Subject: [PATCH 12/28] fix(ros): fix camera info bug --- .../ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) 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 72b5541..c1da91b 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 @@ -623,11 +623,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet { std::vector all_streams{ Stream::RIGHT, Stream::LEFT, - Stream::LEFT_RECTIFIED, + Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, - Stream::DISPARITY, + Stream::DISPARITY, Stream::DISPARITY_NORMALIZED, - Stream::POINTS, + Stream::POINTS, Stream::DEPTH }; @@ -1290,7 +1290,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; } } From 3bda52bc795bd332c1fb0dd60e6f346fe2f4607f Mon Sep 17 00:00:00 2001 From: kalman Date: Fri, 22 Feb 2019 15:14:45 +0800 Subject: [PATCH 13/28] fix(samples): delete useless comment --- samples/tutorials/control/framerate.cc | 4 ---- 1 file changed, 4 deletions(-) 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 From 3644a7e530edaa7cc66cfb60f9ce646319e0d4f6 Mon Sep 17 00:00:00 2001 From: kalman Date: Fri, 22 Feb 2019 20:11:55 +0800 Subject: [PATCH 14/28] chore(doc): update version --- CMakeLists.txt | 2 +- doc/en/api.doxyfile | 2 +- doc/zh-Hans/api.doxyfile | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index de270c0..cf8e0aa 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.1 LANGUAGES C CXX) include(cmake/Common.cmake) diff --git a/doc/en/api.doxyfile b/doc/en/api.doxyfile index 9a6bb26..9551550 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.1 # 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/api.doxyfile b/doc/zh-Hans/api.doxyfile index 312be3e..f4c7e70 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.1 # 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 From 8d54b7844d8323caf16e77c83c3e9c44d8ba8fbe Mon Sep 17 00:00:00 2001 From: kalman Date: Fri, 22 Feb 2019 21:13:30 +0800 Subject: [PATCH 15/28] chore(readme): update readme --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index e5b4fb6..9d9cd89 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.1-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/2893965/mynt-eye-s-sdk-apidoc-2.3.1-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2893979/mynt-eye-s-sdk-apidoc-2.3.1-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/2893985/mynt-eye-s-sdk-apidoc-2.3.1-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2893986/mynt-eye-s-sdk-apidoc-2.3.1-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.1-en/mynt-eye-s-sdk-apidoc-2.3.1-en/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/2893975/mynt-eye-s-sdk-guide-2.3.1-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2893996/mynt-eye-s-sdk-guide-2.3.1-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/2893997/mynt-eye-s-sdk-guide-2.3.1-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2893997/mynt-eye-s-sdk-guide-2.3.1-zh-Hans.pdf) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/sdk/mynt-eye-s-sdk-guide-2.3.1-zh-Hans/mynt-eye-s-sdk-guide-2.3.1-zh-Hans/index.html) > Supported languages: `en`, `zh-Hans`. From e6d2d3ebe473e24345d7534e5dbb567326dfb61b Mon Sep 17 00:00:00 2001 From: kalman Date: Fri, 22 Feb 2019 21:31:13 +0800 Subject: [PATCH 16/28] chore(readme): chore(readme): update readme n --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 9d9cd89..6059be3 100644 --- a/README.md +++ b/README.md @@ -18,7 +18,7 @@ Please follow the guide doc to install the SDK on different platforms. * [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/2893965/mynt-eye-s-sdk-apidoc-2.3.1-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2893979/mynt-eye-s-sdk-apidoc-2.3.1-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/2893985/mynt-eye-s-sdk-apidoc-2.3.1-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2893986/mynt-eye-s-sdk-apidoc-2.3.1-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.1-en/mynt-eye-s-sdk-apidoc-2.3.1-en/index.html) + * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2893985/mynt-eye-s-sdk-apidoc-2.3.1-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2893986/mynt-eye-s-sdk-apidoc-2.3.1-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.1-zh-Hans/mynt-eye-s-sdk-apidoc-2.3.1-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/2893975/mynt-eye-s-sdk-guide-2.3.1-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2893996/mynt-eye-s-sdk-guide-2.3.1-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/2893997/mynt-eye-s-sdk-guide-2.3.1-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2893997/mynt-eye-s-sdk-guide-2.3.1-zh-Hans.pdf) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/sdk/mynt-eye-s-sdk-guide-2.3.1-zh-Hans/mynt-eye-s-sdk-guide-2.3.1-zh-Hans/index.html) From 23d77d0de3dc35078202fa57135603941d4dff73 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Sat, 23 Feb 2019 00:42:07 +0800 Subject: [PATCH 17/28] fix(record): shield diable logic temporarily --- wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c1da91b..434e194 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 @@ -644,7 +644,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { is_published_[stream] = false; } api_->Start(Source::VIDEO_STREAMING); - } else { + } else if (sum_c > sum) { if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 || mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) && !is_published_[Stream::LEFT]) { From 5b5ded25c9207c5e3bc1d46535771adcbc578503 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Mon, 25 Feb 2019 12:09:48 +0800 Subject: [PATCH 18/28] fix(correspondence): also wait stream matched ready --- src/mynteye/api/correspondence.cc | 15 ++++++++++++++- src/mynteye/api/correspondence.h | 3 ++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/mynteye/api/correspondence.cc b/src/mynteye/api/correspondence.cc index 6ebc7da..d4af6cd 100644 --- a/src/mynteye/api/correspondence.cc +++ b/src/mynteye/api/correspondence.cc @@ -171,9 +171,13 @@ void Correspondence::NotifyStreamDataReady() { 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(); @@ -181,6 +185,10 @@ bool Correspondence::IsStreamDataReady() { 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; { @@ -192,7 +200,12 @@ bool Correspondence::IsStreamDataReady() { imu_stamp = data.imu->timestamp; } - return img_stamp + stream_interval_us_half_ < imu_stamp; + 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) { diff --git a/src/mynteye/api/correspondence.h b/src/mynteye/api/correspondence.h index e2cbd98..df822e0 100644 --- a/src/mynteye/api/correspondence.h +++ b/src/mynteye/api/correspondence.h @@ -15,6 +15,7 @@ #define MYNTEYE_API_CONFIG_H_ #pragma once +#include #include #include #include @@ -57,7 +58,7 @@ class Correspondence { std::shared_ptr device_; Stream stream_; Stream stream_match_; - bool stream_match_enabled_; + std::atomic_bool stream_match_enabled_; float stream_interval_us_; float stream_interval_us_half_; From 30ed3ed5e2fbf1d8af6b29c013908aaad37856b0 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Mon, 25 Feb 2019 12:12:19 +0800 Subject: [PATCH 19/28] build(makefile): ensure uninstall before install --- Makefile | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 From b6d40373578167a67f17838a90da34958274c3ce Mon Sep 17 00:00:00 2001 From: kalman Date: Tue, 26 Feb 2019 14:59:28 +0800 Subject: [PATCH 20/28] fix(wrapper): fix camera info repeat bug --- .../ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) 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 434e194..62a5945 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); } @@ -828,9 +828,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( @@ -1517,7 +1515,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_; From 262f64715dee9eda6fe09d0c9cb8623ce0bbe236 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Wed, 27 Feb 2019 08:14:37 +0800 Subject: [PATCH 21/28] feat(api): version check --- CMakeLists.txt | 3 +- include/mynteye/types.h | 2 + src/mynteye/api/api.cc | 24 +++++++++- src/mynteye/api/version_checker.cc | 48 +++++++++++++++++++ src/mynteye/api/version_checker.h | 25 ++++++++++ .../src/wrapper_nodelet.cc | 2 +- 6 files changed, 100 insertions(+), 4 deletions(-) create mode 100644 src/mynteye/api/version_checker.cc create mode 100644 src/mynteye/api/version_checker.h diff --git a/CMakeLists.txt b/CMakeLists.txt index de270c0..5c0623a 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.1 LANGUAGES C CXX) include(cmake/Common.cmake) @@ -220,6 +220,7 @@ if(WITH_API) 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/include/mynteye/types.h b/include/mynteye/types.h index b95d228..627f5d1 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -122,6 +122,8 @@ enum class Info : std::uint8_t { IMU_TYPE, /** Nominal baseline */ NOMINAL_BASELINE, + /** SDK version*/ + SDK_VERSION, /** Last guard */ LAST }; diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index b04a373..5b440a7 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -26,6 +26,7 @@ #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" @@ -222,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( @@ -261,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; } @@ -324,6 +328,22 @@ 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"; + } + std::string vs_main = fs["MYNTEYE_VERSION"]; + int vs_tweak = fs["MYNTEYE_VERSION_TWEAK"]; + return vs_main + std::string(".") + std::to_string(vs_tweak); + } + return device_->GetInfo(info); } diff --git a/src/mynteye/api/version_checker.cc b/src/mynteye/api/version_checker.cc new file mode 100644 index 0000000..363deaf --- /dev/null +++ b/src/mynteye/api/version_checker.cc @@ -0,0 +1,48 @@ +// 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; +}firmware_version_match_table_unit; + +/** firmware/sdk version matched table */ +static const firmware_version_match_table_unit FSVM_TABLE[] ={ + {"MYNT-EYE-S1030", "2.3.1.0", "2.0.0"}, + {"MYNT-EYE-S1030", "2.3.1.0", "2.0.0"}, + {} +}; + +bool checkFirmwareVersion(const std::shared_ptr api) { + LOG(INFO) << "SDK version: " << api->GetInfo(Info::SDK_VERSION); + LOG(INFO) << "Device name: " << api->GetInfo(Info::DEVICE_NAME); + LOG(INFO) << "Serial number: " << api->GetInfo(Info::SERIAL_NUMBER); + LOG(INFO) << "Firmware version: " << api->GetInfo(Info::FIRMWARE_VERSION); + LOG(INFO) << "Hardware version: " << api->GetInfo(Info::HARDWARE_VERSION); + LOG(INFO) << "Spec version: " << api->GetInfo(Info::SPEC_VERSION); + 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); + return true; +} + +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/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index 434e194..c1da91b 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 @@ -644,7 +644,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { is_published_[stream] = false; } api_->Start(Source::VIDEO_STREAMING); - } else if (sum_c > sum) { + } else { if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 || mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) && !is_published_[Stream::LEFT]) { From b2bd90192dfb9183c0d2b65be23352688782eac4 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Thu, 28 Feb 2019 03:53:50 +0800 Subject: [PATCH 22/28] feat(api) sdk/firmware version check --- src/mynteye/api/api.cc | 4 +- src/mynteye/api/version_checker.cc | 123 +++++++++++++++++++++++++---- 2 files changed, 108 insertions(+), 19 deletions(-) diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index 5b440a7..3f034be 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -339,9 +339,7 @@ std::string API::GetInfo(const Info &info) const { LOG(WARNING) << "build.info not found: " << info_path; return "null"; } - std::string vs_main = fs["MYNTEYE_VERSION"]; - int vs_tweak = fs["MYNTEYE_VERSION_TWEAK"]; - return vs_main + std::string(".") + std::to_string(vs_tweak); + return fs["MYNTEYE_VERSION"]; } return device_->GetInfo(info); diff --git a/src/mynteye/api/version_checker.cc b/src/mynteye/api/version_checker.cc index 363deaf..f39c6d5 100644 --- a/src/mynteye/api/version_checker.cc +++ b/src/mynteye/api/version_checker.cc @@ -19,29 +19,120 @@ MYNTEYE_BEGIN_NAMESPACE typedef struct { - const std::string device_type; - const std::string sdk_version; - const std::string firmware_version; + 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[] ={ - {"MYNT-EYE-S1030", "2.3.1.0", "2.0.0"}, - {"MYNT-EYE-S1030", "2.3.1.0", "2.0.0"}, - {} +/** S1030 */ + {"MYNT-EYE-S1030", ">2.3.0", "2.3.0", PASS_DESCRIPTION}, + {"MYNT-EYE-S1030", ">2.3.0", "2.2.2", 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", ">0.0.0", "1.0", PASS_DESCRIPTION}, +/** S210A */ + {"MYNT-EYE-S210A", ">0.0.0", "1.0", PASS_DESCRIPTION}, }; +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) { - LOG(INFO) << "SDK version: " << api->GetInfo(Info::SDK_VERSION); - LOG(INFO) << "Device name: " << api->GetInfo(Info::DEVICE_NAME); - LOG(INFO) << "Serial number: " << api->GetInfo(Info::SERIAL_NUMBER); - LOG(INFO) << "Firmware version: " << api->GetInfo(Info::FIRMWARE_VERSION); - LOG(INFO) << "Hardware version: " << api->GetInfo(Info::HARDWARE_VERSION); - LOG(INFO) << "Spec version: " << api->GetInfo(Info::SPEC_VERSION); - 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); - return true; + 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 From d09f037aee103f3ca650f817be5bab7448608ab8 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Thu, 28 Feb 2019 04:16:59 +0800 Subject: [PATCH 23/28] fix: change cmake version to 2.3.2 --- CMakeLists.txt | 2 +- src/mynteye/api/api.cc | 7 ++++--- src/mynteye/api/version_checker.cc | 3 +-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c0623a..ace55a8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,7 @@ cmake_minimum_required(VERSION 3.0) -project(mynteye VERSION 2.3.1 LANGUAGES C CXX) +project(mynteye VERSION 2.3.2 LANGUAGES C CXX) include(cmake/Common.cmake) diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index 3f034be..46af783 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -224,9 +224,10 @@ std::shared_ptr API::Create(int argc, char *argv[]) { auto &&device = device::select(); if (!device) return nullptr; auto api = Create(argc, argv, device); - if (api && checkFirmwareVersion(api)) - return api; - return nullptr; + return api; + // if (api && checkFirmwareVersion(api)) + // return api; + // return nullptr; } std::shared_ptr API::Create( diff --git a/src/mynteye/api/version_checker.cc b/src/mynteye/api/version_checker.cc index f39c6d5..611bfb3 100644 --- a/src/mynteye/api/version_checker.cc +++ b/src/mynteye/api/version_checker.cc @@ -39,8 +39,7 @@ const char* PASS_DESCRIPTION = "pass"; /**----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.3.0", PASS_DESCRIPTION}, - {"MYNT-EYE-S1030", ">2.3.0", "2.2.2", PASS_DESCRIPTION}, + {"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}, From 852d8d8faf20272fccebb449bc040b192344780a Mon Sep 17 00:00:00 2001 From: John Zhao Date: Thu, 28 Feb 2019 09:31:08 +0800 Subject: [PATCH 24/28] docs(doxyfile): update version --- doc/en/api.doxyfile | 2 +- doc/zh-Hans/api.doxyfile | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/en/api.doxyfile b/doc/en/api.doxyfile index 9551550..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.1 +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/api.doxyfile b/doc/zh-Hans/api.doxyfile index f4c7e70..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.1 +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 From 455bfc72b28adef74e7f536a5352069c94d3c9d7 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Thu, 28 Feb 2019 10:23:19 +0800 Subject: [PATCH 25/28] chore(readme): update readme --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 6059be3..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.1-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/2893965/mynt-eye-s-sdk-apidoc-2.3.1-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2893979/mynt-eye-s-sdk-apidoc-2.3.1-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/2893985/mynt-eye-s-sdk-apidoc-2.3.1-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2893986/mynt-eye-s-sdk-apidoc-2.3.1-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.1-zh-Hans/mynt-eye-s-sdk-apidoc-2.3.1-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/2893975/mynt-eye-s-sdk-guide-2.3.1-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2893996/mynt-eye-s-sdk-guide-2.3.1-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/2893997/mynt-eye-s-sdk-guide-2.3.1-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2893997/mynt-eye-s-sdk-guide-2.3.1-zh-Hans.pdf) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/sdk/mynt-eye-s-sdk-guide-2.3.1-zh-Hans/mynt-eye-s-sdk-guide-2.3.1-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`. From f525c2063e236a0e254db5c83a48cee8921ba916 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Thu, 28 Feb 2019 16:44:55 +0800 Subject: [PATCH 26/28] refactor(synthetic): remove usless logic --- src/mynteye/api/api.cc | 7 +-- src/mynteye/api/synthetic.cc | 98 +++++++++++------------------- src/mynteye/api/version_checker.cc | 8 +-- 3 files changed, 42 insertions(+), 71 deletions(-) diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index 46af783..3f034be 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -224,10 +224,9 @@ std::shared_ptr API::Create(int argc, char *argv[]) { auto &&device = device::select(); if (!device) return nullptr; auto api = Create(argc, argv, device); - return api; - // if (api && checkFirmwareVersion(api)) - // return api; - // return nullptr; + if (api && checkFirmwareVersion(api)) + return api; + return nullptr; } std::shared_ptr API::Create( diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index a88e8bc..654d30e 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -511,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}); @@ -618,25 +612,7 @@ 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( diff --git a/src/mynteye/api/version_checker.cc b/src/mynteye/api/version_checker.cc index 611bfb3..55220a6 100644 --- a/src/mynteye/api/version_checker.cc +++ b/src/mynteye/api/version_checker.cc @@ -99,12 +99,8 @@ STATUS_UNIT checkUnit(const std::string& sdkv, 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 == 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; From da9417852aa481dedcaaee0196827121126ac9a7 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Thu, 28 Feb 2019 16:58:13 +0800 Subject: [PATCH 27/28] feat: forbid 2100/210A uless update sdk to 2.3.1 and above --- src/mynteye/api/version_checker.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/mynteye/api/version_checker.cc b/src/mynteye/api/version_checker.cc index 55220a6..723bbf0 100644 --- a/src/mynteye/api/version_checker.cc +++ b/src/mynteye/api/version_checker.cc @@ -44,9 +44,11 @@ static const firmware_version_match_table_unit FSVM_TABLE[] ={ {"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", ">0.0.0", "1.0", PASS_DESCRIPTION}, + {"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", ">0.0.0", "1.0", PASS_DESCRIPTION}, + {"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) { From c24e2806b35f8641cb0f0669973b5c65dde72257 Mon Sep 17 00:00:00 2001 From: kalman Date: Thu, 28 Feb 2019 14:56:18 +0800 Subject: [PATCH 28/28] feat(*): add function to querry some hardware info --- doc/en/spec_hardware_info.md | 2 ++ doc/zh-Hans/spec_hardware_info.md | 2 ++ include/mynteye/device/types.h | 2 ++ include/mynteye/types.h | 4 +++ samples/tutorials/data/get_device_info.cc | 3 ++ src/mynteye/device/channel/channels.cc | 1 + src/mynteye/device/channel/file_channel.cc | 32 +++++++++++++++++-- src/mynteye/device/device.cc | 4 +++ src/mynteye/types.cc | 2 ++ test/types_test.cc | 2 ++ tools/writer/config/S210A/device.info | 10 +++--- tools/writer/device_writer.cc | 8 ++++- wrappers/python/src/mynteye_py.cc | 4 ++- .../launch/mynteye.launch | 2 +- .../scripts/get_device_info.py | 2 ++ .../src/wrapper_nodelet.cc | 6 ++++ .../src/mynt_eye_ros_wrapper/srv/GetInfo.srv | 10 +++--- 17 files changed, 83 insertions(+), 13 deletions(-) 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/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/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..adf52c8 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -122,6 +122,10 @@ enum class Info : std::uint8_t { IMU_TYPE, /** Nominal baseline */ NOMINAL_BASELINE, + /** Auxiliary chip version */ + AUXILIARY_CHIP_VERSION, + /** Isp version */ + ISP_VERSION, /** Last guard */ LAST }; 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/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 497d5cd..271be74 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 ""; diff --git a/src/mynteye/types.cc b/src/mynteye/types.cc index e723031..27b6beb 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 d6ada67..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 @@ - + 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 62a5945..ededd62 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 @@ -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); 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