From b5f6b21f68974f51a03e6421569517575f9a711a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 6 Dec 2022 21:41:13 +0100 Subject: [PATCH 1/3] Fix navsat frame id (#295) (#298) Signed-off-by: quentinGllmt --- src/NavSatSensor.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/NavSatSensor.cc b/src/NavSatSensor.cc index 414ee2ef..a969a8ac 100644 --- a/src/NavSatSensor.cc +++ b/src/NavSatSensor.cc @@ -158,7 +158,7 @@ bool NavSatSensor::Update(const std::chrono::steady_clock::duration &_now) msgs::NavSat msg; *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); - msg.set_frame_id(this->Name()); + msg.set_frame_id(this->FrameId()); // Apply noise auto iter = this->dataPtr->noises.find(NAVSAT_HORIZONTAL_POSITION_NOISE); From eb0e71f6225eac27c41e52f2733cb790cdd807bf Mon Sep 17 00:00:00 2001 From: Alex <90333090+alex-ssom@users.noreply.github.com> Date: Fri, 13 Jan 2023 18:26:23 -0500 Subject: [PATCH 2/3] CameraInfo is now published when there's a CameraSensor subscriber (#308) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Fixes #307 The CameraInfo listener is now taken into account in the CameraSensor::Update() method. I added infoPub.HasConnections() where required in CameraSensor.cc as per gazebosim/ros_gz#346 (comment). Signed-off-by: alex Co-authored-by: Alejandro Hernández Cordero --- src/CameraSensor.cc | 151 +++++++++++++++++++++++--------------------- 1 file changed, 80 insertions(+), 71 deletions(-) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 2a4d84b2..2b9e3f57 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -394,6 +394,12 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) // move the camera to the current pose this->dataPtr->camera->SetLocalPose(this->Pose()); + if (this->dataPtr->infoPub.HasConnections()) + { + // publish the camera info message + this->PublishInfo(_now); + } + // render only if necessary if (this->dataPtr->isTriggeredCamera && !this->dataPtr->isTriggered) @@ -408,7 +414,7 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) if (this->dataPtr->generatingData) { igndbg << "Disabling camera sensor: '" << this->Name() << "' data " - << "generation. " << std::endl;; + << "generation. " << std::endl; this->dataPtr->generatingData = false; } @@ -419,90 +425,92 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) if (!this->dataPtr->generatingData) { igndbg << "Enabling camera sensor: '" << this->Name() << "' data " - << "generation." << std::endl;; + << "generation." << std::endl; this->dataPtr->generatingData = true; } } - // generate sensor data - this->Render(); + if ((this->dataPtr->pub && this->dataPtr->pub.HasConnections()) || + this->dataPtr->imageEvent.ConnectionCount() > 0u || + this->dataPtr->saveImage) { - IGN_PROFILE("CameraSensor::Update Copy image"); - this->dataPtr->camera->Copy(this->dataPtr->image); - } - - unsigned int width = this->dataPtr->camera->ImageWidth(); - unsigned int height = this->dataPtr->camera->ImageHeight(); - unsigned char *data = this->dataPtr->image.Data(); + // generate sensor data + this->Render(); + { + IGN_PROFILE("CameraSensor::Update Copy image"); + this->dataPtr->camera->Copy(this->dataPtr->image); + } - common::Image::PixelFormatType - format{common::Image::UNKNOWN_PIXEL_FORMAT}; - msgs::PixelFormatType msgsPixelFormat = - msgs::PixelFormatType::UNKNOWN_PIXEL_FORMAT; + unsigned int width = this->dataPtr->camera->ImageWidth(); + unsigned int height = this->dataPtr->camera->ImageHeight(); + unsigned char *data = this->dataPtr->image.Data(); - switch (this->dataPtr->camera->ImageFormat()) - { - case rendering::PF_R8G8B8: - format = common::Image::RGB_INT8; - msgsPixelFormat = msgs::PixelFormatType::RGB_INT8; - break; - case rendering::PF_L8: - format = common::Image::L_INT8; - msgsPixelFormat = msgs::PixelFormatType::L_INT8; - break; - case rendering::PF_L16: - format = common::Image::L_INT16; - msgsPixelFormat = msgs::PixelFormatType::L_INT16; - break; - default: - ignerr << "Unsupported pixel format [" - << this->dataPtr->camera->ImageFormat() << "]\n"; - break; - } - - // create message - msgs::Image msg; - { - IGN_PROFILE("CameraSensor::Update Message"); - msg.set_width(width); - msg.set_height(height); - msg.set_step(width * rendering::PixelUtil::BytesPerPixel( - this->dataPtr->camera->ImageFormat())); - msg.set_pixel_format_type(msgsPixelFormat); - *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); - auto frame = msg.mutable_header()->add_data(); - frame->set_key("frame_id"); - frame->add_value(this->dataPtr->opticalFrameId); - msg.set_data(data, this->dataPtr->camera->ImageMemorySize()); - } + common::Image::PixelFormatType + format{common::Image::UNKNOWN_PIXEL_FORMAT}; + msgs::PixelFormatType msgsPixelFormat = + msgs::PixelFormatType::UNKNOWN_PIXEL_FORMAT; - // publish the image message - { - this->AddSequence(msg.mutable_header()); - IGN_PROFILE("CameraSensor::Update Publish"); - this->dataPtr->pub.Publish(msg); + switch (this->dataPtr->camera->ImageFormat()) + { + case rendering::PF_R8G8B8: + format = common::Image::RGB_INT8; + msgsPixelFormat = msgs::PixelFormatType::RGB_INT8; + break; + case rendering::PF_L8: + format = common::Image::L_INT8; + msgsPixelFormat = msgs::PixelFormatType::L_INT8; + break; + case rendering::PF_L16: + format = common::Image::L_INT16; + msgsPixelFormat = msgs::PixelFormatType::L_INT16; + break; + default: + ignerr << "Unsupported pixel format [" + << this->dataPtr->camera->ImageFormat() << "]\n"; + break; + } - // publish the camera info message - this->PublishInfo(_now); - } + // create message + msgs::Image msg; + { + IGN_PROFILE("CameraSensor::Update Message"); + msg.set_width(width); + msg.set_height(height); + msg.set_step(width * rendering::PixelUtil::BytesPerPixel( + this->dataPtr->camera->ImageFormat())); + msg.set_pixel_format_type(msgsPixelFormat); + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->dataPtr->opticalFrameId); + msg.set_data(data, this->dataPtr->camera->ImageMemorySize()); + } - // Trigger callbacks. - if (this->dataPtr->imageEvent.ConnectionCount() > 0) - { - try + // publish the image message { - this->dataPtr->imageEvent(msg); + this->AddSequence(msg.mutable_header()); + IGN_PROFILE("CameraSensor::Update Publish"); + this->dataPtr->pub.Publish(msg); } - catch(...) + + // Trigger callbacks. + if (this->dataPtr->imageEvent.ConnectionCount() > 0) { - ignerr << "Exception thrown in an image callback.\n"; + try + { + this->dataPtr->imageEvent(msg); + } + catch(...) + { + ignerr << "Exception thrown in an image callback.\n"; + } } - } - // Save image - if (this->dataPtr->saveImage) - { - this->dataPtr->SaveImage(data, width, height, format); + // Save image + if (this->dataPtr->saveImage) + { + this->dataPtr->SaveImage(data, width, height, format); + } } if (this->dataPtr->isTriggeredCamera) @@ -728,5 +736,6 @@ double CameraSensor::Baseline() const bool CameraSensor::HasConnections() const { return (this->dataPtr->pub && this->dataPtr->pub.HasConnections()) || - this->dataPtr->imageEvent.ConnectionCount() > 0u; + this->dataPtr->imageEvent.ConnectionCount() > 0u || + (this->dataPtr->infoPub && this->dataPtr->infoPub.HasConnections()); } From d65b93a8a0ea4e3ea947006b013670ca1c124ebe Mon Sep 17 00:00:00 2001 From: Alex <90333090+alex-ssom@users.noreply.github.com> Date: Tue, 24 Jan 2023 10:55:01 -0500 Subject: [PATCH 3/3] Add HasInfoConnections() method to expose infoPub.HasConnections() to child of CameraSensor class (#310) Signed-off-by: alex --- include/ignition/sensors/CameraSensor.hh | 10 ++++ include/ignition/sensors/DepthCameraSensor.hh | 10 ++++ include/ignition/sensors/RgbdCameraSensor.hh | 15 ++++++ src/BoundingBoxCameraSensor.cc | 10 +++- src/CameraSensor.cc | 23 +++++++--- src/DepthCameraSensor.cc | 33 ++++++++++--- src/RgbdCameraSensor.cc | 46 +++++++++++++++---- src/SegmentationCameraSensor.cc | 10 +++- src/Sensor.cc | 4 +- src/ThermalCameraSensor.cc | 12 +++-- 10 files changed, 141 insertions(+), 32 deletions(-) diff --git a/include/ignition/sensors/CameraSensor.hh b/include/ignition/sensors/CameraSensor.hh index de02c442..c94e1209 100644 --- a/include/ignition/sensors/CameraSensor.hh +++ b/include/ignition/sensors/CameraSensor.hh @@ -143,6 +143,16 @@ namespace ignition /// \todo(iche033) Make this function virtual on Garden public: bool HasConnections() const; + /// \brief Check if there are any image subscribers + /// \return True if there are image subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasImageConnections() const; + + /// \brief Check if there are any info subscribers + /// \return True if there are info subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasInfoConnections() const; + /// \brief Advertise camera info topic. /// \return True if successful. protected: bool AdvertiseInfo(); diff --git a/include/ignition/sensors/DepthCameraSensor.hh b/include/ignition/sensors/DepthCameraSensor.hh index 7823bd2f..33e0c9f1 100644 --- a/include/ignition/sensors/DepthCameraSensor.hh +++ b/include/ignition/sensors/DepthCameraSensor.hh @@ -162,6 +162,16 @@ namespace ignition /// \todo(iche033) Make this function virtual on Garden public: bool HasConnections() const; + /// \brief Check if there are any depth subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasDepthConnections() const; + + /// \brief Check if there are any point subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasPointConnections() const; + /// \brief Create a camera in a scene /// \return True on success. private: bool CreateCamera(); diff --git a/include/ignition/sensors/RgbdCameraSensor.hh b/include/ignition/sensors/RgbdCameraSensor.hh index b5ee7255..12b05bdb 100644 --- a/include/ignition/sensors/RgbdCameraSensor.hh +++ b/include/ignition/sensors/RgbdCameraSensor.hh @@ -95,6 +95,21 @@ namespace ignition /// \todo(iche033) Make this function virtual on Garden public: bool HasConnections() const; + /// \brief Check if there are color subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasColorConnections() const; + + /// \brief Check if there are depth subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasDepthConnections() const; + + /// \brief Check if there are point cloud subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasPointConnections() const; + /// \brief Create an RGB camera and a depth camera. /// \return True on success. private: bool CreateCameras(); diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index adafd9da..e5812c9b 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -383,6 +383,12 @@ bool BoundingBoxCameraSensor::Update( return false; } + if (this->HasInfoConnections()) + { + // publish the camera info message + this->PublishInfo(_now); + } + // don't render if there are no subscribers nor saving if (!this->dataPtr->imagePublisher.HasConnections() && !this->dataPtr->boxesPublisher.HasConnections() && @@ -502,7 +508,6 @@ bool BoundingBoxCameraSensor::Update( std::lock_guard lock(this->dataPtr->mutex); // Publish - this->PublishInfo(_now); if (this->dataPtr->type == rendering::BoundingBoxType::BBT_BOX3D) { this->AddSequence(boxes3DMsg.mutable_header(), "boundingboxes"); @@ -672,5 +677,6 @@ bool BoundingBoxCameraSensor::HasConnections() const return (this->dataPtr->imagePublisher && this->dataPtr->imagePublisher.HasConnections()) || (this->dataPtr->boxesPublisher && - this->dataPtr->boxesPublisher.HasConnections()); + this->dataPtr->boxesPublisher.HasConnections()) || + this->HasInfoConnections(); } diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 2b9e3f57..88a8dc5a 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -394,7 +394,7 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) // move the camera to the current pose this->dataPtr->camera->SetLocalPose(this->Pose()); - if (this->dataPtr->infoPub.HasConnections()) + if (this->HasInfoConnections()) { // publish the camera info message this->PublishInfo(_now); @@ -430,9 +430,7 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) } } - if ((this->dataPtr->pub && this->dataPtr->pub.HasConnections()) || - this->dataPtr->imageEvent.ConnectionCount() > 0u || - this->dataPtr->saveImage) + if (this->HasImageConnections() || this->dataPtr->saveImage) { // generate sensor data this->Render(); @@ -477,7 +475,7 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) msg.set_width(width); msg.set_height(height); msg.set_step(width * rendering::PixelUtil::BytesPerPixel( - this->dataPtr->camera->ImageFormat())); + this->dataPtr->camera->ImageFormat())); msg.set_pixel_format_type(msgsPixelFormat); *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); @@ -734,8 +732,19 @@ double CameraSensor::Baseline() const ////////////////////////////////////////////////// bool CameraSensor::HasConnections() const +{ + return this->HasImageConnections() || this->HasInfoConnections(); +} + +////////////////////////////////////////////////// +bool CameraSensor::HasImageConnections() const { return (this->dataPtr->pub && this->dataPtr->pub.HasConnections()) || - this->dataPtr->imageEvent.ConnectionCount() > 0u || - (this->dataPtr->infoPub && this->dataPtr->infoPub.HasConnections()); + this->dataPtr->imageEvent.ConnectionCount() > 0u; +} + +////////////////////////////////////////////////// +bool CameraSensor::HasInfoConnections() const +{ + return this->dataPtr->infoPub && this->dataPtr->infoPub.HasConnections(); } diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index e62e4579..4cd2c8cd 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -527,6 +527,17 @@ bool DepthCameraSensor::Update( return false; } + if (this->HasInfoConnections()) + { + // publish the camera info message + this->PublishInfo(_now); + } + + if (!this->HasDepthConnections() && !this->HasPointConnections()) + { + return false; + } + // generate sensor data this->Render(); @@ -555,8 +566,6 @@ bool DepthCameraSensor::Update( this->AddSequence(msg.mutable_header(), "default"); this->dataPtr->pub.Publish(msg); - // publish the camera info message - this->PublishInfo(_now); if (this->dataPtr->imageEvent.ConnectionCount() > 0u) { @@ -571,7 +580,7 @@ bool DepthCameraSensor::Update( } } - if (this->dataPtr->pointPub.HasConnections() && + if (this->HasPointConnections() && this->dataPtr->pointCloudBuffer) { // Set the time stamp @@ -637,7 +646,19 @@ double DepthCameraSensor::NearClip() const ////////////////////////////////////////////////// bool DepthCameraSensor::HasConnections() const { - return (this->dataPtr->pub && this->dataPtr->pub.HasConnections()) || - (this->dataPtr->pointPub && this->dataPtr->pointPub.HasConnections()) || - this->dataPtr->imageEvent.ConnectionCount() > 0u; + return this->HasDepthConnections() || this->HasPointConnections() || + this->HasInfoConnections(); +} + +////////////////////////////////////////////////// +bool DepthCameraSensor::HasDepthConnections() const +{ + return (this->dataPtr->pub && this->dataPtr->pub.HasConnections()) + || this->dataPtr->imageEvent.ConnectionCount() > 0u; +} + +////////////////////////////////////////////////// +bool DepthCameraSensor::HasPointConnections() const +{ + return this->dataPtr->pointPub && this->dataPtr->pointPub.HasConnections(); } diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index fb0b3718..02abc40d 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -466,6 +466,19 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) return false; } + if (this->HasInfoConnections()) + { + // publish the camera info message + this->PublishInfo(_now); + } + + // don't render if there are no subscribers + if (!this->HasColorConnections() && !this->HasDepthConnections() && + !this->HasPointConnections()) + { + return false; + } + unsigned int width = this->dataPtr->depthCamera->ImageWidth(); unsigned int height = this->dataPtr->depthCamera->ImageHeight(); unsigned int depthSamples = height * width; @@ -474,7 +487,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) this->Render(); // create and publish the depthmessage - if (this->dataPtr->depthPub.HasConnections()) + if (this->HasDepthConnections()) { msgs::Image msg; msg.set_width(width); @@ -532,7 +545,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) } // publish point cloud msg - if (this->dataPtr->pointPub.HasConnections()) + if (this->HasPointConnections()) { // Set the time stamp *this->dataPtr->pointMsg.mutable_header()->mutable_stamp() = @@ -574,7 +587,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) } // publish the 2d image message - if (this->dataPtr->imagePub.HasConnections()) + if (this->HasColorConnections()) { if (!filledImgData) { @@ -610,9 +623,6 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) } } - // publish the camera info message - this->PublishInfo(_now); - return true; } @@ -631,8 +641,24 @@ unsigned int RgbdCameraSensor::ImageHeight() const ////////////////////////////////////////////////// bool RgbdCameraSensor::HasConnections() const { - return (this->dataPtr->imagePub && - this->dataPtr->imagePub.HasConnections()) || - (this->dataPtr->depthPub && this->dataPtr->depthPub.HasConnections()) || - (this->dataPtr->pointPub && this->dataPtr->pointPub.HasConnections()); + return this->HasColorConnections() || this->HasDepthConnections() || + this->HasPointConnections() || this->HasInfoConnections(); +} + +////////////////////////////////////////////////// +bool RgbdCameraSensor::HasColorConnections() const +{ + return this->dataPtr->imagePub && this->dataPtr->imagePub.HasConnections(); +} + +////////////////////////////////////////////////// +bool RgbdCameraSensor::HasDepthConnections() const +{ + return this->dataPtr->depthPub && this->dataPtr->depthPub.HasConnections(); +} + +////////////////////////////////////////////////// +bool RgbdCameraSensor::HasPointConnections() const +{ + return this->dataPtr->pointPub && this->dataPtr->pointPub.HasConnections(); } diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index 99bbc42f..8132b09a 100644 --- a/src/SegmentationCameraSensor.cc +++ b/src/SegmentationCameraSensor.cc @@ -441,6 +441,12 @@ bool SegmentationCameraSensor::Update( return false; } + if (this->HasInfoConnections()) + { + // publish the camera info message + this->PublishInfo(_now); + } + // don't render if there are no subscribers nor saving if (!this->dataPtr->coloredMapPublisher.HasConnections() && !this->dataPtr->labelsMapPublisher.HasConnections() && @@ -506,7 +512,6 @@ bool SegmentationCameraSensor::Update( width, height)); // Publish - this->PublishInfo(_now); this->dataPtr->coloredMapPublisher.Publish(this->dataPtr->coloredMapMsg); this->dataPtr->labelsMapPublisher.Publish(this->dataPtr->labelsMapMsg); @@ -560,7 +565,8 @@ bool SegmentationCameraSensor::HasConnections() const this->dataPtr->coloredMapPublisher.HasConnections()) || (this->dataPtr->labelsMapPublisher && this->dataPtr->labelsMapPublisher.HasConnections()) || - this->dataPtr->imageEvent.ConnectionCount() > 0u; + this->dataPtr->imageEvent.ConnectionCount() > 0u || + this->HasInfoConnections(); } ////////////////////////////////////////////////// diff --git a/src/Sensor.cc b/src/Sensor.cc index e3f867f7..118c529b 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -305,7 +305,7 @@ void Sensor::PublishMetrics(const std::chrono::duration &_now) ////////////////////////////////////////////////// void SensorPrivate::PublishMetrics(const std::chrono::duration &_now) { - if(!this->performanceSensorMetricsPub) + if (!this->performanceSensorMetricsPub) { const auto validTopic = transport::TopicUtils::AsValidTopic( this->topic + "/performance_metrics"); @@ -329,7 +329,7 @@ void SensorPrivate::PublishMetrics(const std::chrono::duration &_now) double realUpdateRate; const auto clockNow = std::chrono::steady_clock::now(); // If lastUpdateTime == 0 means it wasn't initialized yet. - if(this->lastUpdateTime.count() > 0) + if (this->lastUpdateTime.count() > 0) { const double diffSimUpdate = _now.count() - this->lastUpdateTime.count(); diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index 5a62726f..6442d812 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -422,6 +422,13 @@ bool ThermalCameraSensor::Update( return false; } + if (this->HasInfoConnections()) + { + // publish the camera info message + this->PublishInfo(_now); + } + + // don't render if there are no subscribers if (!this->dataPtr->thermalPub.HasConnections() && this->dataPtr->imageEvent.ConnectionCount() == 0u) return false; @@ -484,8 +491,6 @@ bool ThermalCameraSensor::Update( width, height)); } - // publish the camera info message - this->PublishInfo(_now); this->dataPtr->thermalPub.Publish(this->dataPtr->thermalMsg); @@ -656,5 +661,6 @@ bool ThermalCameraSensor::HasConnections() const { return (this->dataPtr->thermalPub && this->dataPtr->thermalPub.HasConnections()) || - this->dataPtr->imageEvent.ConnectionCount() > 0u; + this->dataPtr->imageEvent.ConnectionCount() > 0u || + this->HasInfoConnections(); }