Move trigger logic to base Sensor class and enable trigger for all ca… (

#431)



---------

Signed-off-by: Shameek Ganguly <[email protected]>

include/gz/sensors/BoundingBoxCameraSensor.hh

@@ -103,10 +103,6 @@ namespace gz
       /// \return True on success.
       private: bool CreateCamera();
 
-      /// \brief Callback for triggered subscription
-      /// \param[in] _msg Boolean message
-      private: void OnTrigger(const gz::msgs::Boolean &/*_msg*/);
-
       GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
       /// \brief Data pointer for private data
       /// \internal

include/gz/sensors/CameraSensor.hh

@@ -178,10 +178,6 @@ namespace gz
       /// \param[in] _scene Pointer to the new scene.
       private: void OnSceneChange(gz::rendering::ScenePtr /*_scene*/);
 
-      /// \brief Callback for triggered subscription
-      /// \param[in] _msg Boolean message
-      private: void OnTrigger(const gz::msgs::Boolean &/*_msg*/);
-
       GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
       /// \brief Data pointer for private data
       /// \internal

include/gz/sensors/Sensor.hh

@@ -113,8 +113,8 @@ namespace gz
       ///   subclasses' Update() method will be called.
       /// \param[in] _now The current time
       /// \param[in] _force Force the update to happen even if it's not time
-      /// \return True if the update was triggered (_force was true or _now
-      /// >= next_update_time) and the sensor's
+      /// \return True if the update was performed (_force was true or _now
+      /// >= next_update_time or sensor had a pending trigger) and the sensor's
       /// bool Sensor::Update(std::chrono::steady_clock::time_point)
       /// function returned true.
       /// False otherwise.
@@ -231,6 +231,28 @@ namespace gz
       /// \return True if there are subscribers, false otherwise
       public: virtual bool HasConnections() const;
 
+      /// \brief Enable or disable triggered mode. In this mode,
+      /// - the sensor will only update if a new message has been published to
+      ///   the passed _triggerTopic since the last update,
+      /// - until the next message is published on _triggerTopic, all Update
+      ///   calls will return false, and
+      /// - if the sensor has a pending trigger, the next Update call will
+      ///   ignore the sensor's update rate and try generate data immediately.
+      /// \param[in] _triggered Sets triggered mode if true and disables it if
+      /// false.
+      /// \param[in] _triggerTopic The topic on which the sensor will listen
+      /// for trigger messages in triggered mode. If _triggered is true, this
+      /// value should not be empty. If _triggered is false, this value is
+      /// ignored.
+      /// \return True if the operation succeeded.
+      public: bool SetTriggered(bool _triggered,
+                                const std::string &_triggerTopic = "");
+
+      /// \brief Whether the sensor has a pending trigger.
+      /// \return True if the sensor is in trigger mode and has a pending
+      /// trigger.
+      public: bool HasPendingTrigger() const;
+
       GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
       /// \internal
       /// \brief Data pointer for private data

src/BoundingBoxCameraSensor.cc

@@ -16,6 +16,8 @@
 */
 
 #include <mutex>
+#include <ostream>
+#include <string>
 
 #include <gz/msgs/boolean.pb.h>
 #include <gz/msgs/image.pb.h>
@@ -32,6 +34,7 @@
 #include <gz/rendering/BoundingBoxCamera.hh>
 #include <gz/transport/Node.hh>
 #include <gz/transport/Publisher.hh>
+#include <gz/transport/TopicUtils.hh>
 
 #include "gz/sensors/BoundingBoxCameraSensor.hh"
 #include "gz/sensors/RenderingEvents.hh"
@@ -88,15 +91,6 @@ class gz::sensors::BoundingBoxCameraSensorPrivate
   /// \brief Just a mutex for thread safety
   public: std::mutex mutex;
 
-  /// \brief True if camera is triggered by a topic
-  public: bool isTriggeredCamera{false};
-
-  /// \brief True if camera has been triggered by a topic
-  public: bool isTriggered{false};
-
-  /// \brief Topic for camera trigger
-  public: std::string triggerTopic{""};
-
   /// \brief BoundingBoxes type
   public: rendering::BoundingBoxType type
     {rendering::BoundingBoxType::BBT_VISIBLEBOX2D};
@@ -231,30 +225,13 @@ bool BoundingBoxCameraSensor::Load(const sdf::Sensor &_sdf)
 
   if (_sdf.CameraSensor()->Triggered())
   {
-    if (!_sdf.CameraSensor()->TriggerTopic().empty())
-    {
-      this->dataPtr->triggerTopic = _sdf.CameraSensor()->TriggerTopic();
-    }
-    else
+    std::string triggerTopic = _sdf.CameraSensor()->TriggerTopic();
+    if (triggerTopic.empty())
     {
-      this->dataPtr->triggerTopic =
-          transport::TopicUtils::AsValidTopic(
-          this->Topic() + "/trigger");
-
-      if (this->dataPtr->triggerTopic.empty())
-      {
-        gzerr << "Invalid trigger topic name [" <<
-        this->dataPtr->triggerTopic << "]" << std::endl;
-        return false;
-      }
+      triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() +
+                                                         "/trigger");
     }
-
-    this->dataPtr->node.Subscribe(this->dataPtr->triggerTopic,
-        &BoundingBoxCameraSensor::OnTrigger, this);
-
-    gzdbg << "Camera trigger messages for [" << this->Name() << "] subscribed"
-          << " on [" << this->dataPtr->triggerTopic << "]" << std::endl;
-    this->dataPtr->isTriggeredCamera = true;
+    this->SetTriggered(true, triggerTopic);
   }
 
   if (!this->AdvertiseInfo())
@@ -430,16 +407,6 @@ bool BoundingBoxCameraSensor::Update(
     this->PublishInfo(_now);
   }
 
-  // render only if necessary
-  {
-    std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
-    if (this->dataPtr->isTriggeredCamera &&
-        !this->dataPtr->isTriggered)
-    {
-      return true;
-    }
-  }
-
   // don't render if there are no subscribers nor saving
   if (!this->dataPtr->imagePublisher.HasConnections() &&
     !this->dataPtr->boxesPublisher.HasConnections() &&
@@ -578,11 +545,6 @@ bool BoundingBoxCameraSensor::Update(
     ++this->dataPtr->saveCounter;
   }
 
-  if (this->dataPtr->isTriggeredCamera)
-  {
-    return this->dataPtr->isTriggered = false;
-  }
-
   return true;
 }
 
@@ -598,13 +560,6 @@ unsigned int BoundingBoxCameraSensor::ImageWidth() const
   return this->dataPtr->rgbCamera->ImageWidth();
 }
 
-//////////////////////////////////////////////////
-void BoundingBoxCameraSensor::OnTrigger(const gz::msgs::Boolean &/*_msg*/)
-{
-  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
-  this->dataPtr->isTriggered = true;
-}
-
 //////////////////////////////////////////////////
 void BoundingBoxCameraSensorPrivate::SaveImage()
 {

src/CameraSensor.cc

@@ -20,6 +20,8 @@
 #include <gz/msgs/image.pb.h>
 
 #include <mutex>
+#include <ostream>
+#include <string>
 
 #include <gz/common/Console.hh>
 #include <gz/common/Event.hh>
@@ -30,6 +32,7 @@
 #include <gz/math/Helpers.hh>
 #include <gz/msgs/Utility.hh>
 #include <gz/transport/Node.hh>
+#include <gz/transport/TopicUtils.hh>
 
 #include "gz/sensors/CameraSensor.hh"
 #include "gz/sensors/ImageBrownDistortionModel.hh"
@@ -159,15 +162,6 @@ class gz::sensors::CameraSensorPrivate
   /// \brief Just a mutex for thread safety
   public: std::mutex mutex;
 
-  /// \brief True if camera is triggered by a topic
-  public: bool isTriggeredCamera = false;
-
-  /// \brief True if camera has been triggered by a topic
-  public: bool isTriggered = false;
-
-  /// \brief Topic for camera trigger
-  public: std::string triggerTopic = "";
-
   /// \brief True to save images
   public: bool saveImage = false;
 
@@ -460,29 +454,13 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf)
 
   if (_sdf.CameraSensor()->Triggered())
   {
-    if (!_sdf.CameraSensor()->TriggerTopic().empty())
+    std::string triggerTopic = _sdf.CameraSensor()->TriggerTopic();
+    if (triggerTopic.empty())
     {
-      this->dataPtr->triggerTopic = _sdf.CameraSensor()->TriggerTopic();
+      triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() +
+                                                         "/trigger");
     }
-    else
-    {
-      this->dataPtr->triggerTopic =
-          transport::TopicUtils::AsValidTopic(
-          this->Topic() + "/trigger");
-
-      if (this->dataPtr->triggerTopic.empty())
-      {
-        gzerr << "Invalid trigger topic name" << std::endl;
-        return false;
-      }
-    }
-
-    this->dataPtr->node.Subscribe(this->dataPtr->triggerTopic,
-        &CameraSensor::OnTrigger, this);
-
-    gzdbg << "Camera trigger messages for [" << this->Name() << "] subscribed"
-           << " on [" << this->dataPtr->triggerTopic << "]" << std::endl;
-    this->dataPtr->isTriggeredCamera = true;
+    this->SetTriggered(true, triggerTopic);
   }
 
   if (!this->AdvertiseInfo())
@@ -556,13 +534,6 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
     this->PublishInfo(_now);
   }
 
-  // render only if necessary
-  if (this->dataPtr->isTriggeredCamera &&
-      !this->dataPtr->isTriggered)
-  {
-    return true;
-  }
-
   if (!this->dataPtr->pub.HasConnections() &&
       this->dataPtr->imageEvent.ConnectionCount() <= 0 &&
       !this->dataPtr->saveImage)
@@ -683,21 +654,9 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
     }
   }
 
-  if (this->dataPtr->isTriggeredCamera)
-  {
-    return this->dataPtr->isTriggered = false;
-  }
-
   return true;
 }
 
-//////////////////////////////////////////////////
-void CameraSensor::OnTrigger(const gz::msgs::Boolean &/*_msg*/)
-{
-  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
-  this->dataPtr->isTriggered = true;
-}
-
 //////////////////////////////////////////////////
 bool CameraSensorPrivate::SaveImage(const unsigned char *_data,
     unsigned int _width, unsigned int _height,

src/DepthCameraSensor.cc

@@ -282,6 +282,17 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf)
   gzdbg << "Depth images for [" << this->Name() << "] advertised on ["
          << this->Topic() << "]" << std::endl;
 
+  if (_sdf.CameraSensor()->Triggered())
+  {
+    std::string triggerTopic = _sdf.CameraSensor()->TriggerTopic();
+    if (triggerTopic.empty())
+    {
+      triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() +
+                                                         "/trigger");
+    }
+    this->SetTriggered(true, triggerTopic);
+  }
+
   if (!this->AdvertiseInfo())
     return false;
 

src/RgbdCameraSensor.cc

@@ -237,6 +237,17 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf)
   gzdbg << "Points for [" << this->Name() << "] advertised on ["
          << this->Topic() << "/points]" << std::endl;
 
+  if (_sdf.CameraSensor()->Triggered())
+  {
+    std::string triggerTopic = _sdf.CameraSensor()->TriggerTopic();
+    if (triggerTopic.empty())
+    {
+      triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() +
+                                                         "/trigger");
+    }
+    this->SetTriggered(true, triggerTopic);
+  }
+
   if (!this->AdvertiseInfo(this->Topic() + "/camera_info"))
     return false;
 

src/SegmentationCameraSensor.cc

@@ -223,6 +223,17 @@ bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf)
     << "] advertised on [" << this->Topic()
     << this->dataPtr->topicLabelsMapSuffix << "]\n";
 
+  if (_sdf.CameraSensor()->Triggered())
+  {
+    std::string triggerTopic = _sdf.CameraSensor()->TriggerTopic();
+    if (triggerTopic.empty())
+    {
+      triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() +
+                                                         "/trigger");
+    }
+    this->SetTriggered(true, triggerTopic);
+  }
+
   // TODO(anyone) Access the info topic from the parent class
   if (!this->AdvertiseInfo(this->Topic() + "/camera_info"))
     return false;