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..66ee4463 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -36,9 +36,15 @@ class gz::sensors::LogicalCameraSensorPrivate /// \brief node to create publisher public: transport::Node node; + /// \brief node to create publisher for frustum + public: transport::Node nodeLogic; + /// \brief publisher to publish logical camera messages. public: transport::Node::Publisher pub; + /// \brief Publisher to publish logical camera frustum information + public: transport::Node::Publisher pubLogic; + /// \brief true if Load() has been called and was successful public: bool initialized = false; @@ -55,7 +61,10 @@ class gz::sensors::LogicalCameraSensorPrivate public: std::map models; /// \brief Msg containg info on models detected by logical camera - msgs::LogicalCameraImage msg; + public: msgs::LogicalCameraImage msg; + + /// \brief Msg containing logical camera frustum info + public: msgs::LogicalCameraSensor msgLogic; }; ////////////////////////////////////////////////// @@ -110,12 +119,23 @@ bool LogicalCameraSensor::Load(sdf::ElementPtr _sdf) this->dataPtr->node.Advertise( this->Topic()); + this->dataPtr->pubLogic = + this->dataPtr->nodeLogic.Advertise( + this->Topic() + "/frustum"); + if (!this->dataPtr->pub) { gzerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; return false; } + if (!this->dataPtr->pubLogic) + { + gzerr << "Unable to create publisher on topic[" << this->Topic() + << "/frustum].\n"; + return false; + } + gzdbg << "Logical images for [" << this->Name() << "] advertised on [" << this->Topic() << "]" << std::endl; @@ -166,9 +186,25 @@ bool LogicalCameraSensor::Update( frame->set_key("frame_id"); frame->add_value(this->FrameId()); + *this->dataPtr->msgLogic.mutable_header()->mutable_stamp() = + msgs::Convert(_now); + this->dataPtr->msgLogic.mutable_header()->clear_data(); + auto frame_log = this->dataPtr->msgLogic.mutable_header()->add_data(); + + frame_log->set_key("frame_id"); + frame_log->add_value(this->FrameId()); + // publish + this->dataPtr->msgLogic.set_near_clip(this->dataPtr->frustum.Near()); + this->dataPtr->msgLogic.set_far_clip(this->dataPtr->frustum.Far()); + this->dataPtr->msgLogic.set_horizontal_fov( + this->dataPtr->frustum.FOV().Radian()); + this->dataPtr->msgLogic.set_aspect_ratio( + this->dataPtr->frustum.AspectRatio()); this->AddSequence(this->dataPtr->msg.mutable_header()); + this->dataPtr->pub.Publish(this->dataPtr->msg); + this->dataPtr->pubLogic.Publish(this->dataPtr->msgLogic); return true; } @@ -206,7 +242,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->pubLogic && this->dataPtr->pubLogic.HasConnections(); +}