add rendering sensor topic component

Signed-off-by: Ian Chen <[email protected]>

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