Visualze Frustum

include/gz/sensors/LogicalCameraSensor.hh

@@ -104,6 +104,14 @@ namespace gz
       /// \return True if there are subscribers, false otherwise
       public: virtual bool HasConnections() const override;
 
+      /// \brief Check if there are any image subscribers
+      /// \return True if there are image subscribers, false otherwise
+      public: virtual bool HasImageConnections() const;
+
+      /// \brief Check if there are any frustum subscribers
+      /// \return True if there are info subscribers, false otherwise
+      public: virtual bool HasFrustumConnections() const;
+
       /// \brief Get the latest image. An image is an instance of
       /// msgs::LogicalCameraImage, which contains a list of detected models.
       /// \return List of detected models.

src/LogicalCameraSensor.cc

@@ -36,9 +36,13 @@ class gz::sensors::LogicalCameraSensorPrivate
   /// \brief node to create publisher
   public: transport::Node node;
 
+  public: transport::Node node_logic;
+
   /// \brief publisher to publish logical camera messages.
   public: transport::Node::Publisher pub;
 
+  public: transport::Node::Publisher pub_logic;
+
   /// \brief true if Load() has been called and was successful
   public: bool initialized = false;
 
@@ -56,6 +60,8 @@ class gz::sensors::LogicalCameraSensorPrivate
 
   /// \brief Msg containg info on models detected by logical camera
   msgs::LogicalCameraImage msg;
+
+  msgs::LogicalCameraSensor msg_logic;
 };
 
 //////////////////////////////////////////////////
@@ -110,12 +116,22 @@ bool LogicalCameraSensor::Load(sdf::ElementPtr _sdf)
       this->dataPtr->node.Advertise<msgs::LogicalCameraImage>(
       this->Topic());
 
+  this->dataPtr->pub_logic =
+      this->dataPtr->node_logic.Advertise<msgs::LogicalCameraSensor>(
+      this->Topic() + "/frustum");
+
   if (!this->dataPtr->pub)
   {
     gzerr << "Unable to create publisher on topic[" << this->Topic() << "].\n";
     return false;
   }
 
+  if (!this->dataPtr->pub_logic)
+  {
+    gzerr << "Unable to create publisher on topic[" << this->Topic() << "].\n";
+    return false;
+  }
+
   gzdbg << "Logical images for [" << this->Name() << "] advertised on ["
          << this->Topic() << "]" << std::endl;
 
@@ -166,9 +182,25 @@ bool LogicalCameraSensor::Update(
   frame->set_key("frame_id");
   frame->add_value(this->FrameId());
 
+  *this->dataPtr->msg_logic.mutable_header()->mutable_stamp() =
+    msgs::Convert(_now);
+  this->dataPtr->msg_logic.mutable_header()->clear_data();
+  auto frame_log = this->dataPtr->msg_logic.mutable_header()->add_data();
+
+  frame_log->set_key("frame_id");
+  frame_log->add_value(this->FrameId());
+
   // publish
+  this->dataPtr->msg_logic.set_near_clip(this->dataPtr->frustum.Near());
+  this->dataPtr->msg_logic.set_far_clip(this->dataPtr->frustum.Far());
+  this->dataPtr->msg_logic.set_horizontal_fov(
+    this->dataPtr->frustum.FOV().Radian());
+  this->dataPtr->msg_logic.set_aspect_ratio(
+    this->dataPtr->frustum.AspectRatio());
   this->AddSequence(this->dataPtr->msg.mutable_header());
+
   this->dataPtr->pub.Publish(this->dataPtr->msg);
+  this->dataPtr->pub_logic.Publish(this->dataPtr->msg_logic);
 
   return true;
 }
@@ -206,7 +238,18 @@ msgs::LogicalCameraImage LogicalCameraSensor::Image() const
 
 //////////////////////////////////////////////////
 bool LogicalCameraSensor::HasConnections() const
+{
+  return this->HasImageConnections() || this->HasFrustumConnections();
+}
+
+//////////////////////////////////////////////////
+bool LogicalCameraSensor::HasImageConnections() const
 {
   return this->dataPtr->pub && this->dataPtr->pub.HasConnections();
 }
 
+//////////////////////////////////////////////////
+bool LogicalCameraSensor::HasFrustumConnections() const
+{
+  return this->dataPtr->pub_logic && this->dataPtr->pub_logic.HasConnections();
+}