From d954dffac735ae6a3dfbd32a45ac0a6e4ee974ae Mon Sep 17 00:00:00 2001 From: Shameek Ganguly Date: Fri, 10 May 2024 12:41:59 -0700 Subject: [PATCH 1/5] Move trigger logic to base Sensor class and enable trigger for all camera sensors Signed-off-by: Shameek Ganguly --- include/gz/sensors/BoundingBoxCameraSensor.hh | 4 - include/gz/sensors/CameraSensor.hh | 4 - include/gz/sensors/Sensor.hh | 25 +++ src/BoundingBoxCameraSensor.cc | 61 +------ src/CameraSensor.cc | 57 +------ src/DepthCameraSensor.cc | 11 ++ src/RgbdCameraSensor.cc | 11 ++ src/SegmentationCameraSensor.cc | 11 ++ src/Sensor.cc | 155 ++++++++++++++++-- src/Sensor_TEST.cc | 61 +++++++ src/ThermalCameraSensor.cc | 11 ++ src/WideAngleCameraSensor.cc | 11 ++ 12 files changed, 302 insertions(+), 120 deletions(-) diff --git a/include/gz/sensors/BoundingBoxCameraSensor.hh b/include/gz/sensors/BoundingBoxCameraSensor.hh index 4fe47496..80fb6686 100644 --- a/include/gz/sensors/BoundingBoxCameraSensor.hh +++ b/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 diff --git a/include/gz/sensors/CameraSensor.hh b/include/gz/sensors/CameraSensor.hh index d6f7ba45..108af8ce 100644 --- a/include/gz/sensors/CameraSensor.hh +++ b/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 diff --git a/include/gz/sensors/Sensor.hh b/include/gz/sensors/Sensor.hh index f6a9e813..fe83b08c 100644 --- a/include/gz/sensors/Sensor.hh +++ b/include/gz/sensors/Sensor.hh @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -231,6 +232,30 @@ namespace gz /// \return True if there are subscribers, false otherwise public: virtual bool HasConnections() const; + /// \brief Set the sensor in triggered mode. In this mode, + /// - the sensor will only update if a new message has been published to + /// the passed _trigger_topic since the last update, + /// - until the next message is published on _trigger_topic, all Update + /// calls will return false, and + /// - update_rate is forced to zero. + /// \param[in] _trigger_topic The topic on which the sensor will listen + /// for trigger messages. + /// \return True if the sensor was successfully set to triggered mode. + public: bool EnableTriggered(const std::string &_trigger_topic); + + /// \brief Disable triggered mode. The sensor will update as per the set + /// update rate. + /// \param[in] _update_rate_hz Optional update rate of sensor in Hertz. If + /// std::nullopt is passed, the rate parsed from the SDF will be used if + /// it is available, otherwise the update rate will be set to zero. + public: void DisableTriggered( + const std::optional &_update_rate_hz); + + /// \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 diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index 795dc88b..e44a9356 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -16,6 +16,8 @@ */ #include +#include +#include #include #include @@ -32,6 +34,7 @@ #include #include #include +#include #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->EnableTriggered(triggerTopic); } if (!this->AdvertiseInfo()) @@ -430,16 +407,6 @@ bool BoundingBoxCameraSensor::Update( this->PublishInfo(_now); } - // render only if necessary - { - std::lock_guard 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 lock(this->dataPtr->mutex); - this->dataPtr->isTriggered = true; -} - ////////////////////////////////////////////////// void BoundingBoxCameraSensorPrivate::SaveImage() { diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 11811862..bee6cd1e 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -20,6 +20,8 @@ #include #include +#include +#include #include #include @@ -30,6 +32,7 @@ #include #include #include +#include #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->EnableTriggered(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 lock(this->dataPtr->mutex); - this->dataPtr->isTriggered = true; -} - ////////////////////////////////////////////////// bool CameraSensorPrivate::SaveImage(const unsigned char *_data, unsigned int _width, unsigned int _height, diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 239d3402..5e9451e2 100644 --- a/src/DepthCameraSensor.cc +++ b/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->EnableTriggered(triggerTopic); + } + if (!this->AdvertiseInfo()) return false; diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 9e583398..e4369707 100644 --- a/src/RgbdCameraSensor.cc +++ b/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->EnableTriggered(triggerTopic); + } + if (!this->AdvertiseInfo(this->Topic() + "/camera_info")) return false; diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index 70fa38f8..8e9353ad 100644 --- a/src/SegmentationCameraSensor.cc +++ b/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->EnableTriggered(triggerTopic); + } + // TODO(anyone) Access the info topic from the parent class if (!this->AdvertiseInfo(this->Topic() + "/camera_info")) return false; diff --git a/src/Sensor.cc b/src/Sensor.cc index 01385ac7..2a6778e3 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -20,6 +20,7 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif +#include #include #include #ifdef _WIN32 @@ -28,9 +29,15 @@ #include "gz/sensors/Sensor.hh" +#include #include +#include #include +#include +#include +#include #include +#include #include #include @@ -61,6 +68,24 @@ class gz::sensors::SensorPrivate /// \return True if a valid topic was set. public: void SetRate(const gz::msgs::Double &_rate); + /// \brief Set the sensor in triggered mode. + /// \param[in] _trigger_topic The topic on which the sensor will listen + /// for trigger messages. + /// \return True if the sensor was successfully set to triggered mode. + public: bool EnableTriggered(const std::string &_topic); + + /// \brief Whether the sensor is triggered by a topic. + /// \return True if sensor is triggered by a topic. + public: bool IsTriggered() const; + + /// \brief Callback for triggered subscription. + /// \param[in] _msg Boolean message. + public: void OnTrigger(const gz::msgs::Boolean &_msg); + + /// \brief Disable triggered mode. + /// \param[in] _update_rate_hz Optional update rate of sensor in Hertz. + public: void DisableTriggered(const std::optional &_update_rate); + /// \brief id given to sensor when constructed public: SensorId id; @@ -90,6 +115,9 @@ class gz::sensors::SensorPrivate /// used value). public: double updateRate = 0.0; + /// \brief Mutex to synchronize concurrent writes to updateRate. + public: std::mutex updateRateMutex; + /// \brief What sim time should this sensor update at public: std::chrono::steady_clock::duration nextUpdateTime {std::chrono::steady_clock::duration::zero()}; @@ -122,6 +150,16 @@ class gz::sensors::SensorPrivate /// \brief If sensor is active or not. public: bool active = true; + + /// \brief Topic on which to listen for trigger messages. Empty indicates that + /// the sensor is not in triggered mode. + public: std::string triggerTopic = ""; + + /// \brief Whether the sensor has a pending trigger. + public: bool pendingTrigger = false; + + /// \brief Mutex to synchronize concurrent writes to pendingTrigger. + public: std::mutex triggerMutex; }; SensorId SensorPrivate::idCounter = 0; @@ -369,6 +407,12 @@ void SensorPrivate::PublishMetrics(const std::chrono::duration &_now) ////////////////////////////////////////////////// void SensorPrivate::SetRate(const gz::msgs::Double &_rate) { + if (this->IsTriggered()) { + gzwarn << "Sensor [" << this->name << "] is set to be triggered, update" + << " rate will be ignored." << std::endl; + return; + } + auto rate = _rate.data(); if (rate < 0.0) rate = 0.0; @@ -399,6 +443,7 @@ void SensorPrivate::SetRate(const gz::msgs::Double &_rate) gzdbg << "Setting update rate of sensor " << this->name << " to " << rate << " Hz" << std::endl; + std::lock_guard lock(this->updateRateMutex); this->updateRate = rate; } @@ -435,14 +480,13 @@ double Sensor::UpdateRate() const ////////////////////////////////////////////////// void Sensor::SetUpdateRate(const double _hz) { - if (_hz < 0) - { - this->dataPtr->updateRate = 0; - } - else - { - this->dataPtr->updateRate = _hz; + if (this->dataPtr->IsTriggered()) { + gzwarn << "Sensor [" << this->Name() << "] is set to be triggered, update" + << " rate will be ignored." << std::endl; + return; } + std::lock_guard lock(this->dataPtr->updateRateMutex); + this->dataPtr->updateRate = std::fmax(_hz, 0); } ////////////////////////////////////////////////// @@ -451,16 +495,29 @@ bool Sensor::Update(const std::chrono::steady_clock::duration &_now, { GZ_PROFILE("Sensor::Update"); bool result = false; + bool force = _force; + if (this->dataPtr->IsTriggered()) + { + std::lock_guard lock(this->dataPtr->triggerMutex); + force |= this->dataPtr->pendingTrigger; + this->dataPtr->pendingTrigger = false; + + // If sensor is in triggered mode, but a trigger is not pending and this is + // not a forced update call, then return early. + if (!force) + return result; + } // Check if it's time to update - if (_now < this->dataPtr->nextUpdateTime && !_force && + std::lock_guard lock(this->dataPtr->updateRateMutex); + if (_now < this->dataPtr->nextUpdateTime && !force && this->dataPtr->updateRate > 0) { return result; } // prevent update if not active, unless forced - if (!this->dataPtr->active && !_force) + if (!this->dataPtr->active && !force) return result; // Make the update happen @@ -473,7 +530,7 @@ bool Sensor::Update(const std::chrono::steady_clock::duration &_now, this->PublishMetrics(secs); } - if (!_force && this->dataPtr->updateRate > 0.0) + if (!force && this->dataPtr->updateRate > 0.0) { // Update the time the plugin should be loaded auto delta = std::chrono::duration_cast< std::chrono::milliseconds> @@ -551,3 +608,81 @@ bool Sensor::HasConnections() const { return true; } + +////////////////////////////////////////////////// +bool Sensor::EnableTriggered(const std::string &_trigger_topic) { + return this->dataPtr->EnableTriggered(_trigger_topic); +} + +////////////////////////////////////////////////// +bool SensorPrivate::EnableTriggered(const std::string &_topic) { + if (_topic.empty()) + { + gzwarn << "Trigger topic is empty for sensor [" << this->name << "]" + << std::endl; + return false; + } + if (!this->node.Subscribe(_topic, &SensorPrivate::OnTrigger, this)) + { + gzwarn << "Failed to subscribe to trigger topic [" << _topic + << "] for sensor [" << this->name << "]" << std::endl; + return false; + } + this->triggerTopic = _topic; + + // Set update rate to 0 so that sensor will update immediately on pending + // trigger. + std::lock_guard lock(this->updateRateMutex); + this->updateRate = 0.0; + + gzmsg << "Sensor trigger messages for [" << name << "] subscribed" + << " on [" << this->triggerTopic << "]" << std::endl; + return true; +} + +////////////////////////////////////////////////// +bool SensorPrivate::IsTriggered() const { + return !this->triggerTopic.empty(); +} + +////////////////////////////////////////////////// +void SensorPrivate::OnTrigger(const gz::msgs::Boolean &/*_msg*/) { + std::lock_guard lock(this->triggerMutex); + this->pendingTrigger = true; +} + +////////////////////////////////////////////////// +void Sensor::DisableTriggered(const std::optional &_update_rate_hz) { + this->dataPtr->DisableTriggered(_update_rate_hz); +} + +////////////////////////////////////////////////// +void SensorPrivate::DisableTriggered( + const std::optional &_update_rate_hz) { + std::lock_guard updateRateLock(this->updateRateMutex); + if (_update_rate_hz.has_value()) { + this->updateRate = std::fmax(0, _update_rate_hz.value()); + } else { + gzmsg << "Setting update rate from sdf since no value was passed." + << std::endl; + this->updateRate = this->sdfUpdateRate; + } + gzmsg << "Disabled triggered mode for sensor [" << this->name + << "]. Sensor will update at " << this->updateRate << "Hz." + << std::endl; + + this->node.Unsubscribe(this->triggerTopic); + this->triggerTopic.clear(); + + std::lock_guard triggerLock(this->triggerMutex); + this->pendingTrigger = false; +} + +////////////////////////////////////////////////// +bool Sensor::HasPendingTrigger() const { + if (!this->dataPtr->IsTriggered()) + return false; + + std::lock_guard triggerLock(this->dataPtr->triggerMutex); + return this->dataPtr->pendingTrigger; +} diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index cffeecbb..64e2f225 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -19,13 +19,16 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif +#include #include #if defined(_MSC_VER) #pragma warning(pop) #endif #include +#include #include +#include #include @@ -462,3 +465,61 @@ TEST_F(SensorUpdate, NextDataUpdateTime) EXPECT_EQ(newNext.count(), sensor->NextDataUpdateTime().count()); } } + +////////////////////////////////////////////////// +TEST(Sensor_TEST, Trigger) +{ + TestSensor sensor; + constexpr char kSensorTopic[] = "/topic"; + EXPECT_TRUE(sensor.SetTopic(kSensorTopic)); + + constexpr double kUpdateRate = 5; + sensor.SetUpdateRate(5); + EXPECT_DOUBLE_EQ(kUpdateRate, sensor.UpdateRate()); + + constexpr char kTriggerTopic[] = "/trigger"; + EXPECT_TRUE(sensor.EnableTriggered(kTriggerTopic)); + EXPECT_DOUBLE_EQ(0, sensor.UpdateRate()); + + transport::Node node; + transport::Node::Publisher triggerPub = + node.Advertise(kTriggerTopic); + + { + // Before triggering, sensor should not update + std::chrono::steady_clock::duration now = std::chrono::seconds(2); + std::chrono::steady_clock::duration next = std::chrono::seconds(1); + sensor.SetNextDataUpdateTime(next); + unsigned int preUpdateCount = sensor.updateCount; + EXPECT_FALSE(sensor.Sensor::Update(now, /*_force=*/false)); + EXPECT_EQ(preUpdateCount, sensor.updateCount); + } + { + // After triggering, sensor should update + msgs::Boolean triggerMsg; + EXPECT_TRUE(triggerPub.Publish(triggerMsg)); + while (!sensor.HasPendingTrigger()) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + std::chrono::steady_clock::duration now = std::chrono::seconds(3); + std::chrono::steady_clock::duration next = std::chrono::seconds(1); + sensor.SetNextDataUpdateTime(next); + unsigned int preUpdateCount = sensor.updateCount; + EXPECT_TRUE(sensor.Sensor::Update(now, /*_force=*/false)); + EXPECT_EQ(preUpdateCount + 1, sensor.updateCount); + } + + sensor.DisableTriggered(kUpdateRate); + EXPECT_DOUBLE_EQ(kUpdateRate, sensor.UpdateRate()); + EXPECT_FALSE(sensor.HasPendingTrigger()); + { + // Sensor should update according to update rate. + std::chrono::steady_clock::duration now = std::chrono::seconds(4); + std::chrono::steady_clock::duration next = std::chrono::seconds(1); + sensor.SetNextDataUpdateTime(next); + unsigned int preUpdateCount = sensor.updateCount; + EXPECT_TRUE(sensor.Sensor::Update(now, /*_force=*/false)); + EXPECT_EQ(preUpdateCount + 1, sensor.updateCount); + EXPECT_GE(sensor.NextDataUpdateTime(), now); + } +} diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index b87112df..8cd01804 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -216,6 +216,17 @@ bool ThermalCameraSensor::Load(const sdf::Sensor &_sdf) gzdbg << "Thermal 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->EnableTriggered(triggerTopic); + } + if (!this->AdvertiseInfo()) return false; diff --git a/src/WideAngleCameraSensor.cc b/src/WideAngleCameraSensor.cc index c1ddb2fb..432068e0 100644 --- a/src/WideAngleCameraSensor.cc +++ b/src/WideAngleCameraSensor.cc @@ -185,6 +185,17 @@ bool WideAngleCameraSensor::Load(const sdf::Sensor &_sdf) gzdbg << "Wide angle camera 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->EnableTriggered(triggerTopic); + } + if (!this->AdvertiseInfo()) return false; From 0e1856e7be81ec9467fad56e862890c753ff359b Mon Sep 17 00:00:00 2001 From: Shameek Ganguly Date: Fri, 10 May 2024 13:55:34 -0700 Subject: [PATCH 2/5] Update tests to not force image rendering Signed-off-by: Shameek Ganguly --- test/integration/triggered_boundingbox_camera.cc | 9 ++++++--- test/integration/triggered_camera.cc | 12 ++++++++---- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/test/integration/triggered_boundingbox_camera.cc b/test/integration/triggered_boundingbox_camera.cc index 9c06496b..42178a69 100644 --- a/test/integration/triggered_boundingbox_camera.cc +++ b/test/integration/triggered_boundingbox_camera.cc @@ -182,7 +182,8 @@ void TriggeredBoundingBoxCameraTest::BoxesWithBuiltinSDF( std::string imageTopic = "/test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF_image"; WaitForMessageTestHelper helper(imageTopic); - mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + std::chrono::steady_clock::duration now = std::chrono::seconds(1); + mgr.RunOnce(now, /*_force=*/false); EXPECT_FALSE(helper.WaitForMessage(1s)) << helper; g_mutex.lock(); EXPECT_EQ(g_boxes.size(), size_t(0)); @@ -204,7 +205,8 @@ void TriggeredBoundingBoxCameraTest::BoxesWithBuiltinSDF( { WaitForMessageTestHelper helper(boxTopic); - mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + std::chrono::steady_clock::duration now = std::chrono::seconds(2); + mgr.RunOnce(now, /*_force=*/false); EXPECT_TRUE(helper.WaitForMessage(10s)) << helper; g_mutex.lock(); EXPECT_EQ(g_boxes.size(), size_t(2)); @@ -286,7 +288,8 @@ void TriggeredBoundingBoxCameraTest::EmptyTriggerTopic( "/test/integration/triggered_bbcamera"; WaitForMessageTestHelper helper(boxTopic); - mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + std::chrono::steady_clock::duration now = std::chrono::seconds(1); + mgr.RunOnce(now, /*_force=*/false); EXPECT_TRUE(helper.WaitForMessage(10s)) << helper; g_mutex.lock(); EXPECT_EQ(g_boxes.size(), size_t(2)); diff --git a/test/integration/triggered_camera.cc b/test/integration/triggered_camera.cc index 873fe486..b72bc180 100644 --- a/test/integration/triggered_camera.cc +++ b/test/integration/triggered_camera.cc @@ -111,13 +111,15 @@ void TriggeredCameraTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) EXPECT_EQ(256u, sensor->ImageWidth()); EXPECT_EQ(257u, sensor->ImageHeight()); - // check camera image before trigger + // Check camera image before trigger. Image should not be published since + // the camera has not been triggered yet. { std::string imageTopic = "/test/integration/TriggeredCameraPlugin_imagesWithBuiltinSDF"; WaitForMessageTestHelper helper(imageTopic); EXPECT_TRUE(sensor->HasConnections()); - mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + std::chrono::steady_clock::duration now = std::chrono::seconds(1); + mgr.RunOnce(now, /*_force=*/false); EXPECT_FALSE(helper.WaitForMessage(1s)) << helper; } @@ -139,7 +141,8 @@ void TriggeredCameraTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) std::string imageTopic = "/test/integration/TriggeredCameraPlugin_imagesWithBuiltinSDF"; WaitForMessageTestHelper helper(imageTopic); - mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + std::chrono::steady_clock::duration now = std::chrono::seconds(2); + mgr.RunOnce(now, /*_force=*/false); EXPECT_TRUE(helper.WaitForMessage(10s)) << helper; } @@ -216,7 +219,8 @@ void TriggeredCameraTest::EmptyTriggerTopic(const std::string &_renderEngine) std::string imageTopic = "/test/integration/triggered_camera"; WaitForMessageTestHelper helper(imageTopic); - mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + std::chrono::steady_clock::duration now = std::chrono::seconds(1); + mgr.RunOnce(now, /*_force=*/false); EXPECT_TRUE(helper.WaitForMessage(10s)) << helper; } From b8a64103349a823eee324052e11c1faeb5c59839 Mon Sep 17 00:00:00 2001 From: Shameek Ganguly Date: Tue, 14 May 2024 15:31:46 -0700 Subject: [PATCH 3/5] Updated as per review comments Signed-off-by: Shameek Ganguly --- include/gz/sensors/Sensor.hh | 34 ++++++++--------- src/BoundingBoxCameraSensor.cc | 2 +- src/CameraSensor.cc | 2 +- src/DepthCameraSensor.cc | 2 +- src/RgbdCameraSensor.cc | 2 +- src/SegmentationCameraSensor.cc | 2 +- src/Sensor.cc | 68 +++++++++++---------------------- src/Sensor_TEST.cc | 6 +-- src/ThermalCameraSensor.cc | 2 +- src/WideAngleCameraSensor.cc | 2 +- 10 files changed, 49 insertions(+), 73 deletions(-) diff --git a/include/gz/sensors/Sensor.hh b/include/gz/sensors/Sensor.hh index fe83b08c..a3f0d394 100644 --- a/include/gz/sensors/Sensor.hh +++ b/include/gz/sensors/Sensor.hh @@ -114,8 +114,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. @@ -232,24 +232,22 @@ namespace gz /// \return True if there are subscribers, false otherwise public: virtual bool HasConnections() const; - /// \brief Set the sensor in triggered mode. In this mode, + /// \brief Enable or disable triggered mode. In this mode, /// - the sensor will only update if a new message has been published to - /// the passed _trigger_topic since the last update, - /// - until the next message is published on _trigger_topic, all Update + /// the passed _triggerTopic since the last update, + /// - until the next message is published on _triggerTopic, all Update /// calls will return false, and - /// - update_rate is forced to zero. - /// \param[in] _trigger_topic The topic on which the sensor will listen - /// for trigger messages. - /// \return True if the sensor was successfully set to triggered mode. - public: bool EnableTriggered(const std::string &_trigger_topic); - - /// \brief Disable triggered mode. The sensor will update as per the set - /// update rate. - /// \param[in] _update_rate_hz Optional update rate of sensor in Hertz. If - /// std::nullopt is passed, the rate parsed from the SDF will be used if - /// it is available, otherwise the update rate will be set to zero. - public: void DisableTriggered( - const std::optional &_update_rate_hz); + /// - 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 diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index e44a9356..08467b22 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -231,7 +231,7 @@ bool BoundingBoxCameraSensor::Load(const sdf::Sensor &_sdf) triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() + "/trigger"); } - this->EnableTriggered(triggerTopic); + this->SetTriggered(true, triggerTopic); } if (!this->AdvertiseInfo()) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index bee6cd1e..0a3237f1 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -460,7 +460,7 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf) triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() + "/trigger"); } - this->EnableTriggered(triggerTopic); + this->SetTriggered(true, triggerTopic); } if (!this->AdvertiseInfo()) diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 5e9451e2..cbd7f896 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -290,7 +290,7 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf) triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() + "/trigger"); } - this->EnableTriggered(triggerTopic); + this->SetTriggered(true, triggerTopic); } if (!this->AdvertiseInfo()) diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index e4369707..d780feb2 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -245,7 +245,7 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() + "/trigger"); } - this->EnableTriggered(triggerTopic); + this->SetTriggered(true, triggerTopic); } if (!this->AdvertiseInfo(this->Topic() + "/camera_info")) diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index 8e9353ad..fd820509 100644 --- a/src/SegmentationCameraSensor.cc +++ b/src/SegmentationCameraSensor.cc @@ -231,7 +231,7 @@ bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf) triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() + "/trigger"); } - this->EnableTriggered(triggerTopic); + this->SetTriggered(true, triggerTopic); } // TODO(anyone) Access the info topic from the parent class diff --git a/src/Sensor.cc b/src/Sensor.cc index 2a6778e3..d7cc0d1f 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -69,10 +69,10 @@ class gz::sensors::SensorPrivate public: void SetRate(const gz::msgs::Double &_rate); /// \brief Set the sensor in triggered mode. - /// \param[in] _trigger_topic The topic on which the sensor will listen + /// \param[in] _topic The topic on which the sensor will listen /// for trigger messages. /// \return True if the sensor was successfully set to triggered mode. - public: bool EnableTriggered(const std::string &_topic); + public: bool SetTriggerTopic(const std::string &_topic); /// \brief Whether the sensor is triggered by a topic. /// \return True if sensor is triggered by a topic. @@ -83,8 +83,7 @@ class gz::sensors::SensorPrivate public: void OnTrigger(const gz::msgs::Boolean &_msg); /// \brief Disable triggered mode. - /// \param[in] _update_rate_hz Optional update rate of sensor in Hertz. - public: void DisableTriggered(const std::optional &_update_rate); + public: void DisableTriggered(); /// \brief id given to sensor when constructed public: SensorId id; @@ -115,9 +114,6 @@ class gz::sensors::SensorPrivate /// used value). public: double updateRate = 0.0; - /// \brief Mutex to synchronize concurrent writes to updateRate. - public: std::mutex updateRateMutex; - /// \brief What sim time should this sensor update at public: std::chrono::steady_clock::duration nextUpdateTime {std::chrono::steady_clock::duration::zero()}; @@ -407,12 +403,6 @@ void SensorPrivate::PublishMetrics(const std::chrono::duration &_now) ////////////////////////////////////////////////// void SensorPrivate::SetRate(const gz::msgs::Double &_rate) { - if (this->IsTriggered()) { - gzwarn << "Sensor [" << this->name << "] is set to be triggered, update" - << " rate will be ignored." << std::endl; - return; - } - auto rate = _rate.data(); if (rate < 0.0) rate = 0.0; @@ -443,7 +433,6 @@ void SensorPrivate::SetRate(const gz::msgs::Double &_rate) gzdbg << "Setting update rate of sensor " << this->name << " to " << rate << " Hz" << std::endl; - std::lock_guard lock(this->updateRateMutex); this->updateRate = rate; } @@ -480,13 +469,14 @@ double Sensor::UpdateRate() const ////////////////////////////////////////////////// void Sensor::SetUpdateRate(const double _hz) { - if (this->dataPtr->IsTriggered()) { - gzwarn << "Sensor [" << this->Name() << "] is set to be triggered, update" - << " rate will be ignored." << std::endl; - return; + if (_hz < 0) + { + this->dataPtr->updateRate = 0; + } + else + { + this->dataPtr->updateRate = _hz; } - std::lock_guard lock(this->dataPtr->updateRateMutex); - this->dataPtr->updateRate = std::fmax(_hz, 0); } ////////////////////////////////////////////////// @@ -509,7 +499,6 @@ bool Sensor::Update(const std::chrono::steady_clock::duration &_now, } // Check if it's time to update - std::lock_guard lock(this->dataPtr->updateRateMutex); if (_now < this->dataPtr->nextUpdateTime && !force && this->dataPtr->updateRate > 0) { @@ -610,12 +599,20 @@ bool Sensor::HasConnections() const } ////////////////////////////////////////////////// -bool Sensor::EnableTriggered(const std::string &_trigger_topic) { - return this->dataPtr->EnableTriggered(_trigger_topic); +bool Sensor::SetTriggered(bool _triggered, const std::string &_triggerTopic) { + if (_triggered) + { + return this->dataPtr->SetTriggerTopic(_triggerTopic); + } + else + { + this->dataPtr->DisableTriggered(); + return true; + } } ////////////////////////////////////////////////// -bool SensorPrivate::EnableTriggered(const std::string &_topic) { +bool SensorPrivate::SetTriggerTopic(const std::string &_topic) { if (_topic.empty()) { gzwarn << "Trigger topic is empty for sensor [" << this->name << "]" @@ -630,13 +627,8 @@ bool SensorPrivate::EnableTriggered(const std::string &_topic) { } this->triggerTopic = _topic; - // Set update rate to 0 so that sensor will update immediately on pending - // trigger. - std::lock_guard lock(this->updateRateMutex); - this->updateRate = 0.0; - gzmsg << "Sensor trigger messages for [" << name << "] subscribed" - << " on [" << this->triggerTopic << "]" << std::endl; + << " on [" << this->triggerTopic << "]" << std::endl; return true; } @@ -652,21 +644,7 @@ void SensorPrivate::OnTrigger(const gz::msgs::Boolean &/*_msg*/) { } ////////////////////////////////////////////////// -void Sensor::DisableTriggered(const std::optional &_update_rate_hz) { - this->dataPtr->DisableTriggered(_update_rate_hz); -} - -////////////////////////////////////////////////// -void SensorPrivate::DisableTriggered( - const std::optional &_update_rate_hz) { - std::lock_guard updateRateLock(this->updateRateMutex); - if (_update_rate_hz.has_value()) { - this->updateRate = std::fmax(0, _update_rate_hz.value()); - } else { - gzmsg << "Setting update rate from sdf since no value was passed." - << std::endl; - this->updateRate = this->sdfUpdateRate; - } +void SensorPrivate::DisableTriggered() { gzmsg << "Disabled triggered mode for sensor [" << this->name << "]. Sensor will update at " << this->updateRate << "Hz." << std::endl; diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 64e2f225..f0121017 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -478,8 +478,8 @@ TEST(Sensor_TEST, Trigger) EXPECT_DOUBLE_EQ(kUpdateRate, sensor.UpdateRate()); constexpr char kTriggerTopic[] = "/trigger"; - EXPECT_TRUE(sensor.EnableTriggered(kTriggerTopic)); - EXPECT_DOUBLE_EQ(0, sensor.UpdateRate()); + EXPECT_TRUE(sensor.SetTriggered(true, kTriggerTopic)); + EXPECT_FALSE(sensor.HasPendingTrigger()); transport::Node node; transport::Node::Publisher triggerPub = @@ -509,7 +509,7 @@ TEST(Sensor_TEST, Trigger) EXPECT_EQ(preUpdateCount + 1, sensor.updateCount); } - sensor.DisableTriggered(kUpdateRate); + sensor.SetTriggered(false); EXPECT_DOUBLE_EQ(kUpdateRate, sensor.UpdateRate()); EXPECT_FALSE(sensor.HasPendingTrigger()); { diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index 8cd01804..90cab5df 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -224,7 +224,7 @@ bool ThermalCameraSensor::Load(const sdf::Sensor &_sdf) triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() + "/trigger"); } - this->EnableTriggered(triggerTopic); + this->SetTriggered(true, triggerTopic); } if (!this->AdvertiseInfo()) diff --git a/src/WideAngleCameraSensor.cc b/src/WideAngleCameraSensor.cc index 432068e0..ab879200 100644 --- a/src/WideAngleCameraSensor.cc +++ b/src/WideAngleCameraSensor.cc @@ -193,7 +193,7 @@ bool WideAngleCameraSensor::Load(const sdf::Sensor &_sdf) triggerTopic = transport::TopicUtils::AsValidTopic(this->Topic() + "/trigger"); } - this->EnableTriggered(triggerTopic); + this->SetTriggered(true, triggerTopic); } if (!this->AdvertiseInfo()) From ffe24661e40d894a6822e80ed173e609b6239b84 Mon Sep 17 00:00:00 2001 From: Shameek Ganguly Date: Tue, 14 May 2024 15:40:30 -0700 Subject: [PATCH 4/5] Remove unnecessary stdlib headers Signed-off-by: Shameek Ganguly --- src/Sensor.cc | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/Sensor.cc b/src/Sensor.cc index d7cc0d1f..3fa6d8e8 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -29,14 +29,10 @@ #include "gz/sensors/Sensor.hh" -#include #include -#include #include #include -#include #include -#include #include #include From ad59aca3bc28c5f12ab65df72f5dcf535ea07625 Mon Sep 17 00:00:00 2001 From: Shameek Ganguly Date: Tue, 14 May 2024 15:43:44 -0700 Subject: [PATCH 5/5] Remove one more unnecessary stdlib header Signed-off-by: Shameek Ganguly --- include/gz/sensors/Sensor.hh | 1 - 1 file changed, 1 deletion(-) diff --git a/include/gz/sensors/Sensor.hh b/include/gz/sensors/Sensor.hh index a3f0d394..75a125a9 100644 --- a/include/gz/sensors/Sensor.hh +++ b/include/gz/sensors/Sensor.hh @@ -29,7 +29,6 @@ #include #include -#include #include #include