diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch index 3654689..a41e52f 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -38,7 +38,7 @@ - + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index 364a3d4..fccda6d 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -173,6 +173,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { } NODELET_INFO_STREAM(it->first << ": " << api_->GetOptionValue(it->first)); } + frame_rate_ = api_->GetOptionValue(Option::FRAME_RATE); // publishers @@ -227,8 +228,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet { NODELET_INFO_STREAM("Advertized service " << DEVICE_INFO_SERVICE); publishStaticTransforms(); + ros::Rate loop_rate(frame_rate_); while (private_nh_.ok()) { publishTopics(); + loop_rate.sleep(); } } @@ -268,6 +271,84 @@ class ROSWrapperNodelet : public nodelet::Nodelet { return true; } + void SetIsPublished(const Stream &stream) { + is_published_[stream] = false; + switch (stream) { + case Stream::LEFT_RECTIFIED: { + if (is_published_[Stream::RIGHT_RECTIFIED]) { + SetIsPublished(Stream::RIGHT_RECTIFIED); + } + if (is_published_[Stream::DISPARITY]) { + SetIsPublished(Stream::DISPARITY); + } + } break; + case Stream::RIGHT_RECTIFIED: { + if (is_published_[Stream::LEFT_RECTIFIED]) { + SetIsPublished(Stream::RIGHT_RECTIFIED); + } + if (is_published_[Stream::DISPARITY]) { + SetIsPublished(Stream::DISPARITY); + } + } break; + case Stream::DISPARITY: { + if (is_published_[Stream::DISPARITY_NORMALIZED]) { + SetIsPublished(Stream::DISPARITY_NORMALIZED); + } + if (is_published_[Stream::POINTS]) { + SetIsPublished(Stream::POINTS); + } + } break; + case Stream::DISPARITY_NORMALIZED: { + } break; + case Stream::POINTS: { + if (is_published_[Stream::DEPTH]) { + SetIsPublished(Stream::DEPTH); + } + } break; + case Stream::DEPTH: { + } break; + default: + return; + } + } + + void publishPoint(const Stream &stream) { + auto &&points_num = points_publisher_.getNumSubscribers(); + if (points_num == 0 && is_published_[stream]) { + SetIsPublished(stream); + api_->DisableStreamData(stream); + } else if (points_num > 0 && !is_published_[Stream::POINTS]) { + api_->EnableStreamData(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; + } + } + + void publishOthers(const Stream &stream) { + auto stream_num = camera_publishers_[stream].getNumSubscribers(); + if (stream_num == 0 && is_published_[stream]) { + // Stop computing when was not subcribed + SetIsPublished(stream); + api_->DisableStreamData(stream); + } else if (stream_num > 0 && !is_published_[stream]) { + // Start computing and publishing when was subcribed + api_->EnableStreamData(stream); + api_->SetStreamCallback( + stream, [this, stream](const api::StreamData &data) { + // data.img is null, not hard timestamp + static std::size_t count = 0; + ++count; + publishCamera(stream, data, count, ros::Time::now()); + }); + is_published_[stream] = true; + } + } + void publishTopics() { if (camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 && !is_published_[Stream::LEFT]) { @@ -317,33 +398,16 @@ class ROSWrapperNodelet : public nodelet::Nodelet { } std::vector other_streams{ - Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, Stream::DISPARITY, - Stream::DISPARITY_NORMALIZED, Stream::DEPTH}; + Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, + Stream::DISPARITY, Stream::DISPARITY_NORMALIZED, + Stream::POINTS, Stream::DEPTH}; for (auto &&stream : other_streams) { - if (camera_publishers_[stream].getNumSubscribers() == 0 && - is_published_[stream]) { - continue; + if (stream != Stream::POINTS) { + publishOthers(stream); + } else { + publishPoint(stream); } - api_->SetStreamCallback( - stream, [this, stream](const api::StreamData &data) { - // data.img is null, not hard timestamp - static std::size_t count = 0; - ++count; - publishCamera(stream, data, count, ros::Time::now()); - }); - is_published_[stream] = true; - } - - if (points_publisher_.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_) { @@ -882,6 +946,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { std::map is_published_; bool is_motion_published_; bool is_started_; + int frame_rate_; }; MYNTEYE_END_NAMESPACE