Merge branch into develop

include/mynteye/api/api.h

@@ -92,6 +92,8 @@ class MYNTEYE_API API {
   using stream_callback_t = std::function<void(const api::StreamData &data)>;
   /** The api::MotionData callback. */
   using motion_callback_t = std::function<void(const api::MotionData &data)>;
+  /** The enable/disable switch callback. */
+  using stream_switch_callback_t = std::function<void(const Stream &stream)>;
 
   explicit API(std::shared_ptr<Device> device, CalibrationModel calib_model);
   virtual ~API();
@@ -281,11 +283,31 @@ class MYNTEYE_API API {
    * still support this stream.
    */
   void EnableStreamData(const Stream &stream);
+
+  /**
+   * Enable the data of stream.
+   * callback function will call before the father processor enable.
+   * when try_tag is true, the function will do nothing except callback. 
+   */
+  void EnableStreamData(
+      const Stream &stream,
+      stream_switch_callback_t callback,
+      bool try_tag = false);
   /**
    * Disable the data of stream.
    */
   void DisableStreamData(const Stream &stream);
 
+  /**
+   * Disable the data of stream.
+   * callback function will call before the children processor disable.
+   * when try_tag is true, the function will do nothing except callback. 
+   */
+  void DisableStreamData(
+      const Stream &stream,
+      stream_switch_callback_t callback,
+      bool try_tag = false);
+
   /**
    * Get the latest data of stream.
    */

src/mynteye/api/api.cc

@@ -456,6 +456,17 @@ void API::DisableStreamData(const Stream &stream) {
   synthetic_->DisableStreamData(stream);
 }
 
+void API::EnableStreamData(
+    const Stream &stream, stream_switch_callback_t callback,
+    bool try_tag) {
+  synthetic_->EnableStreamData(stream, callback, try_tag);
+}
+void API::DisableStreamData(
+    const Stream &stream, stream_switch_callback_t callback,
+    bool try_tag) {
+  synthetic_->DisableStreamData(stream, callback, try_tag);
+}
+
 api::StreamData API::GetStreamData(const Stream &stream) {
   if (correspondence_ && correspondence_->Watch(stream)) {
     return correspondence_->GetStreamData(stream);

src/mynteye/api/synthetic.cc

@@ -238,27 +238,6 @@ bool Synthetic::checkControlDateWithStream(const Stream& stream) const {
   return false;
 }
 
-void Synthetic::EnableStreamData(const Stream &stream) {
-  // Activate processors of synthetic stream
-  auto processor = getProcessorWithStream(stream);
-  iterate_processor_CtoP_before(processor,
-      [](std::shared_ptr<Processor> proce){
-        auto streams = proce->getTargetStreams();
-        int act_tag = 0;
-        for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) {
-          if (proce->target_streams_[i].enabled_mode_ == MODE_LAST) {
-            act_tag++;
-            proce->target_streams_[i].enabled_mode_ = MODE_SYNTHETIC;
-          }
-        }
-        if (act_tag > 0 && !proce->IsActivated()) {
-          // std::cout << proce->Name() << " Active now" << std::endl;
-          proce->Activate();
-        }
-      });
-}
-
-
 bool Synthetic::Supports(const Stream &stream) const {
   return checkControlDateWithStream(stream);
 }
@@ -271,18 +250,47 @@ Synthetic::mode_t Synthetic::SupportsMode(const Stream &stream) const {
   return MODE_LAST;
 }
 
-void Synthetic::DisableStreamData(const Stream &stream) {
+void Synthetic::EnableStreamData(
+    const Stream &stream, stream_switch_callback_t callback,
+    bool try_tag) {
+  // Activate processors of synthetic stream
+  auto processor = getProcessorWithStream(stream);
+  iterate_processor_CtoP_before(processor,
+      [callback, try_tag](std::shared_ptr<Processor> proce){
+        auto streams = proce->getTargetStreams();
+        int act_tag = 0;
+        for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) {
+          if (proce->target_streams_[i].enabled_mode_ == MODE_LAST) {
+            callback(proce->target_streams_[i].stream);
+            if (!try_tag) {
+              act_tag++;
+              proce->target_streams_[i].enabled_mode_ = MODE_SYNTHETIC;
+            }
+          }
+        }
+        if (act_tag > 0 && !proce->IsActivated()) {
+          // std::cout << proce->Name() << " Active now" << std::endl;
+          proce->Activate();
+        }
+      });
+}
+void Synthetic::DisableStreamData(
+    const Stream &stream, stream_switch_callback_t callback,
+    bool try_tag) {
   auto processor = getProcessorWithStream(stream);
   iterate_processor_PtoC_before(processor,
-      [](std::shared_ptr<Processor> proce){
+      [callback, try_tag](std::shared_ptr<Processor> proce){
         auto streams = proce->getTargetStreams();
         int act_tag = 0;
         for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) {
           if (proce->target_streams_[i].enabled_mode_ == MODE_SYNTHETIC) {
+            callback(proce->target_streams_[i].stream);
+            if (!try_tag) {
               act_tag++;
               proce->target_streams_[i].enabled_mode_ = MODE_LAST;
             }
           }
+        }
         if (act_tag > 0 && proce->IsActivated()) {
           // std::cout << proce->Name() << "Deactive now" << std::endl;
           proce->Deactivate();
@@ -290,6 +298,20 @@ void Synthetic::DisableStreamData(const Stream &stream) {
       });
 }
 
+void Synthetic::EnableStreamData(const Stream &stream) {
+  EnableStreamData(stream, [](const Stream &stream){
+        // std::cout << stream << "enabled in callback" << std::endl;
+        MYNTEYE_UNUSED(stream);
+      }, false);
+}
+
+void Synthetic::DisableStreamData(const Stream &stream) {
+  DisableStreamData(stream, [](const Stream &stream){
+        // std::cout << stream << "disabled in callback" << std::endl;
+        MYNTEYE_UNUSED(stream);
+      }, false);
+}
+
 bool Synthetic::IsStreamDataEnabled(const Stream &stream) const {
   if (checkControlDateWithStream(stream)) {
     auto data = getControlDateWithStream(stream);

src/mynteye/api/synthetic.h

@@ -37,6 +37,7 @@ class Synthetic {
   using stream_callback_t = API::stream_callback_t;
   using stream_data_listener_t =
       std::function<void(const Stream &stream, const api::StreamData &data)>;
+  using stream_switch_callback_t = API::stream_switch_callback_t;
 
   typedef enum Mode {
     MODE_NATIVE,     // Native stream
@@ -63,6 +64,11 @@ class Synthetic {
 
   void EnableStreamData(const Stream &stream);
   void DisableStreamData(const Stream &stream);
+
+  void EnableStreamData(
+      const Stream &stream, stream_switch_callback_t callback, bool try_tag);
+  void DisableStreamData(
+      const Stream &stream, stream_switch_callback_t callback, bool try_tag);
   bool IsStreamDataEnabled(const Stream &stream) const;
 
   void SetStreamCallback(const Stream &stream, stream_callback_t callback);

wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch

@@ -347,6 +347,10 @@
         - 'image_transport/compressedDepth'
       </rosparam>
     </group>
-
+    <group ns="$(arg depth_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
   </group> <!-- mynteye -->
 </launch>

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

@@ -554,75 +554,41 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     return true;
   }
 
-  void SetIsPublished(const Stream &stream) {
+  void publishData(
-    is_published_[stream] = false;
+      const Stream &stream, const api::StreamData &data, std::uint32_t seq,
-    switch (stream) {
+      ros::Time stamp) {
-      case Stream::LEFT_RECTIFIED: {
+    if (stream == Stream::LEFT || stream == Stream::RIGHT) {
-        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::LEFT_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;
+    } else if (stream == Stream::POINTS) {
+      publishPoints(data, seq, stamp);
+    } else {
+      publishCamera(stream, data, seq, stamp);
     }
   }
 
-  void publishPoint(const Stream &stream) {
+  int getStreamSubscribers(const Stream &stream) {
-    auto &&points_num = points_publisher_.getNumSubscribers();
+    if (stream == Stream::POINTS) {
-    if (points_num == 0 && is_published_[stream]) {
+      return points_publisher_.getNumSubscribers();
-      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) {
-            // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
-            ros::Time stamp = checkUpTimeStamp(
-                data.img->timestamp, Stream::POINTS);
-            static std::size_t count = 0;
-            ++count;
-            publishPoints(data, count, stamp);
-          });
-      is_published_[Stream::POINTS] = true;
     }
+    auto pub = camera_publishers_[stream];
+    if (pub)
+      return pub.getNumSubscribers();
+    return -1;
   }
 
   void publishOthers(const Stream &stream) {
-    auto stream_num = camera_publishers_[stream].getNumSubscribers();
+    // std::cout << stream << "===============================" << std::endl;
-    if (stream_num == 0 && is_published_[stream]) {
+    // int enable_tag = 0;
-      // Stop computing when was not subcribed
+    // api_->EnableStreamData(stream, [&](const Stream &stream) {
-      SetIsPublished(stream);
+    //       enable_tag += getStreamSubscribers(stream);
-      api_->DisableStreamData(stream);
+    //       std::cout << "EnableStreamData callback test"
-    } else if (stream_num > 0 && !is_published_[stream]) {
+    //                 << stream << "|| Sum:"
-      // Start computing and publishing when was subcribed
+    //                 << 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) {
@@ -631,13 +597,54 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
                 data.img->timestamp, stream);
             static std::size_t count = 0;
             ++count;
-            publishCamera(stream, data, count, stamp);
+            publishData(stream, data, count, stamp);
           });
       is_published_[stream] = true;
+      return;
+    }
+
+    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;
+          });
+      return;
     }
   }
 
   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 (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]) {
@@ -698,15 +705,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
         std::vector<Stream> other_streams{
             Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED,
             Stream::DISPARITY,      Stream::DISPARITY_NORMALIZED,
-        Stream::POINTS,         Stream::DEPTH};
+            Stream::POINTS,         Stream::DEPTH
-
+            };
         for (auto &&stream : other_streams) {
-      if (stream != Stream::POINTS) {
           publishOthers(stream);
-      } else {
-        publishPoint(stream);
         }
       }
+      sum = sum_c;
+    }
 
     if (!is_motion_published_) {
       api_->SetMotionCallback([this](const api::MotionData &data) {