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));

43
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,6 +481,7 @@ void Synthetic::OnDevicePostProcess(Object *const out) {
} }
if (HasStreamCallback(Stream::RIGHT)) { if (HasStreamCallback(Stream::RIGHT)) {
auto data = getControlDateWithStream(Stream::RIGHT); auto data = getControlDateWithStream(Stream::RIGHT);
if (data.stream_callback)
data.stream_callback(obj_data_second(output)); 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_;

54
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,31 +611,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
void publishTopics() { void publishTopics() {
std::vector<Stream> 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 (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 || if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 ||
mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) && mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) &&
!is_published_[Stream::LEFT]) { !is_published_[Stream::LEFT]) {
@ -661,17 +621,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp( ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, Stream::LEFT); 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); publishCamera(Stream::LEFT, data, left_count_, stamp);
publishMono(Stream::LEFT, data, left_count_, stamp); publishMono(Stream::LEFT, data, left_count_, stamp);
NODELET_DEBUG_STREAM( NODELET_DEBUG_STREAM(
@ -716,9 +665,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
for (auto &&stream : other_streams) { for (auto &&stream : other_streams) {
publishOthers(stream); publishOthers(stream);
} }
}
sum = sum_c;
}
if (!is_motion_published_) { if (!is_motion_published_) {
api_->SetMotionCallback([this](const api::MotionData &data) { api_->SetMotionCallback([this](const api::MotionData &data) {