Add SensorTopic component to rendering sensors (#1908)

The SensorTopic component is already available for non-rendering sensors (e.g. imu, force-torque, etc). This PR adds the component to rendering sensors, i.e. all sensors managed by the sensors system. The SensorTopic components stores the topic published by the sensor.

Note that rendering sensors may publish more than one topics, e.g. <prefix>/image and <prefix>/camera_info. In this case it'll only store the <prefix>/image topic.

Signed-off-by: Ian Chen <ichen@openrobotics.org>

src/rendering/RenderUtil.cc

@@ -79,6 +79,7 @@
 #include "ignition/gazebo/components/Scene.hh"
 #include "ignition/gazebo/components/SegmentationCamera.hh"
 #include "ignition/gazebo/components/SemanticLabel.hh"
+#include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/SourceFilePath.hh"
 #include "ignition/gazebo/components/Temperature.hh"
 #include "ignition/gazebo/components/TemperatureRange.hh"
@@ -288,6 +289,9 @@ class ignition::gazebo::RenderUtilPrivate
   public: std::unordered_map<Entity, msgs::ParticleEmitter>
       newParticleEmittersCmds;
 
+  /// \brief New sensor topics that should be added to ECM as new components
+  public: std::unordered_map<Entity, std::string> newSensorTopics;
+
   /// \brief A list of entities with particle emitter cmds to remove
   public: std::vector<Entity> particleCmdsToRemove;
 
@@ -759,6 +763,16 @@ void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/,
       _ecm.RemoveComponent<components::VisualCmd>(entity);
     }
   }
+
+  // create sensor topic components
+  {
+    for (const auto &it : this->dataPtr->newSensorTopics)
+    {
+      // Set topic
+      _ecm.CreateComponent(it.first, components::SensorTopic(it.second));
+    }
+    this->dataPtr->newSensorTopics.clear();
+  }
 }
 
 //////////////////////////////////////////////////
@@ -1629,6 +1643,7 @@ void RenderUtilPrivate::AddNewSensor(const EntityComponentManager &_ecm,
   {
     sdfDataCopy.SetTopic(scopedName(_entity, _ecm) + _topicSuffix);
   }
+  this->newSensorTopics[_entity] = sdfDataCopy.Topic();
   this->newSensors.push_back(
       std::make_tuple(_entity, std::move(sdfDataCopy), _parent));
   this->sensorEntities.insert(_entity);

test/integration/sensors_system.cc

@@ -94,6 +94,39 @@ void testSensorEntityIds(const gazebo::EntityComponentManager &_ecm,
   }
 }
 
+/////////////////////////////////////////////////
+void testSensorTopicComponents(const gazebo::EntityComponentManager &_ecm,
+    const std::unordered_set<gazebo::Entity> &_ids,
+    const std::vector<std::string> &_topics)
+{
+  EXPECT_FALSE(_ids.empty());
+  for (const auto & id : _ids)
+  {
+    auto sensorTopicComp = _ecm.Component<gazebo::components::SensorTopic>(id);
+    EXPECT_TRUE(sensorTopicComp);
+    std::string topicStr = "/" + sensorTopicComp->Data();
+    EXPECT_FALSE(topicStr.empty());
+
+    // verify that the topic string stored in sensor topic component
+    // exits in the list of topics
+    // For rendering sensors, they may advertize more than one topics but
+    // the the sensor topic component will only contain one of them, e.g.
+    //   * <topic_prefix>/image - stored in sensor topic component
+    //   * <topic_prefix>/camera_info
+    bool foundTopic = false;
+    for (auto it = _topics.begin(); it != _topics.end(); ++it)
+    {
+      std::string topic = *it;
+      if (topic.find(topicStr) == 0u)
+      {
+        foundTopic = true;
+        break;
+      }
+    }
+    EXPECT_TRUE(foundTopic);
+  }
+}
+
 //////////////////////////////////////////////////
 class SensorsFixture : public InternalFixture<InternalFixture<::testing::Test>>
 {
@@ -118,36 +151,25 @@ class SensorsFixture : public InternalFixture<InternalFixture<::testing::Test>>
 };
 
 //////////////////////////////////////////////////
-void testDefaultTopics()
+void testDefaultTopics(const std::vector<std::string> &_topics)
 {
   // TODO(anyone) This should be a new test, but running multiple tests with
   // sensors is not currently working
-  std::string prefix{"/world/camera_sensor/model/default_topics/"};
-  std::vector<std::string> topics{
-      prefix + "link/camera_link/sensor/camera/image",
-      prefix + "link/camera_link/sensor/camera/camera_info",
-      prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan",
-      prefix + "link/depth_camera_link/sensor/depth_camera/depth_image",
-      prefix + "link/depth_camera_link/sensor/depth_camera/camera_info",
-      prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image",
-      prefix + "link/rgbd_camera_link/sensor/rgbd_camera/depth_image"
-  };
-
   std::vector<transport::MessagePublisher> publishers;
   transport::Node node;
 
   // Sensors are created in a separate thread, so we sleep here to give them
   // time
   int sleep{0};
   int maxSleep{30};
-  for (; sleep < maxSleep && !node.TopicInfo(topics.front(), publishers);
+  for (; sleep < maxSleep && !node.TopicInfo(_topics.front(), publishers);
       ++sleep)
   {
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
   ASSERT_LT(sleep, maxSleep);
 
-  for (const std::string &topic : topics)
+  for (const std::string &topic : _topics)
   {
     bool result = node.TopicInfo(topic, publishers);
 
@@ -199,9 +221,30 @@ TEST_F(SensorsFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(HandleRemovedEntities))
   server.Run(true, 50, false);
   ASSERT_NE(nullptr, ecm);
 
-  testDefaultTopics();
+  std::string prefix{"/world/camera_sensor/model/default_topics/"};
+  std::vector<std::string> topics{
+      prefix + "link/camera_link/sensor/camera/image",
+      prefix + "link/camera_link/sensor/camera/camera_info",
+      prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan",
+      prefix + "link/depth_camera_link/sensor/depth_camera/depth_image",
+      prefix + "link/depth_camera_link/sensor/depth_camera/camera_info",
+      prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image",
+      prefix + "link/rgbd_camera_link/sensor/rgbd_camera/depth_image",
+      prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan",
+      prefix + "link/thermal_camera_link/sensor/thermal_camera/image",
+      prefix + "link/segmentation_camera_link/sensor/segmentation_camera/"
+             + "segmentation/colored_map",
+      prefix + "link/segmentation_camera_link/sensor/segmentation_camera/"
+             + "segmentation/labels_map",
+      prefix + "link/segmentation_camera_link/sensor/segmentation_camera/"
+             + "segmentation/camera_info",
+      "/camera"
+  };
 
+  testDefaultTopics(topics);
   testSensorEntityIds(*ecm, g_sensorEntityIds);
+  testSensorTopicComponents(*ecm, g_sensorEntityIds, topics);
+
   g_sensorEntityIds.clear();
   g_scene.reset();