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
Download Patch File Download Diff File Expand all files Collapse all files

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_) {