diff --git a/include/gz/sensors/LogicalCameraSensor.hh b/include/gz/sensors/LogicalCameraSensor.hh index f7e10449..3a5a7b54 100644 --- a/include/gz/sensors/LogicalCameraSensor.hh +++ b/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. diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index 6aab67ce..939dd08c 100644 --- a/src/LogicalCameraSensor.cc +++ b/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( this->Topic()); + this->dataPtr->pub_logic = + this->dataPtr->node_logic.Advertise( + 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,22 @@ 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 +235,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(); +}