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

178
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) { for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
stream_topics[it->first] = it->second; stream_topics[it->first] = it->second;
private_nh_.getParam(it->second + "_topic", stream_topics[it->first]); 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 imu_topic = "imu";
std::string temp_topic = "temp"; std::string temp_topic = "temp";
@ -213,7 +218,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
NODELET_INFO_STREAM("Advertized service " << DEVICE_INFO_SERVICE); NODELET_INFO_STREAM("Advertized service " << DEVICE_INFO_SERVICE);
publishStaticTransforms(); publishStaticTransforms();
publishTopics(); while (private_nh_.ok()) {
publishTopics();
}
} }
bool getInfo( bool getInfo(
@ -253,47 +260,60 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
void publishTopics() { void publishTopics() {
api_->SetStreamCallback( if (camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 &&
Stream::LEFT, [this](const api::StreamData &data) { !is_published_[Stream::LEFT]) {
ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); api_->SetStreamCallback(
Stream::LEFT, [this](const api::StreamData &data) {
ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
// static double img_time_prev = -1; // static double img_time_prev = -1;
// NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION << // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
// ros_time_beg // ros_time_beg
// << ", img_time_elapsed: " << FULL_PRECISION // << ", img_time_elapsed: " << FULL_PRECISION
// << ((data.img->timestamp - img_time_beg) * 0.00001f) // << ((data.img->timestamp - img_time_beg) * 0.00001f)
// << ", img_time_diff: " << FULL_PRECISION // << ", img_time_diff: " << FULL_PRECISION
// << ((img_time_prev < 0) ? 0 // << ((img_time_prev < 0) ? 0
// : (data.img->timestamp - img_time_prev) * 0.01f) << " ms"); // : (data.img->timestamp - img_time_prev) * 0.01f) << "
// img_time_prev = data.img->timestamp; // ms");
// img_time_prev = data.img->timestamp;
++left_count_; ++left_count_;
publishCamera(Stream::LEFT, data, left_count_, stamp); publishCamera(Stream::LEFT, data, left_count_, stamp);
NODELET_DEBUG_STREAM( NODELET_DEBUG_STREAM(
Stream::LEFT << ", count: " << left_count_ Stream::LEFT << ", count: " << left_count_
<< ", frame_id: " << data.img->frame_id << ", frame_id: " << data.img->frame_id
<< ", timestamp: " << data.img->timestamp << ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time); << ", exposure_time: " << data.img->exposure_time);
}); });
is_published_[Stream::LEFT] = true;
}
api_->SetStreamCallback( if (camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 &&
Stream::RIGHT, [this](const api::StreamData &data) { !is_published_[Stream::RIGHT]) {
ros::Time stamp = hardTimeToSoftTime(data.img->timestamp); api_->SetStreamCallback(
Stream::RIGHT, [this](const api::StreamData &data) {
ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
++right_count_; ++right_count_;
publishCamera(Stream::RIGHT, data, right_count_, stamp); publishCamera(Stream::RIGHT, data, right_count_, stamp);
NODELET_DEBUG_STREAM( NODELET_DEBUG_STREAM(
Stream::RIGHT << ", count: " << right_count_ Stream::RIGHT
<< ", frame_id: " << data.img->frame_id << ", count: " << right_count_ << ", frame_id: "
<< ", timestamp: " << data.img->timestamp << data.img->frame_id << ", timestamp: " << data.img->timestamp
<< ", exposure_time: " << data.img->exposure_time); << ", exposure_time: " << data.img->exposure_time);
}); });
is_published_[Stream::RIGHT] = true;
}
std::vector<Stream> other_streams{ std::vector<Stream> other_streams{
Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, Stream::DISPARITY, Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, Stream::DISPARITY,
Stream::DISPARITY_NORMALIZED, Stream::DEPTH}; Stream::DISPARITY_NORMALIZED, Stream::DEPTH};
for (auto &&stream : other_streams) { for (auto &&stream : other_streams) {
if (camera_publishers_[stream].getNumSubscribers() == 0 &&
is_published_[stream]) {
continue;
}
api_->SetStreamCallback( api_->SetStreamCallback(
stream, [this, stream](const api::StreamData &data) { stream, [this, stream](const api::StreamData &data) {
// data.img is null, not hard timestamp // data.img is null, not hard timestamp
@ -301,53 +321,67 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
++count; ++count;
publishCamera(stream, data, count, ros::Time::now()); publishCamera(stream, data, count, ros::Time::now());
}); });
is_published_[stream] = true;
} }
api_->SetStreamCallback( if (camera_publishers_[Stream::POINTS].getNumSubscribers() > 0 &&
Stream::POINTS, [this](const api::StreamData &data) { !is_published_[Stream::POINTS]) {
static std::size_t count = 0; api_->SetStreamCallback(
++count; Stream::POINTS, [this](const api::StreamData &data) {
publishPoints(data, count, ros::Time::now()); static std::size_t count = 0;
}); ++count;
publishPoints(data, count, ros::Time::now());
});
is_published_[Stream::POINTS] = true;
}
api_->SetMotionCallback([this](const api::MotionData &data) { if (!is_motion_published_) {
ros::Time stamp = hardTimeToSoftTime(data.imu->timestamp); api_->SetMotionCallback([this](const api::MotionData &data) {
ros::Time stamp = hardTimeToSoftTime(data.imu->timestamp);
// static double imu_time_prev = -1; // 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 <<
// << ", imu_time_elapsed: " << FULL_PRECISION // ros_time_beg
// << ((data.imu->timestamp - imu_time_beg) * 0.00001f) // << ", imu_time_elapsed: " << FULL_PRECISION
// << ", imu_time_diff: " << FULL_PRECISION // << ((data.imu->timestamp - imu_time_beg) * 0.00001f)
// << ((imu_time_prev < 0) ? 0 // << ", imu_time_diff: " << FULL_PRECISION
// : (data.imu->timestamp - imu_time_prev) * 0.01f) << " ms"); // << ((imu_time_prev < 0) ? 0
// imu_time_prev = data.imu->timestamp; // : (data.imu->timestamp - imu_time_prev) * 0.01f) << " ms");
// imu_time_prev = data.imu->timestamp;
++imu_count_; ++imu_count_;
publishImu(data, imu_count_, stamp); publishImu(data, imu_count_, stamp);
publishTemp(data.imu->temperature, imu_count_, stamp); publishTemp(data.imu->temperature, imu_count_, stamp);
NODELET_DEBUG_STREAM( NODELET_DEBUG_STREAM(
"Imu count: " << imu_count_ << ", frame_id: " << data.imu->frame_id "Imu count: " << imu_count_ << ", frame_id: " << data.imu->frame_id
<< ", timestamp: " << data.imu->timestamp << ", timestamp: " << data.imu->timestamp
<< ", accel_x: " << data.imu->accel[0] << ", accel_x: " << data.imu->accel[0]
<< ", accel_y: " << data.imu->accel[1] << ", accel_y: " << data.imu->accel[1]
<< ", accel_z: " << data.imu->accel[2] << ", accel_z: " << data.imu->accel[2]
<< ", gyro_x: " << data.imu->gyro[0] << ", gyro_x: " << data.imu->gyro[0]
<< ", gyro_y: " << data.imu->gyro[1] << ", gyro_y: " << data.imu->gyro[1]
<< ", gyro_z: " << data.imu->gyro[2] << ", gyro_z: " << data.imu->gyro[2]
<< ", temperature: " << data.imu->temperature); << ", temperature: " << data.imu->temperature);
// Sleep 1ms, otherwise publish may drop some datas. // Sleep 1ms, otherwise publish may drop some datas.
ros::Duration(0.001).sleep(); ros::Duration(0.001).sleep();
}); });
is_motion_published_ = true;
}
time_beg_ = ros::Time::now().toSec(); time_beg_ = ros::Time::now().toSec();
api_->Start(Source::ALL); if (!is_started_) {
api_->Start(Source::ALL);
is_started_ = true;
}
} }
void publishCamera( void publishCamera(
const Stream &stream, const api::StreamData &data, std::uint32_t seq, const Stream &stream, const api::StreamData &data, std::uint32_t seq,
ros::Time stamp) { ros::Time stamp) {
if (camera_publishers_[stream].getNumSubscribers() == 0) /*
return; if (camera_publishers_[stream].getNumSubscribers() == 0)
return;
*/
std_msgs::Header header; std_msgs::Header header;
header.seq = seq; header.seq = seq;
header.stamp = stamp; header.stamp = stamp;
@ -385,8 +419,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
void publishPoints( void publishPoints(
const api::StreamData &data, std::uint32_t seq, ros::Time stamp) { 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); auto &&in = api_->GetIntrinsics(Stream::LEFT);
@ -831,6 +867,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
std::size_t left_count_ = 0; std::size_t left_count_ = 0;
std::size_t right_count_ = 0; std::size_t right_count_ = 0;
std::size_t imu_count_ = 0; std::size_t imu_count_ = 0;
std::map<Stream, bool> is_published_;
bool is_motion_published_;
bool is_started_;
}; };
MYNTEYE_END_NAMESPACE MYNTEYE_END_NAMESPACE