diff --git a/src/mynteye/api/processor.cc b/src/mynteye/api/processor.cc index 93abed6..1f30e4c 100644 --- a/src/mynteye/api/processor.cc +++ b/src/mynteye/api/processor.cc @@ -249,7 +249,7 @@ void Processor::Run() { api::StreamData Processor::GetStreamData(const Stream &stream) { auto sum = getStreamsSum(); auto &&out = GetOutput(); - Synthetic::Mode enable_mode = Synthetic::MODE_LAST; + Synthetic::Mode enable_mode = Synthetic::MODE_OFF; auto streams = getTargetStreams(); for (auto it_s : streams) { if (it_s.stream == stream) { @@ -257,7 +257,7 @@ api::StreamData Processor::GetStreamData(const Stream &stream) { break; } } - if (enable_mode == Synthetic::MODE_SYNTHETIC) { + if (enable_mode == Synthetic::MODE_ON) { if (sum == 1) { if (out != nullptr) { auto &&output = Object::Cast(out); @@ -297,7 +297,7 @@ api::StreamData Processor::GetStreamData(const Stream &stream) { } std::vector Processor::GetStreamDatas(const Stream &stream) { - Synthetic::Mode enable_mode = Synthetic::MODE_LAST; + Synthetic::Mode enable_mode = Synthetic::MODE_OFF; auto streams = getTargetStreams(); for (auto it_s : streams) { if (it_s.stream == stream) { @@ -305,7 +305,7 @@ std::vector Processor::GetStreamDatas(const Stream &stream) { break; } } - if (enable_mode == Synthetic::MODE_SYNTHETIC) { + if (enable_mode == Synthetic::MODE_ON) { return {GetStreamData(stream)}; } else { LOG(ERROR) << "Failed to get stream data of " << stream diff --git a/src/mynteye/api/processor/root_camera_processor.cc b/src/mynteye/api/processor/root_camera_processor.cc index 0280b1a..75fca23 100644 --- a/src/mynteye/api/processor/root_camera_processor.cc +++ b/src/mynteye/api/processor/root_camera_processor.cc @@ -108,7 +108,7 @@ void s1s2Processor::StopVideoStreaming() { } api::StreamData s1s2Processor::GetStreamData(const Stream &stream) { - Synthetic::Mode enable_mode = Synthetic::MODE_LAST; + Synthetic::Mode enable_mode = Synthetic::MODE_OFF; auto streams = getTargetStreams(); for (auto it_s : streams) { if (it_s.stream == stream) { @@ -116,17 +116,18 @@ api::StreamData s1s2Processor::GetStreamData(const Stream &stream) { break; } } - if (enable_mode == Synthetic::MODE_SYNTHETIC) { + if (enable_mode == Synthetic::MODE_ON) { return data2api(device_->GetStreamData(stream)); } LOG(ERROR) << "Failed to get device stream data of " << stream << ", unsupported or disabled"; + LOG(ERROR) << "Make sure you have enable " << stream; return {}; } std::vector s1s2Processor::GetStreamDatas( const Stream &stream) { - Synthetic::Mode enable_mode = Synthetic::MODE_LAST; + Synthetic::Mode enable_mode = Synthetic::MODE_OFF; auto streams = getTargetStreams(); for (auto it_s : streams) { if (it_s.stream == stream) { @@ -134,7 +135,7 @@ std::vector s1s2Processor::GetStreamDatas( break; } } - if (enable_mode == Synthetic::MODE_SYNTHETIC) { + if (enable_mode == Synthetic::MODE_ON) { std::vector datas; for (auto &&data : device_->GetStreamDatas(stream)) { datas.push_back(data2api(data)); diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index f3ddd9f..5f85ea4 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -183,11 +183,11 @@ void Synthetic::EnableStreamData( 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) { + if (proce->target_streams_[i].enabled_mode_ == MODE_OFF) { callback(proce->target_streams_[i].stream); if (!try_tag) { act_tag++; - proce->target_streams_[i].enabled_mode_ = MODE_SYNTHETIC; + proce->target_streams_[i].enabled_mode_ = MODE_ON; } } } @@ -206,11 +206,11 @@ void Synthetic::DisableStreamData( 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) { + if (proce->target_streams_[i].enabled_mode_ == MODE_ON) { callback(proce->target_streams_[i].stream); if (!try_tag) { act_tag++; - proce->target_streams_[i].enabled_mode_ = MODE_LAST; + proce->target_streams_[i].enabled_mode_ = MODE_OFF; } } } @@ -238,7 +238,7 @@ void Synthetic::DisableStreamData(const Stream &stream) { bool Synthetic::IsStreamDataEnabled(const Stream &stream) const { if (checkControlDateWithStream(stream)) { auto data = getControlDateWithStream(stream); - return data.enabled_mode_ == MODE_SYNTHETIC; + return data.enabled_mode_ == MODE_ON; } return false; } @@ -300,7 +300,7 @@ Synthetic::mode_t Synthetic::GetStreamEnabledMode(const Stream &stream) const { auto data = getControlDateWithStream(stream); return data.enabled_mode_; } - return MODE_LAST; + return MODE_OFF; } void Synthetic::InitProcessors() { @@ -361,21 +361,21 @@ void Synthetic::InitProcessors() { } rectify_processor->addTargetStreams( - {Stream::LEFT_RECTIFIED, Mode::MODE_LAST, nullptr}); + {Stream::LEFT_RECTIFIED, Mode::MODE_OFF, nullptr}); rectify_processor->addTargetStreams( - {Stream::RIGHT_RECTIFIED, Mode::MODE_LAST, nullptr}); + {Stream::RIGHT_RECTIFIED, Mode::MODE_OFF, nullptr}); disparity_processor->addTargetStreams( - {Stream::DISPARITY, Mode::MODE_LAST, nullptr}); + {Stream::DISPARITY, Mode::MODE_OFF, nullptr}); disparitynormalized_processor->addTargetStreams( - {Stream::DISPARITY_NORMALIZED, Mode::MODE_LAST, nullptr}); + {Stream::DISPARITY_NORMALIZED, Mode::MODE_OFF, nullptr}); points_processor->addTargetStreams( - {Stream::POINTS, Mode::MODE_LAST, nullptr}); + {Stream::POINTS, Mode::MODE_OFF, nullptr}); depth_processor->addTargetStreams( - {Stream::DEPTH, Mode::MODE_LAST, nullptr}); + {Stream::DEPTH, Mode::MODE_OFF, nullptr}); root_processor->addTargetStreams( - {Stream::LEFT, Mode::MODE_LAST, nullptr}); + {Stream::LEFT, Mode::MODE_OFF, nullptr}); root_processor->addTargetStreams( - {Stream::RIGHT, Mode::MODE_LAST, nullptr}); + {Stream::RIGHT, Mode::MODE_OFF, nullptr}); processors_.push_back(root_processor); processors_.push_back(rectify_processor); @@ -417,7 +417,7 @@ bool Synthetic::OnDeviceProcess( Object *const in, Object *const out, std::shared_ptr const parent) { MYNTEYE_UNUSED(parent) - return GetStreamEnabledMode(Stream::LEFT) != MODE_SYNTHETIC; + return GetStreamEnabledMode(Stream::LEFT) != MODE_ON; } bool Synthetic::OnRectifyProcess( @@ -427,8 +427,8 @@ bool Synthetic::OnRectifyProcess( if (plugin_ && plugin_->OnRectifyProcess(in, out)) { return true; } - return GetStreamEnabledMode(Stream::LEFT_RECTIFIED) != MODE_SYNTHETIC; - // && GetStreamEnabledMode(Stream::RIGHT_RECTIFIED) != MODE_SYNTHETIC + return GetStreamEnabledMode(Stream::LEFT_RECTIFIED) != MODE_ON; + // && GetStreamEnabledMode(Stream::RIGHT_RECTIFIED) != MODE_ON } bool Synthetic::OnDisparityProcess( @@ -438,7 +438,7 @@ bool Synthetic::OnDisparityProcess( if (plugin_ && plugin_->OnDisparityProcess(in, out)) { return true; } - return GetStreamEnabledMode(Stream::DISPARITY) != MODE_SYNTHETIC; + return GetStreamEnabledMode(Stream::DISPARITY) != MODE_ON; } bool Synthetic::OnDisparityNormalizedProcess( @@ -448,7 +448,7 @@ bool Synthetic::OnDisparityNormalizedProcess( if (plugin_ && plugin_->OnDisparityNormalizedProcess(in, out)) { return true; } - return GetStreamEnabledMode(Stream::DISPARITY_NORMALIZED) != MODE_SYNTHETIC; + return GetStreamEnabledMode(Stream::DISPARITY_NORMALIZED) != MODE_ON; } bool Synthetic::OnPointsProcess( @@ -458,7 +458,7 @@ bool Synthetic::OnPointsProcess( if (plugin_ && plugin_->OnPointsProcess(in, out)) { return true; } - return GetStreamEnabledMode(Stream::POINTS) != MODE_SYNTHETIC; + return GetStreamEnabledMode(Stream::POINTS) != MODE_ON; } bool Synthetic::OnDepthProcess( @@ -468,7 +468,7 @@ bool Synthetic::OnDepthProcess( if (plugin_ && plugin_->OnDepthProcess(in, out)) { return true; } - return GetStreamEnabledMode(Stream::DEPTH) != MODE_SYNTHETIC; + return GetStreamEnabledMode(Stream::DEPTH) != MODE_ON; } void Synthetic::OnDevicePostProcess(Object *const out) { @@ -481,7 +481,8 @@ void Synthetic::OnDevicePostProcess(Object *const out) { } if (HasStreamCallback(Stream::RIGHT)) { auto data = getControlDateWithStream(Stream::RIGHT); - data.stream_callback(obj_data_second(output)); + if (data.stream_callback) + data.stream_callback(obj_data_second(output)); } } diff --git a/src/mynteye/api/synthetic.h b/src/mynteye/api/synthetic.h index 5476792..993b7c6 100644 --- a/src/mynteye/api/synthetic.h +++ b/src/mynteye/api/synthetic.h @@ -40,9 +40,8 @@ class Synthetic { using stream_switch_callback_t = API::stream_switch_callback_t; typedef enum Mode { - MODE_NATIVE, // Native stream - MODE_SYNTHETIC, // Synthetic stream - MODE_LAST // Unsupported + MODE_ON, // On + MODE_OFF // Off } mode_t; struct stream_control_t { @@ -142,7 +141,6 @@ 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_; 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 75b381b..abaecf0 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 @@ -583,22 +583,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet { } void publishOthers(const Stream &stream) { - // 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) { - // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); ros::Time stamp = checkUpTimeStamp( data.img->timestamp, stream); static std::size_t count = 0; @@ -612,12 +600,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet { 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; }); @@ -626,98 +611,59 @@ class ROSWrapperNodelet : public nodelet::Nodelet { } void publishTopics() { - std::vector all_streams{ - Stream::RIGHT, - Stream::LEFT, - Stream::LEFT_RECTIFIED, - Stream::RIGHT_RECTIFIED, - Stream::DISPARITY, - Stream::DISPARITY_NORMALIZED, - Stream::POINTS, - Stream::DEPTH - }; - - static int sum = 0; - int sum_c = 0; - for (auto &&stream : all_streams) { - sum_c += getStreamSubscribers(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); + 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 (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 { - 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); + 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; + } - // 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; + 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); } if (!is_motion_published_) {