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

fix(ros): ros record -a

Browse Source
This commit is contained in:
TinyOh 2019-03-02 09:58:48 +08:00
parent 22bd0fab3a
commit 95d733b2b4
5 changed files with 84 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/mynteye/api/processor.cc
View File

@ -249,7 +249,7 @@ void Processor::Run() {
api::StreamData Processor::GetStreamData(const Stream &stream) { api::StreamData Processor::GetStreamData(const Stream &stream) {
auto sum = getStreamsSum(); auto sum = getStreamsSum();
auto &&out = GetOutput(); auto &&out = GetOutput();
Synthetic::Mode enable_mode = Synthetic::MODE_LAST; Synthetic::Mode enable_mode = Synthetic::MODE_OFF;
auto streams = getTargetStreams(); auto streams = getTargetStreams();
for (auto it_s : streams) { for (auto it_s : streams) {
if (it_s.stream == stream) { if (it_s.stream == stream) {
@ -257,7 +257,7 @@ api::StreamData Processor::GetStreamData(const Stream &stream) {
break; break;
} }
} }
if (enable_mode == Synthetic::MODE_SYNTHETIC) { if (enable_mode == Synthetic::MODE_ON) {
if (sum == 1) { if (sum == 1) {
if (out != nullptr) { if (out != nullptr) {
auto &&output = Object::Cast<ObjMat>(out); auto &&output = Object::Cast<ObjMat>(out);
@ -297,7 +297,7 @@ api::StreamData Processor::GetStreamData(const Stream &stream) {
} }
std::vector<api::StreamData> Processor::GetStreamDatas(const Stream &stream) { std::vector<api::StreamData> Processor::GetStreamDatas(const Stream &stream) {
Synthetic::Mode enable_mode = Synthetic::MODE_LAST; Synthetic::Mode enable_mode = Synthetic::MODE_OFF;
auto streams = getTargetStreams(); auto streams = getTargetStreams();
for (auto it_s : streams) { for (auto it_s : streams) {
if (it_s.stream == stream) { if (it_s.stream == stream) {
@ -305,7 +305,7 @@ std::vector<api::StreamData> Processor::GetStreamDatas(const Stream &stream) {
break; break;
} }
} }
if (enable_mode == Synthetic::MODE_SYNTHETIC) { if (enable_mode == Synthetic::MODE_ON) {
return {GetStreamData(stream)}; return {GetStreamData(stream)};
} else { } else {
LOG(ERROR) << "Failed to get stream data of " << stream LOG(ERROR) << "Failed to get stream data of " << stream

9
src/mynteye/api/processor/root_camera_processor.cc
View File

@ -108,7 +108,7 @@ void s1s2Processor::StopVideoStreaming() {
} }
api::StreamData s1s2Processor::GetStreamData(const Stream &stream) { api::StreamData s1s2Processor::GetStreamData(const Stream &stream) {
Synthetic::Mode enable_mode = Synthetic::MODE_LAST; Synthetic::Mode enable_mode = Synthetic::MODE_OFF;
auto streams = getTargetStreams(); auto streams = getTargetStreams();
for (auto it_s : streams) { for (auto it_s : streams) {
if (it_s.stream == stream) { if (it_s.stream == stream) {
@ -116,17 +116,18 @@ api::StreamData s1s2Processor::GetStreamData(const Stream &stream) {
break; break;
} }
} }
if (enable_mode == Synthetic::MODE_SYNTHETIC) { if (enable_mode == Synthetic::MODE_ON) {
return data2api(device_->GetStreamData(stream)); return data2api(device_->GetStreamData(stream));
} }
LOG(ERROR) << "Failed to get device stream data of " << stream LOG(ERROR) << "Failed to get device stream data of " << stream
<< ", unsupported or disabled"; << ", unsupported or disabled";
LOG(ERROR) << "Make sure you have enable " << stream;
return {}; return {};
} }
std::vector<api::StreamData> s1s2Processor::GetStreamDatas( std::vector<api::StreamData> s1s2Processor::GetStreamDatas(
const Stream &stream) { const Stream &stream) {
Synthetic::Mode enable_mode = Synthetic::MODE_LAST; Synthetic::Mode enable_mode = Synthetic::MODE_OFF;
auto streams = getTargetStreams(); auto streams = getTargetStreams();
for (auto it_s : streams) { for (auto it_s : streams) {
if (it_s.stream == stream) { if (it_s.stream == stream) {
@ -134,7 +135,7 @@ std::vector<api::StreamData> s1s2Processor::GetStreamDatas(
break; break;
} }
} }
if (enable_mode == Synthetic::MODE_SYNTHETIC) { if (enable_mode == Synthetic::MODE_ON) {
std::vector<api::StreamData> datas; std::vector<api::StreamData> datas;
for (auto &&data : device_->GetStreamDatas(stream)) { for (auto &&data : device_->GetStreamDatas(stream)) {
datas.push_back(data2api(data)); datas.push_back(data2api(data));

45
src/mynteye/api/synthetic.cc
View File

@ -183,11 +183,11 @@ void Synthetic::EnableStreamData(
auto streams = proce->getTargetStreams(); auto streams = proce->getTargetStreams();
int act_tag = 0; int act_tag = 0;
for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) { 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); callback(proce->target_streams_[i].stream);
if (!try_tag) { if (!try_tag) {
act_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(); auto streams = proce->getTargetStreams();
int act_tag = 0; int act_tag = 0;
for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) { 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); callback(proce->target_streams_[i].stream);
if (!try_tag) { if (!try_tag) {
act_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 { bool Synthetic::IsStreamDataEnabled(const Stream &stream) const {
if (checkControlDateWithStream(stream)) { if (checkControlDateWithStream(stream)) {
auto data = getControlDateWithStream(stream); auto data = getControlDateWithStream(stream);
return data.enabled_mode_ == MODE_SYNTHETIC; return data.enabled_mode_ == MODE_ON;
} }
return false; return false;
} }
@ -300,7 +300,7 @@ Synthetic::mode_t Synthetic::GetStreamEnabledMode(const Stream &stream) const {
auto data = getControlDateWithStream(stream); auto data = getControlDateWithStream(stream);
return data.enabled_mode_; return data.enabled_mode_;
} }
return MODE_LAST; return MODE_OFF;
} }
void Synthetic::InitProcessors() { void Synthetic::InitProcessors() {
@ -361,21 +361,21 @@ void Synthetic::InitProcessors() {
} }
rectify_processor->addTargetStreams( rectify_processor->addTargetStreams(
{Stream::LEFT_RECTIFIED, Mode::MODE_LAST, nullptr}); {Stream::LEFT_RECTIFIED, Mode::MODE_OFF, nullptr});
rectify_processor->addTargetStreams( rectify_processor->addTargetStreams(
{Stream::RIGHT_RECTIFIED, Mode::MODE_LAST, nullptr}); {Stream::RIGHT_RECTIFIED, Mode::MODE_OFF, nullptr});
disparity_processor->addTargetStreams( disparity_processor->addTargetStreams(
{Stream::DISPARITY, Mode::MODE_LAST, nullptr}); {Stream::DISPARITY, Mode::MODE_OFF, nullptr});
disparitynormalized_processor->addTargetStreams( disparitynormalized_processor->addTargetStreams(
{Stream::DISPARITY_NORMALIZED, Mode::MODE_LAST, nullptr}); {Stream::DISPARITY_NORMALIZED, Mode::MODE_OFF, nullptr});
points_processor->addTargetStreams( points_processor->addTargetStreams(
{Stream::POINTS, Mode::MODE_LAST, nullptr}); {Stream::POINTS, Mode::MODE_OFF, nullptr});
depth_processor->addTargetStreams( depth_processor->addTargetStreams(
{Stream::DEPTH, Mode::MODE_LAST, nullptr}); {Stream::DEPTH, Mode::MODE_OFF, nullptr});
root_processor->addTargetStreams( root_processor->addTargetStreams(
{Stream::LEFT, Mode::MODE_LAST, nullptr}); {Stream::LEFT, Mode::MODE_OFF, nullptr});
root_processor->addTargetStreams( root_processor->addTargetStreams(
{Stream::RIGHT, Mode::MODE_LAST, nullptr}); {Stream::RIGHT, Mode::MODE_OFF, nullptr});
processors_.push_back(root_processor); processors_.push_back(root_processor);
processors_.push_back(rectify_processor); processors_.push_back(rectify_processor);
@ -417,7 +417,7 @@ bool Synthetic::OnDeviceProcess(
Object *const in, Object *const out, Object *const in, Object *const out,
std::shared_ptr<Processor> const parent) { std::shared_ptr<Processor> const parent) {
MYNTEYE_UNUSED(parent) MYNTEYE_UNUSED(parent)
return GetStreamEnabledMode(Stream::LEFT) != MODE_SYNTHETIC; return GetStreamEnabledMode(Stream::LEFT) != MODE_ON;
} }
bool Synthetic::OnRectifyProcess( bool Synthetic::OnRectifyProcess(
@ -427,8 +427,8 @@ bool Synthetic::OnRectifyProcess(
if (plugin_ && plugin_->OnRectifyProcess(in, out)) { if (plugin_ && plugin_->OnRectifyProcess(in, out)) {
return true; return true;
} }
return GetStreamEnabledMode(Stream::LEFT_RECTIFIED) != MODE_SYNTHETIC; return GetStreamEnabledMode(Stream::LEFT_RECTIFIED) != MODE_ON;
// && GetStreamEnabledMode(Stream::RIGHT_RECTIFIED) != MODE_SYNTHETIC // && GetStreamEnabledMode(Stream::RIGHT_RECTIFIED) != MODE_ON
} }
bool Synthetic::OnDisparityProcess( bool Synthetic::OnDisparityProcess(
@ -438,7 +438,7 @@ bool Synthetic::OnDisparityProcess(
if (plugin_ && plugin_->OnDisparityProcess(in, out)) { if (plugin_ && plugin_->OnDisparityProcess(in, out)) {
return true; return true;
} }
return GetStreamEnabledMode(Stream::DISPARITY) != MODE_SYNTHETIC; return GetStreamEnabledMode(Stream::DISPARITY) != MODE_ON;
} }
bool Synthetic::OnDisparityNormalizedProcess( bool Synthetic::OnDisparityNormalizedProcess(
@ -448,7 +448,7 @@ bool Synthetic::OnDisparityNormalizedProcess(
if (plugin_ && plugin_->OnDisparityNormalizedProcess(in, out)) { if (plugin_ && plugin_->OnDisparityNormalizedProcess(in, out)) {
return true; return true;
} }
return GetStreamEnabledMode(Stream::DISPARITY_NORMALIZED) != MODE_SYNTHETIC; return GetStreamEnabledMode(Stream::DISPARITY_NORMALIZED) != MODE_ON;
} }
bool Synthetic::OnPointsProcess( bool Synthetic::OnPointsProcess(
@ -458,7 +458,7 @@ bool Synthetic::OnPointsProcess(
if (plugin_ && plugin_->OnPointsProcess(in, out)) { if (plugin_ && plugin_->OnPointsProcess(in, out)) {
return true; return true;
} }
return GetStreamEnabledMode(Stream::POINTS) != MODE_SYNTHETIC; return GetStreamEnabledMode(Stream::POINTS) != MODE_ON;
} }
bool Synthetic::OnDepthProcess( bool Synthetic::OnDepthProcess(
@ -468,7 +468,7 @@ bool Synthetic::OnDepthProcess(
if (plugin_ && plugin_->OnDepthProcess(in, out)) { if (plugin_ && plugin_->OnDepthProcess(in, out)) {
return true; return true;
} }
return GetStreamEnabledMode(Stream::DEPTH) != MODE_SYNTHETIC; return GetStreamEnabledMode(Stream::DEPTH) != MODE_ON;
} }
void Synthetic::OnDevicePostProcess(Object *const out) { void Synthetic::OnDevicePostProcess(Object *const out) {
@ -481,7 +481,8 @@ void Synthetic::OnDevicePostProcess(Object *const out) {
} }
if (HasStreamCallback(Stream::RIGHT)) { if (HasStreamCallback(Stream::RIGHT)) {
auto data = getControlDateWithStream(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));
} }
} }

6
src/mynteye/api/synthetic.h
View File

@ -40,9 +40,8 @@ class Synthetic {
using stream_switch_callback_t = API::stream_switch_callback_t; using stream_switch_callback_t = API::stream_switch_callback_t;
typedef enum Mode { typedef enum Mode {
MODE_NATIVE, // Native stream MODE_ON, // On
MODE_SYNTHETIC, // Synthetic stream MODE_OFF // Off
MODE_LAST // Unsupported
} mode_t; } mode_t;
struct stream_control_t { struct stream_control_t {
@ -142,7 +141,6 @@ class Synthetic {
std::shared_ptr<Plugin> plugin_; std::shared_ptr<Plugin> plugin_;
CalibrationModel calib_model_; CalibrationModel calib_model_;
std::mutex mtx_left_right_ready_;
std::shared_ptr<IntrinsicsBase> intr_left_; std::shared_ptr<IntrinsicsBase> intr_left_;
std::shared_ptr<IntrinsicsBase> intr_right_; std::shared_ptr<IntrinsicsBase> intr_right_;

154
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -583,22 +583,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
void publishOthers(const Stream &stream) { 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]) { 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_->EnableStreamData(stream);
api_->SetStreamCallback( api_->SetStreamCallback(
stream, [this, stream](const api::StreamData &data) { stream, [this, stream](const api::StreamData &data) {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp( ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, stream); data.img->timestamp, stream);
static std::size_t count = 0; static std::size_t count = 0;
@ -612,12 +600,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
int disable_tag = 0; int disable_tag = 0;
api_->DisableStreamData(stream, [&](const Stream &stream) { api_->DisableStreamData(stream, [&](const Stream &stream) {
disable_tag += getStreamSubscribers(stream); disable_tag += getStreamSubscribers(stream);
// std::cout << "DisableStreamData callback test: "
// << stream << "|| Sum:"<< getStreamSubscribers(stream) << std::endl;
}, true); }, true);
if (disable_tag == 0 && is_published_[stream]) { if (disable_tag == 0 && is_published_[stream]) {
api_->DisableStreamData(stream, [&](const Stream &stream) { api_->DisableStreamData(stream, [&](const Stream &stream) {
// std::cout << "disable " << stream << std::endl;
api_->SetStreamCallback(stream, nullptr); api_->SetStreamCallback(stream, nullptr);
is_published_[stream] = false; is_published_[stream] = false;
}); });
@ -626,98 +611,59 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
void publishTopics() { void publishTopics() {
std::vector<Stream> all_streams{ if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 ||
Stream::RIGHT, mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) &&
Stream::LEFT, !is_published_[Stream::LEFT]) {
Stream::LEFT_RECTIFIED, api_->SetStreamCallback(
Stream::RIGHT_RECTIFIED, Stream::LEFT, [&](const api::StreamData &data) {
Stream::DISPARITY, ++left_count_;
Stream::DISPARITY_NORMALIZED, if (left_count_ > 10) {
Stream::POINTS, // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
Stream::DEPTH ros::Time stamp = checkUpTimeStamp(
}; data.img->timestamp, Stream::LEFT);
publishCamera(Stream::LEFT, data, left_count_, stamp);
static int sum = 0; publishMono(Stream::LEFT, data, left_count_, stamp);
int sum_c = 0; NODELET_DEBUG_STREAM(
for (auto &&stream : all_streams) { Stream::LEFT << ", count: " << left_count_
sum_c += getStreamSubscribers(stream); << ", 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 ((camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 ||
if (sum_c == 0) { mono_publishers_[Stream::RIGHT].getNumSubscribers() > 0) &&
api_->Stop(Source::VIDEO_STREAMING); !is_published_[Stream::RIGHT]) {
for (auto &&stream : all_streams) { api_->SetStreamCallback(
is_published_[stream] = false; Stream::RIGHT, [&](const api::StreamData &data) {
} ++right_count_;
api_->Start(Source::VIDEO_STREAMING); if (right_count_ > 10) {
} else { // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 || ros::Time stamp = checkUpTimeStamp(
mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) && data.img->timestamp, Stream::RIGHT);
!is_published_[Stream::LEFT]) { publishCamera(Stream::RIGHT, data, right_count_, stamp);
api_->SetStreamCallback( publishMono(Stream::RIGHT, data, right_count_, stamp);
Stream::LEFT, [&](const api::StreamData &data) { NODELET_DEBUG_STREAM(
++left_count_; Stream::RIGHT << ", count: " << right_count_
if (left_count_ > 10) { << ", frame_id: " << data.img->frame_id
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); << ", timestamp: " << data.img->timestamp
ros::Time stamp = checkUpTimeStamp( << ", exposure_time: " << data.img->exposure_time);
data.img->timestamp, Stream::LEFT); }
});
right_time_beg_ = ros::Time::now().toSec();
is_published_[Stream::RIGHT] = true;
}
// static double img_time_prev = -1; std::vector<Stream> other_streams{
// NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION << Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED,
// ros_time_beg Stream::DISPARITY, Stream::DISPARITY_NORMALIZED,
// << ", img_time_elapsed: " << FULL_PRECISION Stream::POINTS, Stream::DEPTH
// << ((data.img->timestamp - img_time_beg) * 0.00001f) };
// << ", img_time_diff: " << FULL_PRECISION for (auto &&stream : other_streams) {
// << ((img_time_prev < 0) ? 0 publishOthers(stream);
// : (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<Stream> 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_) { if (!is_motion_published_) {