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) {
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<ObjMat>(out);
@ -297,7 +297,7 @@ api::StreamData Processor::GetStreamData(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();
for (auto it_s : streams) {
if (it_s.stream == stream) {
@ -305,7 +305,7 @@ std::vector<api::StreamData> 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

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) {
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<api::StreamData> 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<api::StreamData> s1s2Processor::GetStreamDatas(
break;
}
}
if (enable_mode == Synthetic::MODE_SYNTHETIC) {
if (enable_mode == Synthetic::MODE_ON) {
std::vector<api::StreamData> datas;
for (auto &&data : device_->GetStreamDatas(stream)) {
datas.push_back(data2api(data));

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

@ -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<Processor> 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));
}
}

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

@ -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> plugin_;
CalibrationModel calib_model_;
std::mutex mtx_left_right_ready_;
std::shared_ptr<IntrinsicsBase> intr_left_;
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) {
// 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<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 ((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<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;
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);
}
if (!is_motion_published_) {