From 898570dc7fc874de1dc2a3895317b46ef950646f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 15 Feb 2023 23:37:03 +0100 Subject: [PATCH] Added Camera Info topic support for cameras (#285) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added Camera Info topic support for cameras --------- Signed-off-by: ahcorde Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Ian Chen --- src/CameraSensor.cc | 23 ++++++++++++++--------- src/Camera_TEST.cc | 19 ++++++++++++------- 2 files changed, 26 insertions(+), 16 deletions(-) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index e340c74d..e2cfa049 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -257,6 +257,11 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf) if (this->Topic().empty()) this->SetTopic("/camera"); + if (!_sdf.CameraSensor()->CameraInfoTopic().empty()) + { + this->dataPtr->infoTopic = _sdf.CameraSensor()->CameraInfoTopic(); + } + this->dataPtr->pub = this->dataPtr->node.Advertise( this->Topic()); @@ -491,17 +496,17 @@ std::string CameraSensor::InfoTopic() const ////////////////////////////////////////////////// bool CameraSensor::AdvertiseInfo() { - // TODO(anyone) Make info topic configurable from SDF - // Info topic must be at same level as image topic - auto parts = common::Split(this->Topic(), '/'); - parts.pop_back(); - - for (const auto &part : parts) + if (this->dataPtr->infoTopic.empty()) { - if (!part.empty()) - this->dataPtr->infoTopic += "/" + part; + auto parts = common::Split(this->Topic(), '/'); + parts.pop_back(); + for (const auto &part : parts) + { + if (!part.empty()) + this->dataPtr->infoTopic += "/" + part; + } + this->dataPtr->infoTopic += "/camera_info"; } - this->dataPtr->infoTopic += "/camera_info"; return this->AdvertiseInfo(this->dataPtr->infoTopic); } diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 3c077019..912b22a3 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -48,7 +48,8 @@ sdf::ElementPtr cameraToBadSdf() sdf::ElementPtr CameraToSdf(const std::string &_type, const std::string &_name, double _updateRate, - const std::string &_topic, bool _alwaysOn, bool _visualize) + const std::string &_topic, const std::string &_cameraInfoTopic, + bool _alwaysOn, bool _visualize) { std::ostringstream stream; stream @@ -58,6 +59,7 @@ sdf::ElementPtr CameraToSdf(const std::string &_type, << " " << " " << " " << _topic << "" + << " " << _cameraInfoTopic << "" << " "<< _updateRate <<"" << " "<< _alwaysOn <<"" << " " << _visualize << "" @@ -146,8 +148,8 @@ TEST(Camera_TEST, CreateCamera) { gz::sensors::Manager mgr; - sdf::ElementPtr camSdf = CameraToSdf("camera", "my_camera", 60.0, "/cam", - true, true); + sdf::ElementPtr camSdf = CameraToSdf("camera", "my_camera", 60.0, + "/cam", "my_camera/camera_info", true, true); // Create a CameraSensor gz::sensors::CameraSensor *cam = @@ -190,8 +192,9 @@ TEST(Camera_TEST, Topic) // Default topic { const std::string topic; - auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, - visualize); + const std::string cameraInfoTopic; + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, cameraInfoTopic, + alwaysOn, visualize); auto sensorId = mgr.CreateSensor(cameraSdf); EXPECT_NE(gz::sensors::NO_SENSOR, sensorId); @@ -203,12 +206,13 @@ TEST(Camera_TEST, Topic) ASSERT_NE(nullptr, camera); EXPECT_EQ("/camera", camera->Topic()); + EXPECT_EQ("/camera_info", camera->InfoTopic()); } // Convert to valid topic { const std::string topic = "/topic with spaces/@~characters//"; - auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, "", alwaysOn, visualize); auto sensorId = mgr.CreateSensor(cameraSdf); @@ -221,12 +225,13 @@ TEST(Camera_TEST, Topic) ASSERT_NE(nullptr, camera); EXPECT_EQ("/topic_with_spaces/characters", camera->Topic()); + EXPECT_EQ("/topic_with_spaces/camera_info", camera->InfoTopic()); } // Invalid topic { const std::string topic = "@@@"; - auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, "", alwaysOn, visualize); auto sensorId = mgr.CreateSensor(cameraSdf);