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

Modification of publishing topic only when that was subscribed.

Browse Source
This commit is contained in:
Osenberg-Y 2018-08-07 11:15:45 +08:00
parent 8ab9b42896
commit 455b5b931e
1 changed files with 109 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -110,7 +110,12 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
stream_topics[it->first] = it->second;
private_nh_.getParam(it->second + "_topic", stream_topics[it->first]);
// if published init
is_published_[it->first] = false;
}
is_motion_published_ = false;
is_started_ = false;
std::string imu_topic = "imu";
std::string temp_topic = "temp";
@ -213,8 +218,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
NODELET_INFO_STREAM("Advertized service " << DEVICE_INFO_SERVICE);
publishStaticTransforms();
while (private_nh_.ok()) {
publishTopics();
}
}
bool getInfo(
mynt_eye_ros_wrapper::GetInfo::Request &req, // NOLINT
@ -253,6 +260,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}
void publishTopics() {
if (camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 &&
!is_published_[Stream::LEFT]) {
api_->SetStreamCallback(
Stream::LEFT, [this](const api::StreamData &data) {
ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
@ -264,7 +273,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// << ((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");
// : (data.img->timestamp - img_time_prev) * 0.01f) << "
// ms");
// img_time_prev = data.img->timestamp;
++left_count_;
@ -275,7 +285,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
<< ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time);
});
is_published_[Stream::LEFT] = true;
}
if (camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 &&
!is_published_[Stream::RIGHT]) {
api_->SetStreamCallback(
Stream::RIGHT, [this](const api::StreamData &data) {
ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
@ -283,17 +297,23 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
++right_count_;
publishCamera(Stream::RIGHT, data, right_count_, stamp);
NODELET_DEBUG_STREAM(
Stream::RIGHT << ", count: " << right_count_
<< ", frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp
Stream::RIGHT
<< ", count: " << right_count_ << ", frame_id: "
<< data.img->frame_id << ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time);
});
is_published_[Stream::RIGHT] = true;
}
std::vector<Stream> other_streams{
Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, Stream::DISPARITY,
Stream::DISPARITY_NORMALIZED, Stream::DEPTH};
for (auto &&stream : other_streams) {
if (camera_publishers_[stream].getNumSubscribers() == 0 &&
is_published_[stream]) {
continue;
}
api_->SetStreamCallback(
stream, [this, stream](const api::StreamData &data) {
// data.img is null, not hard timestamp
@ -301,20 +321,27 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
++count;
publishCamera(stream, data, count, ros::Time::now());
});
is_published_[stream] = true;
}
if (camera_publishers_[Stream::POINTS].getNumSubscribers() > 0 &&
!is_published_[Stream::POINTS]) {
api_->SetStreamCallback(
Stream::POINTS, [this](const api::StreamData &data) {
static std::size_t count = 0;
++count;
publishPoints(data, count, ros::Time::now());
});
is_published_[Stream::POINTS] = true;
}
if (!is_motion_published_) {
api_->SetMotionCallback([this](const api::MotionData &data) {
ros::Time stamp = hardTimeToSoftTime(data.imu->timestamp);
// static double imu_time_prev = -1;
// NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION << ros_time_beg
// NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
// ros_time_beg
// << ", imu_time_elapsed: " << FULL_PRECISION
// << ((data.imu->timestamp - imu_time_beg) * 0.00001f)
// << ", imu_time_diff: " << FULL_PRECISION
@ -338,16 +365,23 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// Sleep 1ms, otherwise publish may drop some datas.
ros::Duration(0.001).sleep();
});
is_motion_published_ = true;
}
time_beg_ = ros::Time::now().toSec();
if (!is_started_) {
api_->Start(Source::ALL);
is_started_ = true;
}
}
void publishCamera(
const Stream &stream, const api::StreamData &data, std::uint32_t seq,
ros::Time stamp) {
if (camera_publishers_[stream].getNumSubscribers() == 0)
return;
/*
if (camera_publishers_[stream].getNumSubscribers() == 0)
return;
*/
std_msgs::Header header;
header.seq = seq;
header.stamp = stamp;
@ -385,8 +419,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
void publishPoints(
const api::StreamData &data, std::uint32_t seq, ros::Time stamp) {
if (points_publisher_.getNumSubscribers() == 0)
return;
/*
if (points_publisher_.getNumSubscribers() == 0)
return;
*/
auto &&in = api_->GetIntrinsics(Stream::LEFT);
@ -831,6 +867,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
std::size_t left_count_ = 0;
std::size_t right_count_ = 0;
std::size_t imu_count_ = 0;
std::map<Stream, bool> is_published_;
bool is_motion_published_;
bool is_started_;
};
MYNTEYE_END_NAMESPACE