Modification of publishing topic only when that was subscribed.

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc

@@ -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_
+                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);
           });
+      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;
+            */
     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;
+            */
 
     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