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] 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);