From 898570dc7fc874de1dc2a3895317b46ef950646f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= <ahcorde@gmail.com> Date: Wed, 15 Feb 2023 23:37:03 +0100 Subject: [PATCH 1/2] 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 <ahcorde@gmail.com> Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Co-authored-by: Ian Chen <ichen@openrobotics.org> --- 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<msgs::Image>( 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, << " <link name='link1'>" << " <sensor name='" << _name << "' type='" << _type << "'>" << " <topic>" << _topic << "</topic>" + << " <topic>" << _cameraInfoTopic << "</topic>" << " <update_rate>"<< _updateRate <<"</update_rate>" << " <always_on>"<< _alwaysOn <<"</always_on>" << " <visualize>" << _visualize << "</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); From 6c242474b0ffc7892d1d9a591012e65cd0d40431 Mon Sep 17 00:00:00 2001 From: Ian Chen <ichen@openrobotics.org> Date: Mon, 27 Feb 2023 13:35:58 -0800 Subject: [PATCH 2/2] Fix Camera info test (#326) * fix camera info test --------- Signed-off-by: Ian Chen <ichen@openrobotics.org> --- src/Camera_TEST.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 912b22a3..d90402c5 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -59,11 +59,12 @@ sdf::ElementPtr CameraToSdf(const std::string &_type, << " <link name='link1'>" << " <sensor name='" << _name << "' type='" << _type << "'>" << " <topic>" << _topic << "</topic>" - << " <topic>" << _cameraInfoTopic << "</topic>" << " <update_rate>"<< _updateRate <<"</update_rate>" << " <always_on>"<< _alwaysOn <<"</always_on>" << " <visualize>" << _visualize << "</visualize>" << " <camera>" + << " <camera_info_topic>" << _cameraInfoTopic + << "</camera_info_topic>" << " <horizontal_fov>.75</horizontal_fov>" << " <image>" << " <width>640</width>" @@ -160,7 +161,7 @@ TEST(Camera_TEST, CreateCamera) // Check topics EXPECT_EQ("/cam", cam->Topic()); - EXPECT_EQ("/camera_info", cam->InfoTopic()); + EXPECT_EQ("my_camera/camera_info", cam->InfoTopic()); // however camera is not loaded because a rendering scene is missing so // updates will not be successful and image size will be 0